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

Code-style consistency improvement:

Browse Source 
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files.
make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type.
This commit contains no other changes aside from adding and applying clang-format-all.sh
This commit is contained in:
erwincoumans
2018-09-23 14:17:31 -07:00
parent b73b05e9fb
commit ab8f16961e
1773 changed files with 1081087 additions and 474249 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
examples/SharedMemory/BodyJointInfoUtility.h
View File

@@ -5,9 +5,9 @@
namespace Bullet
{
class btMultiBodyDoubleData;
class btMultiBodyFloatData;
};
class btMultiBodyDoubleData;
class btMultiBodyFloatData;
}; // namespace Bullet
inline char* strDup(const char* const str)
{
@@ -18,11 +18,12 @@ inline char* strDup(const char* const str)
#endif
}
template <typename T, typename U> void addJointInfoFromMultiBodyData(const T* mb, U* bodyJoints, bool verboseOutput)
template <typename T, typename U>
void addJointInfoFromMultiBodyData(const T* mb, U* bodyJoints, bool verboseOutput)
{
if (mb->m_baseName)
if (mb->m_baseName)
{
if (verboseOutput)
if (verboseOutput)
{
b3Printf("mb->m_baseName = %s\n", mb->m_baseName);
}
@@ -30,7 +31,7 @@ template <typename T, typename U> void addJointInfoFromMultiBodyData(const T* mb
int qOffset = 7;
int uOffset = 6;
for (int link = 0; link < mb->m_numLinks; link++)
for (int link = 0; link < mb->m_numLinks; link++)
{
{
b3JointInfo info;
@@ -43,20 +44,23 @@ template <typename T, typename U> void addJointInfoFromMultiBodyData(const T* mb
info.m_uIndex =
(0 < mb->m_links[link].m_dofCount) ? uOffset : -1;
if (mb->m_links[link].m_linkName) {
if (verboseOutput) {
if (mb->m_links[link].m_linkName)
{
if (verboseOutput)
{
b3Printf("mb->m_links[%d].m_linkName = %s\n", link,
mb->m_links[link].m_linkName);
mb->m_links[link].m_linkName);
}
strcpy(info.m_linkName,mb->m_links[link].m_linkName);
strcpy(info.m_linkName, mb->m_links[link].m_linkName);
}
if (mb->m_links[link].m_jointName) {
if (verboseOutput) {
if (mb->m_links[link].m_jointName)
{
if (verboseOutput)
{
b3Printf("mb->m_links[%d].m_jointName = %s\n", link,
mb->m_links[link].m_jointName);
mb->m_links[link].m_jointName);
}
strcpy(info.m_jointName,mb->m_links[link].m_jointName);
strcpy(info.m_jointName, mb->m_links[link].m_jointName);
//info.m_jointName = strDup(mb->m_links[link].m_jointName);
}
@@ -67,7 +71,7 @@ template <typename T, typename U> void addJointInfoFromMultiBodyData(const T* mb
info.m_jointUpperLimit = mb->m_links[link].m_jointUpperLimit;
info.m_jointMaxForce = mb->m_links[link].m_jointMaxForce;
info.m_jointMaxVelocity = mb->m_links[link].m_jointMaxVelocity;
info.m_parentFrame[0] = mb->m_links[link].m_parentComToThisPivotOffset.m_floats[0];
info.m_parentFrame[1] = mb->m_links[link].m_parentComToThisPivotOffset.m_floats[1];
info.m_parentFrame[2] = mb->m_links[link].m_parentComToThisPivotOffset.m_floats[2];
@@ -95,7 +99,8 @@ template <typename T, typename U> void addJointInfoFromMultiBodyData(const T* mb
}
if ((mb->m_links[link].m_jointType == eRevoluteType) ||
(mb->m_links[link].m_jointType == ePrismaticType)) {
(mb->m_links[link].m_jointType == ePrismaticType))
{
info.m_flags |= JOINT_HAS_MOTORIZED_POWER;
}
bodyJoints->m_jointInfo.push_back(info);
@@ -103,9 +108,6 @@ template <typename T, typename U> void addJointInfoFromMultiBodyData(const T* mb
qOffset += mb->m_links[link].m_posVarCount;
uOffset += mb->m_links[link].m_dofCount;
}
}
#endif //BODY_JOINT_INFO_UTILITY_H
#endif //BODY_JOINT_INFO_UTILITY_H

332
examples/SharedMemory/IKTrajectoryHelper.cpp
View File

@@ -7,217 +7,221 @@
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "BulletDynamics/Featherstone/btMultiBody.h"
#define RADIAN(X) ((X)*RadiansToDegrees)
#define RADIAN(X) ((X)*RadiansToDegrees)
//use BussIK and Reflexxes to convert from Cartesian endeffector future target to
//joint space positions at each real-time (simulation) step
struct IKTrajectoryHelperInternalData
{
VectorR3 m_endEffectorTargetPosition;
VectorRn m_nullSpaceVelocity;
VectorRn m_dampingCoeff;
b3AlignedObjectArray<Node*> m_ikNodes;
IKTrajectoryHelperInternalData()
{
m_endEffectorTargetPosition.SetZero();
m_nullSpaceVelocity.SetZero();
m_dampingCoeff.SetZero();
}
};
VectorR3 m_endEffectorTargetPosition;
VectorRn m_nullSpaceVelocity;
VectorRn m_dampingCoeff;
b3AlignedObjectArray<Node*> m_ikNodes;
IKTrajectoryHelperInternalData()
{
m_endEffectorTargetPosition.SetZero();
m_nullSpaceVelocity.SetZero();
m_dampingCoeff.SetZero();
}
};
IKTrajectoryHelper::IKTrajectoryHelper()
{
m_data = new IKTrajectoryHelperInternalData;
m_data = new IKTrajectoryHelperInternalData;
}
IKTrajectoryHelper::~IKTrajectoryHelper()
{
delete m_data;
delete m_data;
}
bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
const double endEffectorTargetOrientation[4],
const double endEffectorWorldPosition[3],
const double endEffectorWorldOrientation[4],
const double* q_current, int numQ,int endEffectorIndex,
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6])
const double endEffectorTargetOrientation[4],
const double endEffectorWorldPosition[3],
const double endEffectorWorldOrientation[4],
const double* q_current, int numQ, int endEffectorIndex,
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6])
{
bool useAngularPart = (ikMethod==IK2_VEL_DLS_WITH_ORIENTATION || ikMethod==IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE
|| ikMethod==IK2_VEL_SDLS_WITH_ORIENTATION) ? true : false;
bool useAngularPart = (ikMethod == IK2_VEL_DLS_WITH_ORIENTATION || ikMethod == IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE || ikMethod == IK2_VEL_SDLS_WITH_ORIENTATION) ? true : false;
Jacobian ikJacobian(useAngularPart,numQ);
Jacobian ikJacobian(useAngularPart, numQ);
ikJacobian.Reset();
ikJacobian.Reset();
bool UseJacobianTargets1 = false;
if ( UseJacobianTargets1 ) {
ikJacobian.SetJtargetActive();
}
else {
ikJacobian.SetJendActive();
}
VectorR3 targets;
targets.Set(endEffectorTargetPosition[0],endEffectorTargetPosition[1],endEffectorTargetPosition[2]);
ikJacobian.ComputeJacobian(&targets); // Set up Jacobian and deltaS vectors
// Set one end effector world position from Bullet
VectorRn deltaS(3);
for (int i = 0; i < 3; ++i)
{
deltaS.Set(i,dampIk[i]*(endEffectorTargetPosition[i]-endEffectorWorldPosition[i]));
}
// Set one end effector world orientation from Bullet
VectorRn deltaR(3);
if (useAngularPart)
{
btQuaternion startQ(endEffectorWorldOrientation[0],endEffectorWorldOrientation[1],endEffectorWorldOrientation[2],endEffectorWorldOrientation[3]);
btQuaternion endQ(endEffectorTargetOrientation[0],endEffectorTargetOrientation[1],endEffectorTargetOrientation[2],endEffectorTargetOrientation[3]);
btQuaternion deltaQ = endQ*startQ.inverse();
float angle = deltaQ.getAngle();
btVector3 axis = deltaQ.getAxis();
if (angle > PI) {
angle -= 2.0*PI;
}
else if (angle < -PI) {
angle += 2.0*PI;
}
float angleDot = angle;
btVector3 angularVel = angleDot*axis.normalize();
for (int i = 0; i < 3; ++i)
{
deltaR.Set(i,dampIk[i+3]*angularVel[i]);
}
}
{
bool UseJacobianTargets1 = false;
if (UseJacobianTargets1)
{
ikJacobian.SetJtargetActive();
}
else
{
ikJacobian.SetJendActive();
}
VectorR3 targets;
targets.Set(endEffectorTargetPosition[0], endEffectorTargetPosition[1], endEffectorTargetPosition[2]);
ikJacobian.ComputeJacobian(&targets); // Set up Jacobian and deltaS vectors
// Set one end effector world position from Bullet
VectorRn deltaS(3);
for (int i = 0; i < 3; ++i)
{
deltaS.Set(i, dampIk[i] * (endEffectorTargetPosition[i] - endEffectorWorldPosition[i]));
}
// Set one end effector world orientation from Bullet
VectorRn deltaR(3);
if (useAngularPart)
{
btQuaternion startQ(endEffectorWorldOrientation[0], endEffectorWorldOrientation[1], endEffectorWorldOrientation[2], endEffectorWorldOrientation[3]);
btQuaternion endQ(endEffectorTargetOrientation[0], endEffectorTargetOrientation[1], endEffectorTargetOrientation[2], endEffectorTargetOrientation[3]);
btQuaternion deltaQ = endQ * startQ.inverse();
float angle = deltaQ.getAngle();
btVector3 axis = deltaQ.getAxis();
if (angle > PI)
{
angle -= 2.0 * PI;
}
else if (angle < -PI)
{
angle += 2.0 * PI;
}
float angleDot = angle;
btVector3 angularVel = angleDot * axis.normalize();
for (int i = 0; i < 3; ++i)
{
deltaR.Set(i, dampIk[i + 3] * angularVel[i]);
}
}
{
if (useAngularPart)
{
VectorRn deltaC(6);
MatrixRmn completeJacobian(6,numQ);
MatrixRmn completeJacobian(6, numQ);
for (int i = 0; i < 3; ++i)
{
deltaC.Set(i,deltaS[i]);
deltaC.Set(i+3,deltaR[i]);
deltaC.Set(i, deltaS[i]);
deltaC.Set(i + 3, deltaR[i]);
for (int j = 0; j < numQ; ++j)
{
completeJacobian.Set(i,j,linear_jacobian[i*numQ+j]);
completeJacobian.Set(i+3,j,angular_jacobian[i*numQ+j]);
}
}
ikJacobian.SetDeltaS(deltaC);
ikJacobian.SetJendTrans(completeJacobian);
} else
{
VectorRn deltaC(3);
MatrixRmn completeJacobian(3,numQ);
for (int i = 0; i < 3; ++i)
{
deltaC.Set(i,deltaS[i]);
for (int j = 0; j < numQ; ++j)
{
completeJacobian.Set(i,j,linear_jacobian[i*numQ+j]);
completeJacobian.Set(i, j, linear_jacobian[i * numQ + j]);
completeJacobian.Set(i + 3, j, angular_jacobian[i * numQ + j]);
}
}
ikJacobian.SetDeltaS(deltaC);
ikJacobian.SetJendTrans(completeJacobian);
}
}
// Calculate the change in theta values
switch (ikMethod) {
case IK2_JACOB_TRANS:
ikJacobian.CalcDeltaThetasTranspose(); // Jacobian transpose method
break;
else
{
VectorRn deltaC(3);
MatrixRmn completeJacobian(3, numQ);
for (int i = 0; i < 3; ++i)
{
deltaC.Set(i, deltaS[i]);
for (int j = 0; j < numQ; ++j)
{
completeJacobian.Set(i, j, linear_jacobian[i * numQ + j]);
}
}
ikJacobian.SetDeltaS(deltaC);
ikJacobian.SetJendTrans(completeJacobian);
}
}
// Calculate the change in theta values
switch (ikMethod)
{
case IK2_JACOB_TRANS:
ikJacobian.CalcDeltaThetasTranspose(); // Jacobian transpose method
break;
case IK2_DLS:
case IK2_VEL_DLS_WITH_ORIENTATION:
case IK2_VEL_DLS:
//ikJacobian.CalcDeltaThetasDLS(); // Damped least squares method
assert(m_data->m_dampingCoeff.GetLength()==numQ);
ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff);
break;
case IK2_VEL_DLS_WITH_NULLSPACE:
case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE:
assert(m_data->m_nullSpaceVelocity.GetLength()==numQ);
ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity);
break;
case IK2_DLS_SVD:
ikJacobian.CalcDeltaThetasDLSwithSVD();
break;
case IK2_PURE_PSEUDO:
ikJacobian.CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method
break;
case IK2_SDLS:
//ikJacobian.CalcDeltaThetasDLS(); // Damped least squares method
assert(m_data->m_dampingCoeff.GetLength() == numQ);
ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff);
break;
case IK2_VEL_DLS_WITH_NULLSPACE:
case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE:
assert(m_data->m_nullSpaceVelocity.GetLength() == numQ);
ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity);
break;
case IK2_DLS_SVD:
ikJacobian.CalcDeltaThetasDLSwithSVD();
break;
case IK2_PURE_PSEUDO:
ikJacobian.CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method
break;
case IK2_SDLS:
case IK2_VEL_SDLS:
case IK2_VEL_SDLS_WITH_ORIENTATION:
ikJacobian.CalcDeltaThetasSDLS(); // Selectively damped least squares method
break;
default:
ikJacobian.ZeroDeltaThetas();
break;
}
// Use for velocity IK, update theta dot
//ikJacobian.UpdateThetaDot();
// Use for position IK, incrementally update theta
//ikJacobian.UpdateThetas();
// Apply the change in the theta values
//ikJacobian.UpdatedSClampValue(&targets);
for (int i=0;i<numQ;i++)
{
// Use for velocity IK
q_new[i] = ikJacobian.dTheta[i] + q_current[i];
// Use for position IK
//q_new[i] = m_data->m_ikNodes[i]->GetTheta();
}
return true;
ikJacobian.CalcDeltaThetasSDLS(); // Selectively damped least squares method
break;
default:
ikJacobian.ZeroDeltaThetas();
break;
}
// Use for velocity IK, update theta dot
//ikJacobian.UpdateThetaDot();
// Use for position IK, incrementally update theta
//ikJacobian.UpdateThetas();
// Apply the change in the theta values
//ikJacobian.UpdatedSClampValue(&targets);
for (int i = 0; i < numQ; i++)
{
// Use for velocity IK
q_new[i] = ikJacobian.dTheta[i] + q_current[i];
// Use for position IK
//q_new[i] = m_data->m_ikNodes[i]->GetTheta();
}
return true;
}
bool IKTrajectoryHelper::computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range, const double* rest_pose)
{
m_data->m_nullSpaceVelocity.SetLength(numQ);
m_data->m_nullSpaceVelocity.SetZero();
m_data->m_nullSpaceVelocity.SetLength(numQ);
m_data->m_nullSpaceVelocity.SetZero();
// TODO: Expose the coefficents of the null space term so that the user can choose to balance the null space task and the IK target task.
// Can also adaptively adjust the coefficients based on the residual of the null space velocity in the IK target task space.
double stayCloseToZeroGain = 0.001;
double stayAwayFromLimitsGain = 10.0;
double stayCloseToZeroGain = 0.001;
double stayAwayFromLimitsGain = 10.0;
// Stay close to zero
for (int i = 0; i < numQ; ++i)
{
m_data->m_nullSpaceVelocity[i] = stayCloseToZeroGain * (rest_pose[i]-q_current[i]);
}
// Stay close to zero
for (int i = 0; i < numQ; ++i)
{
m_data->m_nullSpaceVelocity[i] = stayCloseToZeroGain * (rest_pose[i] - q_current[i]);
}
// Stay away from joint limits
for (int i = 0; i < numQ; ++i) {
if (q_current[i] > upper_limit[i]) {
m_data->m_nullSpaceVelocity[i] += stayAwayFromLimitsGain * (upper_limit[i] - q_current[i]) / joint_range[i];
}
if (q_current[i] < lower_limit[i]) {
m_data->m_nullSpaceVelocity[i] += stayAwayFromLimitsGain * (lower_limit[i] - q_current[i]) / joint_range[i];
}
}
return true;
// Stay away from joint limits
for (int i = 0; i < numQ; ++i)
{
if (q_current[i] > upper_limit[i])
{
m_data->m_nullSpaceVelocity[i] += stayAwayFromLimitsGain * (upper_limit[i] - q_current[i]) / joint_range[i];
}
if (q_current[i] < lower_limit[i])
{
m_data->m_nullSpaceVelocity[i] += stayAwayFromLimitsGain * (lower_limit[i] - q_current[i]) / joint_range[i];
}
}
return true;
}
bool IKTrajectoryHelper::setDampingCoeff(int numQ, const double* coeff)
{
m_data->m_dampingCoeff.SetLength(numQ);
m_data->m_dampingCoeff.SetZero();
for (int i = 0; i < numQ; ++i)
{
m_data->m_dampingCoeff[i] = coeff[i];
}
return true;
m_data->m_dampingCoeff.SetLength(numQ);
m_data->m_dampingCoeff.SetZero();
for (int i = 0; i < numQ; ++i)
{
m_data->m_dampingCoeff[i] = coeff[i];
}
return true;
}

46
examples/SharedMemory/IKTrajectoryHelper.h
View File

@@ -3,37 +3,35 @@
enum IK2_Method
{
IK2_JACOB_TRANS=0,
IK2_PURE_PSEUDO,
IK2_DLS,
IK2_SDLS ,
IK2_DLS_SVD,
IK2_VEL_DLS,
IK2_JACOB_TRANS = 0,
IK2_PURE_PSEUDO,
IK2_DLS,
IK2_SDLS,
IK2_DLS_SVD,
IK2_VEL_DLS,
IK2_VEL_DLS_WITH_ORIENTATION,
IK2_VEL_DLS_WITH_NULLSPACE,
IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE,
IK2_VEL_DLS_WITH_NULLSPACE,
IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE,
IK2_VEL_SDLS,
IK2_VEL_SDLS_WITH_ORIENTATION,
};
class IKTrajectoryHelper
{
struct IKTrajectoryHelperInternalData* m_data;
struct IKTrajectoryHelperInternalData* m_data;
public:
IKTrajectoryHelper();
virtual ~IKTrajectoryHelper();
IKTrajectoryHelper();
virtual ~IKTrajectoryHelper();
bool computeIK(const double endEffectorTargetPosition[3],
const double endEffectorTargetOrientation[4],
const double endEffectorWorldPosition[3],
const double endEffectorWorldOrientation[4],
const double* q_old, int numQ, int endEffectorIndex,
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6]);
bool computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range, const double* rest_pose);
bool setDampingCoeff(int numQ, const double* coeff);
const double endEffectorTargetOrientation[4],
const double endEffectorWorldPosition[3],
const double endEffectorWorldOrientation[4],
const double* q_old, int numQ, int endEffectorIndex,
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6]);
bool computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range, const double* rest_pose);
bool setDampingCoeff(int numQ, const double* coeff);
};
#endif //IK_TRAJECTORY_HELPER_H
#endif //IK_TRAJECTORY_HELPER_H

11
examples/SharedMemory/InProcessMemory.cpp
View File

@@ -1,15 +1,12 @@
#include "InProcessMemory.h"
#include "LinearMath/btHashMap.h"
struct InProcessMemoryInternalData
{
btHashMap<btHashInt, void*> m_memoryPointers;
};
InProcessMemory::InProcessMemory()
{
m_data = new InProcessMemoryInternalData;
@@ -17,7 +14,7 @@ InProcessMemory::InProcessMemory()
InProcessMemory::~InProcessMemory()
{
for (int i=0;i<m_data->m_memoryPointers.size();i++)
for (int i = 0; i < m_data->m_memoryPointers.size(); i++)
{
void** ptrptr = m_data->m_memoryPointers.getAtIndex(i);
if (ptrptr)
@@ -29,7 +26,7 @@ InProcessMemory::~InProcessMemory()
delete m_data;
}
void* InProcessMemory::allocateSharedMemory(int key, int size, bool allowCreation)
void* InProcessMemory::allocateSharedMemory(int key, int size, bool allowCreation)
{
void** ptrptr = m_data->m_memoryPointers[key];
if (ptrptr)
@@ -38,11 +35,11 @@ void* InProcessMemory::allocateSharedMemory(int key, int size, bool allowCreatio
}
void* ptr = malloc(size);
m_data->m_memoryPointers.insert(key,ptr);
m_data->m_memoryPointers.insert(key, ptr);
return ptr;
}
void InProcessMemory::releaseSharedMemory(int /*key*/, int /*size*/)
void InProcessMemory::releaseSharedMemory(int /*key*/, int /*size*/)
{
//we don't release the memory here, but in the destructor instead,
//so multiple users could 'share' the memory given some key

5
examples/SharedMemory/InProcessMemory.h
View File

@@ -8,12 +8,11 @@ class InProcessMemory : public SharedMemoryInterface
struct InProcessMemoryInternalData* m_data;
public:
InProcessMemory();
virtual ~InProcessMemory();
virtual void* allocateSharedMemory(int key, int size, bool allowCreation);
virtual void releaseSharedMemory(int key, int size);
virtual void* allocateSharedMemory(int key, int size, bool allowCreation);
virtual void releaseSharedMemory(int key, int size);
};
#endif

1
examples/SharedMemory/PhysicsClient.cpp
View File

@@ -3,4 +3,3 @@
PhysicsClient::~PhysicsClient()
{
}

78
examples/SharedMemory/PhysicsClient.h
View File

@@ -4,58 +4,59 @@
//#include "SharedMemoryCommands.h"
#include "LinearMath/btVector3.h"
class PhysicsClient {
class PhysicsClient
{
public:
virtual ~PhysicsClient();
virtual ~PhysicsClient();
// return true if connection succesfull, can also check 'isConnected'
virtual bool connect() = 0;
// return true if connection succesfull, can also check 'isConnected'
virtual bool connect() = 0;
virtual void disconnectSharedMemory() = 0;
virtual void disconnectSharedMemory() = 0;
virtual bool isConnected() const = 0;
virtual bool isConnected() const = 0;
// return non-null if there is a status, nullptr otherwise
virtual const struct SharedMemoryStatus* processServerStatus() = 0;
// return non-null if there is a status, nullptr otherwise
virtual const struct SharedMemoryStatus* processServerStatus() = 0;
virtual struct SharedMemoryCommand* getAvailableSharedMemoryCommand() = 0;
virtual struct SharedMemoryCommand* getAvailableSharedMemoryCommand() = 0;
virtual bool canSubmitCommand() const = 0;
virtual bool canSubmitCommand() const = 0;
virtual bool submitClientCommand(const struct SharedMemoryCommand& command) = 0;
virtual bool submitClientCommand(const struct SharedMemoryCommand& command) = 0;
virtual int getNumBodies() const = 0;
virtual int getBodyUniqueId(int serialIndex) const = 0;
virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const = 0;
virtual int getNumJoints(int bodyUniqueId) const = 0;
virtual bool getJointInfo(int bodyUniqueId, int jointIndex, struct b3JointInfo& info) const = 0;
virtual int getNumJoints(int bodyUniqueId) const = 0;
virtual bool getJointInfo(int bodyUniqueId, int jointIndex, struct b3JointInfo& info) const = 0;
virtual int getNumUserConstraints() const = 0;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const = 0;
virtual int getNumUserConstraints() const = 0;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const = 0;
virtual int getUserConstraintId(int serialIndex) const = 0;
virtual void setSharedMemoryKey(int key) = 0;
virtual void uploadBulletFileToSharedMemory(const char* data, int len) = 0;
virtual void setSharedMemoryKey(int key) = 0;
virtual void uploadBulletFileToSharedMemory(const char* data, int len) = 0;
virtual void uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays) = 0;
virtual int getNumDebugLines() const = 0;
virtual const float* getDebugLinesFrom() const = 0;
virtual const float* getDebugLinesTo() const = 0;
virtual const float* getDebugLinesColor() const = 0;
virtual int getNumDebugLines() const = 0;
virtual const float* getDebugLinesFrom() const = 0;
virtual const float* getDebugLinesTo() const = 0;
virtual const float* getDebugLinesColor() const = 0;
virtual void getCachedCameraImage(struct b3CameraImageData* cameraData) = 0;
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData) = 0;
virtual void getCachedCameraImage(struct b3CameraImageData* cameraData)=0;
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData)=0;
virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects) = 0;
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo) = 0;
@@ -73,18 +74,15 @@ public:
virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix) = 0;
virtual void setTimeOut(double timeOutInSeconds) = 0;
virtual double getTimeOut() const = 0;
virtual double getTimeOut() const = 0;
virtual bool getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const = 0;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const = 0;
virtual bool getCachedUserData(int userDataId, struct b3UserDataValue& valueOut) const = 0;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char* key) const = 0;
virtual int getNumUserData(int bodyUniqueId) const = 0;
virtual void getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const = 0;
virtual void getUserDataInfo(int bodyUniqueId, int userDataIndex, const char** keyOut, int* userDataIdOut, int* linkIndexOut, int* visualShapeIndexOut) const = 0;
virtual void pushProfileTiming(const char* timingName)=0;
virtual void popProfileTiming()=0;
virtual void pushProfileTiming(const char* timingName) = 0;
virtual void popProfileTiming() = 0;
};
#endif // BT_PHYSICS_CLIENT_API_H

3520
examples/SharedMemory/PhysicsClientC_API.cpp
View File

File diff suppressed because it is too large Load Diff

1229
examples/SharedMemory/PhysicsClientC_API.h
View File

File diff suppressed because it is too large Load Diff

1295
examples/SharedMemory/PhysicsClientExample.cpp
View File

File diff suppressed because it is too large Load Diff

10
examples/SharedMemory/PhysicsClientExample.h
View File

@@ -3,11 +3,11 @@
enum ClientExampleOptions
{
eCLIENTEXAMPLE_LOOPBACK=1,
eCLIENTEXAMPLE_DIRECT=2,
eCLIENTEXAMPLE_SERVER=3,
eCLIENTEXAMPLE_LOOPBACK = 1,
eCLIENTEXAMPLE_DIRECT = 2,
eCLIENTEXAMPLE_SERVER = 3,
};
class CommonExampleInterface* PhysicsClientCreateFunc(struct CommonExampleOptions& options);
class CommonExampleInterface* PhysicsClientCreateFunc(struct CommonExampleOptions& options);
#endif//PHYSICS_CLIENT_EXAMPLE_H
#endif //PHYSICS_CLIENT_EXAMPLE_H

48
examples/SharedMemory/PhysicsClientGRPC.cpp
View File

@@ -24,14 +24,12 @@ static unsigned int b3DeserializeInt2(const unsigned char* input)
bool gVerboseNetworkMessagesClient3 = false;
struct GRPCNetworkedInternalData
struct GRPCNetworkedInternalData
{
std::shared_ptr<grpc::Channel> m_grpcChannel;
std::unique_ptr< PyBulletAPI::Stub> m_stub;
std::unique_ptr<PyBulletAPI::Stub> m_stub;
bool m_isConnected;
bool m_isConnected;
SharedMemoryCommand m_clientCmd;
bool m_hasCommand;
@@ -46,12 +44,10 @@ struct GRPCNetworkedInternalData
double m_timeOutInSeconds;
GRPCNetworkedInternalData()
:
m_isConnected(false),
m_hasCommand(false),
m_timeOutInSeconds(60)
: m_isConnected(false),
m_hasCommand(false),
m_timeOutInSeconds(60)
{
}
void disconnect()
@@ -72,16 +68,14 @@ struct GRPCNetworkedInternalData
{
hostport += ':' + std::to_string(m_port);
}
m_grpcChannel = grpc::CreateChannel(
m_grpcChannel = grpc::CreateChannel(
hostport, grpc::InsecureChannelCredentials());
m_stub = PyBulletAPI::NewStub(m_grpcChannel);
// Set timeout for API
std::chrono::system_clock::time_point deadline =
std::chrono::system_clock::now()+std::chrono::seconds((long long)m_timeOutInSeconds);
std::chrono::system_clock::now() + std::chrono::seconds((long long)m_timeOutInSeconds);
grpc::ClientContext context;
context.set_deadline(deadline);
::pybullet_grpc::PyBulletCommand request;
@@ -106,21 +100,16 @@ struct GRPCNetworkedInternalData
printf("Error: cannot connect to GRPC server\n");
}
return m_isConnected;
}
bool checkData()
{
bool hasStatus = false;
return hasStatus;
}
};
GRPCNetworkedPhysicsProcessor::GRPCNetworkedPhysicsProcessor(const char* hostName, int port)
{
m_data = new GRPCNetworkedInternalData;
@@ -129,7 +118,6 @@ GRPCNetworkedPhysicsProcessor::GRPCNetworkedPhysicsProcessor(const char* hostNam
m_data->m_hostName = hostName;
}
m_data->m_port = port;
}
GRPCNetworkedPhysicsProcessor::~GRPCNetworkedPhysicsProcessor()
@@ -140,7 +128,7 @@ GRPCNetworkedPhysicsProcessor::~GRPCNetworkedPhysicsProcessor()
bool GRPCNetworkedPhysicsProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
if(gVerboseNetworkMessagesClient3)
if (gVerboseNetworkMessagesClient3)
{
printf("GRPCNetworkedPhysicsProcessor::processCommand\n");
}
@@ -157,18 +145,16 @@ bool GRPCNetworkedPhysicsProcessor::processCommand(const struct SharedMemoryComm
::pybullet_grpc::PyBulletStatus status;
// The actual RPC.
grpc::Status grpcStatus = m_data->m_stub->SubmitCommand(&context, grpcCommand, &status);
//convert grpc status to Bullet status
bool convertedOk = convertGRPCToStatus(status, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
if (!convertedOk)
{
disconnect();
}
return convertedOk;
}
return false;
}
@@ -197,20 +183,16 @@ bool GRPCNetworkedPhysicsProcessor::receiveStatus(struct SharedMemoryStatus& ser
{
printf("Error: steam buffer overflow\n");
}
}
return hasStatus;
}
void GRPCNetworkedPhysicsProcessor::renderScene(int renderFlags)
{
}
void GRPCNetworkedPhysicsProcessor::physicsDebugDraw(int debugDrawFlags)
void GRPCNetworkedPhysicsProcessor::physicsDebugDraw(int debugDrawFlags)
{
}
@@ -223,7 +205,6 @@ bool GRPCNetworkedPhysicsProcessor::isConnected() const
return m_data->m_isConnected;
}
bool GRPCNetworkedPhysicsProcessor::connect()
{
bool isConnected = m_data->connectGRPC();
@@ -233,14 +214,11 @@ bool GRPCNetworkedPhysicsProcessor::connect()
void GRPCNetworkedPhysicsProcessor::disconnect()
{
m_data->disconnect();
}
void GRPCNetworkedPhysicsProcessor::setTimeOut(double timeOutInSeconds)
{
m_data->m_timeOutInSeconds = timeOutInSeconds;
}
#endif //BT_ENABLE_GRPC
#endif //BT_ENABLE_GRPC

10
examples/SharedMemory/PhysicsClientGRPC.h
View File

@@ -6,7 +6,6 @@
class GRPCNetworkedPhysicsProcessor : public PhysicsCommandProcessorInterface
{
struct GRPCNetworkedInternalData* m_data;
public:
@@ -19,23 +18,20 @@ public:
virtual void disconnect();
virtual bool isConnected() const;
virtual bool processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
virtual bool receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
virtual void renderScene(int renderFlags);
virtual void physicsDebugDraw(int debugDrawFlags);
virtual void physicsDebugDraw(int debugDrawFlags);
virtual void setGuiHelper(struct GUIHelperInterface* guiHelper);
virtual void setTimeOut(double timeOutInSeconds);
virtual void reportNotifications() {}
};
#endif //PHYSICS_CLIENT_GRPC_H
#endif //PHYSICS_CLIENT_GRPC_H

6
examples/SharedMemory/PhysicsClientGRPC_C_API.cpp
View File

@@ -5,9 +5,8 @@
#include "PhysicsDirect.h"
#include <stdio.h>
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsGRPC(const char* hostName, int port)
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsGRPC(const char* hostName, int port)
{
GRPCNetworkedPhysicsProcessor* tcp = new GRPCNetworkedPhysicsProcessor(hostName, port);
PhysicsDirect* direct = new PhysicsDirect(tcp, true);
@@ -21,9 +20,8 @@ B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsGRPC(const char* hostName, i
else
{
printf("b3ConnectPhysicsGRPC connection failed.\n");
}
return (b3PhysicsClientHandle)direct;
}
#endif //BT_ENABLE_GRPC
#endif //BT_ENABLE_GRPC

11
examples/SharedMemory/PhysicsClientGRPC_C_API.h
View File

@@ -4,16 +4,15 @@
#include "PhysicsClientC_API.h"
#ifdef __cplusplus
extern "C" {
extern "C"
{
#endif
///send physics commands using GRPC connection
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsGRPC(const char* hostName, int port);
///send physics commands using GRPC connection
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsGRPC(const char* hostName, int port);
#ifdef __cplusplus
}
#endif
#endif //PHYSICS_CLIENT_GRPC_C_API_H
#endif //PHYSICS_CLIENT_GRPC_C_API_H

1544
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

File diff suppressed because it is too large Load Diff

70
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

@@ -6,34 +6,36 @@
//#include "SharedMemoryCommands.h"
#include "LinearMath/btVector3.h"
class PhysicsClientSharedMemory : public PhysicsClient {
struct PhysicsClientSharedMemoryInternalData* m_data;
class PhysicsClientSharedMemory : public PhysicsClient
{
struct PhysicsClientSharedMemoryInternalData* m_data;
protected:
virtual void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem);
void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
void resetData();
void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
void resetData();
void removeCachedBody(int bodyUniqueId);
virtual void renderSceneInternal() {};
virtual void renderSceneInternal(){};
public:
PhysicsClientSharedMemory();
virtual ~PhysicsClientSharedMemory();
PhysicsClientSharedMemory();
virtual ~PhysicsClientSharedMemory();
// return true if connection succesfull, can also check 'isConnected'
virtual bool connect();
// return true if connection succesfull, can also check 'isConnected'
virtual bool connect();
virtual void disconnectSharedMemory();
virtual void disconnectSharedMemory();
virtual bool isConnected() const;
virtual bool isConnected() const;
// return non-null if there is a status, nullptr otherwise
virtual const struct SharedMemoryStatus* processServerStatus();
// return non-null if there is a status, nullptr otherwise
virtual const struct SharedMemoryStatus* processServerStatus();
virtual struct SharedMemoryCommand* getAvailableSharedMemoryCommand();
virtual struct SharedMemoryCommand* getAvailableSharedMemoryCommand();
virtual bool canSubmitCommand() const;
virtual bool canSubmitCommand() const;
virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
virtual int getNumBodies() const;
@@ -41,29 +43,29 @@ public:
virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const;
virtual int getNumJoints(int bodyUniqueId) const;
virtual int getNumJoints(int bodyUniqueId) const;
virtual bool getJointInfo(int bodyUniqueId, int jointIndex, struct b3JointInfo& info) const;
virtual bool getJointInfo(int bodyUniqueId, int jointIndex, struct b3JointInfo& info) const;
virtual int getNumUserConstraints() const;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const;
virtual int getNumUserConstraints() const;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const;
virtual int getUserConstraintId(int serialIndex) const;
virtual void setSharedMemoryKey(int key);
virtual void uploadBulletFileToSharedMemory(const char* data, int len);
virtual void setSharedMemoryKey(int key);
virtual 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 int getNumDebugLines() const;
virtual const float* getDebugLinesFrom() const;
virtual const float* getDebugLinesTo() const;
virtual const float* getDebugLinesColor() const;
virtual const float* getDebugLinesFrom() const;
virtual const float* getDebugLinesTo() const;
virtual const float* getDebugLinesColor() const;
virtual void getCachedCameraImage(struct b3CameraImageData* cameraData);
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects);
@@ -85,10 +87,10 @@ public:
virtual void setTimeOut(double timeOutInSeconds);
virtual double getTimeOut() const;
virtual bool getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const;
virtual bool getCachedUserData(int userDataId, struct b3UserDataValue& valueOut) const;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char* key) const;
virtual int getNumUserData(int bodyUniqueId) const;
virtual void getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const;
virtual void getUserDataInfo(int bodyUniqueId, int userDataIndex, const char** keyOut, int* userDataIdOut, int* linkIndexOut, int* visualShapeIndexOut) const;
virtual void pushProfileTiming(const char* timingName);
virtual void popProfileTiming();

4
examples/SharedMemory/PhysicsClientSharedMemory2.cpp
View File

@@ -7,9 +7,8 @@
#include "SharedMemoryCommandProcessor.h"
PhysicsClientSharedMemory2::PhysicsClientSharedMemory2(SharedMemoryCommandProcessor* proc)
:PhysicsDirect(proc,false)
: PhysicsDirect(proc, false)
{
m_proc = proc;
}
@@ -24,4 +23,3 @@ void PhysicsClientSharedMemory2::setSharedMemoryInterface(class SharedMemoryInte
m_proc->setSharedMemoryInterface(sharedMem);
}
}

3
examples/SharedMemory/PhysicsClientSharedMemory2.h
View File

@@ -12,7 +12,6 @@ public:
virtual ~PhysicsClientSharedMemory2();
void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem);
};
#endif //PHYSICS_CLIENT_SHARED_MEMORY2_H
#endif //PHYSICS_CLIENT_SHARED_MEMORY2_H

4
examples/SharedMemory/PhysicsClientSharedMemory2_C_API.cpp
View File

@@ -5,10 +5,9 @@
b3PhysicsClientHandle b3ConnectSharedMemory2(int key)
{
SharedMemoryCommandProcessor* cmdProc = new SharedMemoryCommandProcessor();
cmdProc->setSharedMemoryKey(key);
PhysicsDirect* cl = new PhysicsDirect(cmdProc,true);
PhysicsDirect* cl = new PhysicsDirect(cmdProc, true);
cl->setSharedMemoryKey(key);
@@ -16,4 +15,3 @@ b3PhysicsClientHandle b3ConnectSharedMemory2(int key)
return (b3PhysicsClientHandle)cl;
}

9
examples/SharedMemory/PhysicsClientSharedMemory2_C_API.h
View File

@@ -4,15 +4,14 @@
#include "PhysicsClientC_API.h"
#ifdef __cplusplus
extern "C" {
extern "C"
{
#endif
b3PhysicsClientHandle b3ConnectSharedMemory2(int key);
b3PhysicsClientHandle b3ConnectSharedMemory2(int key);
#ifdef __cplusplus
}
#endif
#endif //PHYSICS_CLIENT_SHARED_MEMORY2_H
#endif //PHYSICS_CLIENT_SHARED_MEMORY2_H

7
examples/SharedMemory/PhysicsClientSharedMemory_C_API.h
View File

@@ -4,13 +4,14 @@
#include "PhysicsClientC_API.h"
#ifdef __cplusplus
extern "C" {
extern "C"
{
#endif
B3_SHARED_API b3PhysicsClientHandle b3ConnectSharedMemory(int key);
B3_SHARED_API b3PhysicsClientHandle b3ConnectSharedMemory(int key);
#ifdef __cplusplus
}
#endif
#endif //PHYSICS_CLIENT_SHARED_MEMORY_H
#endif //PHYSICS_CLIENT_SHARED_MEMORY_H

92
examples/SharedMemory/PhysicsClientTCP.cpp
View File

@@ -1,6 +1,6 @@
#include "PhysicsClientTCP.h"
#include "ActiveSocket.h"
#include "ActiveSocket.h"
#include <stdio.h>
#include <string.h>
@@ -12,8 +12,6 @@
#include "Bullet3Common/b3Logging.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
unsigned int b3DeserializeInt2(const unsigned char* input)
{
unsigned int tmp = (input[3] << 24) + (input[2] << 16) + (input[1] << 8) + input[0];
@@ -21,21 +19,19 @@ unsigned int b3DeserializeInt2(const unsigned char* input)
}
bool gVerboseNetworkMessagesClient2 = false;
struct TcpNetworkedInternalData
struct TcpNetworkedInternalData
{
/*
/*
ENetHost* m_client;
ENetAddress m_address;
ENetPeer* m_peer;
ENetEvent m_event;
*/
CActiveSocket m_tcpSocket;
bool m_isConnected;
CActiveSocket m_tcpSocket;
bool m_isConnected;
TcpNetworkedInternalData* m_tcpInternalData;
SharedMemoryCommand m_clientCmd;
bool m_hasCommand;
@@ -50,12 +46,10 @@ struct TcpNetworkedInternalData
double m_timeOutInSeconds;
TcpNetworkedInternalData()
:
m_isConnected(false),
m_hasCommand(false),
m_timeOutInSeconds(60)
: m_isConnected(false),
m_hasCommand(false),
m_timeOutInSeconds(60)
{
}
bool connectTCP()
@@ -63,17 +57,17 @@ struct TcpNetworkedInternalData
if (m_isConnected)
return true;
m_tcpSocket.Initialize();
m_isConnected = m_tcpSocket.Open(m_hostName.c_str(),m_port);
if (m_isConnected)
m_tcpSocket.Initialize();
m_isConnected = m_tcpSocket.Open(m_hostName.c_str(), m_port);
if (m_isConnected)
{
m_tcpSocket.SetSendTimeout(m_timeOutInSeconds,0);
m_tcpSocket.SetReceiveTimeout(m_timeOutInSeconds,0);
m_tcpSocket.SetSendTimeout(m_timeOutInSeconds, 0);
m_tcpSocket.SetReceiveTimeout(m_timeOutInSeconds, 0);
}
int key = SHARED_MEMORY_MAGIC_NUMBER;
m_tcpSocket.Send((uint8*)&key,4);
m_tcpSocket.Send((uint8*)&key, 4);
return m_isConnected;
}
@@ -82,10 +76,10 @@ struct TcpNetworkedInternalData
bool hasStatus = false;
//int serviceResult = enet_host_service(m_client, &m_event, 0);
int maxLen = 4 + sizeof(SharedMemoryStatus)+SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE;
int rBytes = m_tcpSocket.Receive(maxLen);
if (rBytes<=0)
int maxLen = 4 + sizeof(SharedMemoryStatus) + SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE;
int rBytes = m_tcpSocket.Receive(maxLen);
if (rBytes <= 0)
return false;
//append to tmp buffer
@@ -93,17 +87,16 @@ struct TcpNetworkedInternalData
unsigned char* d2 = (unsigned char*)m_tcpSocket.GetData();
int curSize = m_tempBuffer.size();
m_tempBuffer.resize(curSize+rBytes);
for (int i=0;i<rBytes;i++)
m_tempBuffer.resize(curSize + rBytes);
for (int i = 0; i < rBytes; i++)
{
m_tempBuffer[curSize+i] = d2[i];
m_tempBuffer[curSize + i] = d2[i];
}
int packetSizeInBytes = -1;
if (m_tempBuffer.size()>=4)
if (m_tempBuffer.size() >= 4)
{
packetSizeInBytes = b3DeserializeInt2(&m_tempBuffer[0]);
}
@@ -127,7 +120,6 @@ struct TcpNetworkedInternalData
}
else
{
m_lastStatus = *statPtr;
int streamOffsetInBytes = 4 + sizeof(SharedMemoryStatus);
int numStreamBytes = packetSizeInBytes - streamOffsetInBytes;
@@ -141,10 +133,8 @@ struct TcpNetworkedInternalData
}
return hasStatus;
}
};
TcpNetworkedPhysicsProcessor::TcpNetworkedPhysicsProcessor(const char* hostName, int port)
{
m_data = new TcpNetworkedInternalData;
@@ -153,7 +143,6 @@ TcpNetworkedPhysicsProcessor::TcpNetworkedPhysicsProcessor(const char* hostName,
m_data->m_hostName = hostName;
}
m_data->m_port = port;
}
TcpNetworkedPhysicsProcessor::~TcpNetworkedPhysicsProcessor()
@@ -164,24 +153,23 @@ TcpNetworkedPhysicsProcessor::~TcpNetworkedPhysicsProcessor()
bool TcpNetworkedPhysicsProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
if(gVerboseNetworkMessagesClient2)
if (gVerboseNetworkMessagesClient2)
{
printf("PhysicsClientTCP::processCommand\n");
}
{
int sz = 0;
unsigned char* data = 0;
m_data->m_tempBuffer.clear();
unsigned char* data = 0;
m_data->m_tempBuffer.clear();
if (clientCmd.m_type == CMD_STEP_FORWARD_SIMULATION)
{
sz = sizeof(int);
data = (unsigned char*) &clientCmd.m_type;
data = (unsigned char*)&clientCmd.m_type;
}
else
{
if (clientCmd.m_type == CMD_REQUEST_VR_EVENTS_DATA)
{
sz = 3 * sizeof(int) + sizeof(smUint64_t) + 16;
@@ -192,11 +180,10 @@ bool TcpNetworkedPhysicsProcessor::processCommand(const struct SharedMemoryComma
sz = sizeof(SharedMemoryCommand);
data = (unsigned char*)&clientCmd;
}
}
m_data->m_tcpSocket.Send((const uint8 *)data,sz);
}
}
m_data->m_tcpSocket.Send((const uint8*)data, sz);
}
return false;
}
@@ -226,20 +213,16 @@ bool TcpNetworkedPhysicsProcessor::receiveStatus(struct SharedMemoryStatus& serv
{
printf("Error: steam buffer overflow\n");
}
}
return hasStatus;
}
void TcpNetworkedPhysicsProcessor::renderScene(int renderFlags)
{
}
void TcpNetworkedPhysicsProcessor::physicsDebugDraw(int debugDrawFlags)
void TcpNetworkedPhysicsProcessor::physicsDebugDraw(int debugDrawFlags)
{
}
@@ -252,7 +235,6 @@ bool TcpNetworkedPhysicsProcessor::isConnected() const
return m_data->m_isConnected;
}
bool TcpNetworkedPhysicsProcessor::connect()
{
bool isConnected = m_data->connectTCP();
@@ -261,17 +243,13 @@ bool TcpNetworkedPhysicsProcessor::connect()
void TcpNetworkedPhysicsProcessor::disconnect()
{
const char msg[16]="disconnect";
m_data->m_tcpSocket.Send((const uint8 *)msg,10);
const char msg[16] = "disconnect";
m_data->m_tcpSocket.Send((const uint8*)msg, 10);
m_data->m_tcpSocket.Close();
m_data->m_isConnected = false;
}
void TcpNetworkedPhysicsProcessor::setTimeOut(double timeOutInSeconds)
{
m_data->m_timeOutInSeconds = timeOutInSeconds;
}

9
examples/SharedMemory/PhysicsClientTCP.h
View File

@@ -6,7 +6,6 @@
class TcpNetworkedPhysicsProcessor : public PhysicsCommandProcessorInterface
{
struct TcpNetworkedInternalData* m_data;
public:
@@ -19,14 +18,14 @@ public:
virtual void disconnect();
virtual bool isConnected() const;
virtual bool processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
virtual bool receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
virtual void renderScene(int renderFlags);
virtual void physicsDebugDraw(int debugDrawFlags);
virtual void physicsDebugDraw(int debugDrawFlags);
virtual void setGuiHelper(struct GUIHelperInterface* guiHelper);
@@ -35,6 +34,4 @@ public:
virtual void reportNotifications() {}
};
#endif //PHYSICS_CLIENT_TCP_H
#endif //PHYSICS_CLIENT_TCP_H

7
examples/SharedMemory/PhysicsClientTCP_C_API.cpp
View File

@@ -4,9 +4,8 @@
#include "PhysicsDirect.h"
#include <stdio.h>
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsTCP(const char* hostName, int port)
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsTCP(const char* hostName, int port)
{
TcpNetworkedPhysicsProcessor* tcp = new TcpNetworkedPhysicsProcessor(hostName, port);
PhysicsDirect* direct = new PhysicsDirect(tcp, true);
@@ -20,10 +19,6 @@ B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsTCP(const char* hostName, in
else
{
printf("b3ConnectPhysicsTCP connection failed.\n");
}
return (b3PhysicsClientHandle)direct;
}

9
examples/SharedMemory/PhysicsClientTCP_C_API.h
View File

@@ -4,16 +4,15 @@
#include "PhysicsClientC_API.h"
#ifdef __cplusplus
extern "C" {
extern "C"
{
#endif
///send physics commands using TCP networking
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsTCP(const char* hostName, int port);
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsTCP(const char* hostName, int port);
#ifdef __cplusplus
}
#endif
#endif //PHYSICS_CLIENT_TCP_C_API_H
#endif //PHYSICS_CLIENT_TCP_C_API_H

306
examples/SharedMemory/PhysicsClientUDP.cpp
View File

@@ -9,9 +9,9 @@
#include <string>
#include "Bullet3Common/b3Logging.h"
#include "../MultiThreading/b3ThreadSupportInterface.h"
void UDPThreadFunc(void* userPtr, void* lsMemory);
void* UDPlsMemoryFunc();
void UDPlsMemoryReleaseFunc(void* ptr);
void UDPThreadFunc(void* userPtr, void* lsMemory);
void* UDPlsMemoryFunc();
void UDPlsMemoryReleaseFunc(void* ptr);
bool gVerboseNetworkMessagesClient = false;
@@ -21,63 +21,55 @@ bool gVerboseNetworkMessagesClient = false;
b3ThreadSupportInterface* createUDPThreadSupport(int numThreads)
{
b3PosixThreadSupport::ThreadConstructionInfo constructionInfo("UDPThread",
UDPThreadFunc,
UDPlsMemoryFunc,
UDPlsMemoryReleaseFunc,
numThreads);
UDPThreadFunc,
UDPlsMemoryFunc,
UDPlsMemoryReleaseFunc,
numThreads);
b3ThreadSupportInterface* threadSupport = new b3PosixThreadSupport(constructionInfo);
return threadSupport;
}
#elif defined( _WIN32)
#elif defined(_WIN32)
#include "../MultiThreading/b3Win32ThreadSupport.h"
b3ThreadSupportInterface* createUDPThreadSupport(int numThreads)
{
b3Win32ThreadSupport::Win32ThreadConstructionInfo threadConstructionInfo("UDPThread", UDPThreadFunc, UDPlsMemoryFunc,UDPlsMemoryReleaseFunc, numThreads);
b3Win32ThreadSupport::Win32ThreadConstructionInfo threadConstructionInfo("UDPThread", UDPThreadFunc, UDPlsMemoryFunc, UDPlsMemoryReleaseFunc, numThreads);
b3Win32ThreadSupport* threadSupport = new b3Win32ThreadSupport(threadConstructionInfo);
return threadSupport;
}
#endif
struct UDPThreadLocalStorage
{
int threadId;
};
unsigned int b3DeserializeInt(const unsigned char* input)
{
unsigned int tmp = (input[3] << 24) + (input[2] << 16) + (input[1] << 8) + input[0];
return tmp;
}
struct UdpNetworkedInternalData
struct UdpNetworkedInternalData
{
ENetHost* m_client;
ENetAddress m_address;
ENetPeer* m_peer;
ENetEvent m_event;
bool m_isConnected;
ENetHost* m_client;
ENetAddress m_address;
ENetPeer* m_peer;
ENetEvent m_event;
bool m_isConnected;
b3ThreadSupportInterface* m_threadSupport;
b3CriticalSection* m_cs;
UdpNetworkedInternalData* m_udpInternalData;
SharedMemoryCommand m_clientCmd;
bool m_hasCommand;
bool m_hasStatus;
bool m_hasStatus;
SharedMemoryStatus m_lastStatus;
b3AlignedObjectArray<char> m_stream;
@@ -86,15 +78,14 @@ struct UdpNetworkedInternalData
double m_timeOutInSeconds;
UdpNetworkedInternalData()
:m_client(0),
m_peer(0),
m_isConnected(false),
m_threadSupport(0),
m_hasCommand(false),
m_hasStatus(false),
m_timeOutInSeconds(60)
: m_client(0),
m_peer(0),
m_isConnected(false),
m_threadSupport(0),
m_hasCommand(false),
m_hasStatus(false),
m_timeOutInSeconds(60)
{
}
bool connectUDP()
@@ -102,23 +93,21 @@ struct UdpNetworkedInternalData
if (m_isConnected)
return true;
if (enet_initialize() != 0)
{
fprintf(stderr, "Error initialising enet");
exit(EXIT_FAILURE);
exit(EXIT_FAILURE);
}
m_client = enet_host_create(NULL, /* create a client host */
1, /* number of clients */
2, /* number of channels */
57600 / 8, /* incoming bandwith */
14400 / 8); /* outgoing bandwith */
m_client = enet_host_create(NULL, /* create a client host */
1, /* number of clients */
2, /* number of channels */
57600 / 8, /* incoming bandwith */
14400 / 8); /* outgoing bandwith */
if (m_client == NULL) {
if (m_client == NULL)
{
fprintf(stderr, "Could not create client host");
return false;
}
@@ -127,19 +116,19 @@ struct UdpNetworkedInternalData
m_address.port = m_port;
m_peer = enet_host_connect(m_client,
&m_address, /* address to connect to */
2, /* number of channels */
0); /* user data supplied to
&m_address, /* address to connect to */
2, /* number of channels */
0); /* user data supplied to
the receiving host */
if (m_peer == NULL) {
fprintf(stderr, "No available peers for initiating an ENet "
"connection.\n");
if (m_peer == NULL)
{
fprintf(stderr,
"No available peers for initiating an ENet "
"connection.\n");
return false;
}
/* Try to connect to server within 5 seconds */
if (enet_host_service(m_client, &m_event, 5000) > 0 &&
m_event.type == ENET_EVENT_TYPE_CONNECT)
@@ -157,45 +146,45 @@ struct UdpNetworkedInternalData
return false;
}
int serviceResult = enet_host_service(m_client, &m_event, 0);
if (serviceResult > 0)
{
switch (m_event.type)
{
case ENET_EVENT_TYPE_CONNECT:
printf("A new client connected from %x:%u.\n",
m_event.peer->address.host,
m_event.peer->address.port);
m_event.peer->data = (void*)"New User";
break;
case ENET_EVENT_TYPE_CONNECT:
printf("A new client connected from %x:%u.\n",
m_event.peer->address.host,
m_event.peer->address.port);
m_event.peer->data = (void*)"New User";
break;
case ENET_EVENT_TYPE_RECEIVE:
if (gVerboseNetworkMessagesClient)
{
printf("A packet of length %lu containing '%s' was "
"received from %s on channel %u.\n",
m_event.packet->dataLength,
(char*)m_event.packet->data,
(char*)m_event.peer->data,
m_event.channelID);
}
/* Clean up the packet now that we're done using it.
> */
enet_packet_destroy(m_event.packet);
case ENET_EVENT_TYPE_RECEIVE:
break;
case ENET_EVENT_TYPE_DISCONNECT:
printf("%s disconnected.\n", (char*)m_event.peer->data);
break;
default:
if (gVerboseNetworkMessagesClient)
{
printf("unknown event type: %d.\n", m_event.type);
printf(
"A packet of length %lu containing '%s' was "
"received from %s on channel %u.\n",
m_event.packet->dataLength,
(char*)m_event.packet->data,
(char*)m_event.peer->data,
m_event.channelID);
}
/* Clean up the packet now that we're done using it.
> */
enet_packet_destroy(m_event.packet);
break;
case ENET_EVENT_TYPE_DISCONNECT:
printf("%s disconnected.\n", (char*)m_event.peer->data);
break;
default:
{
printf("unknown event type: %d.\n", m_event.type);
}
}
}
else if (serviceResult > 0)
@@ -218,67 +207,66 @@ struct UdpNetworkedInternalData
{
switch (m_event.type)
{
case ENET_EVENT_TYPE_CONNECT:
printf("A new client connected from %x:%u.\n",
m_event.peer->address.host,
m_event.peer->address.port);
case ENET_EVENT_TYPE_CONNECT:
printf("A new client connected from %x:%u.\n",
m_event.peer->address.host,
m_event.peer->address.port);
m_event.peer->data = (void*)"New User";
break;
m_event.peer->data = (void*)"New User";
break;
case ENET_EVENT_TYPE_RECEIVE:
{
if (gVerboseNetworkMessagesClient)
case ENET_EVENT_TYPE_RECEIVE:
{
printf("A packet of length %lu containing '%s' was "
"received from %s on channel %u.\n",
m_event.packet->dataLength,
(char*)m_event.packet->data,
(char*)m_event.peer->data,
m_event.channelID);
}
int packetSizeInBytes = b3DeserializeInt(m_event.packet->data);
if (packetSizeInBytes == m_event.packet->dataLength)
{
SharedMemoryStatus* statPtr = (SharedMemoryStatus*)&m_event.packet->data[4];
if (statPtr->m_type == CMD_STEP_FORWARD_SIMULATION_COMPLETED)
if (gVerboseNetworkMessagesClient)
{
SharedMemoryStatus dummy;
dummy.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED;
m_lastStatus = dummy;
m_stream.resize(0);
printf(
"A packet of length %lu containing '%s' was "
"received from %s on channel %u.\n",
m_event.packet->dataLength,
(char*)m_event.packet->data,
(char*)m_event.peer->data,
m_event.channelID);
}
int packetSizeInBytes = b3DeserializeInt(m_event.packet->data);
if (packetSizeInBytes == m_event.packet->dataLength)
{
SharedMemoryStatus* statPtr = (SharedMemoryStatus*)&m_event.packet->data[4];
if (statPtr->m_type == CMD_STEP_FORWARD_SIMULATION_COMPLETED)
{
SharedMemoryStatus dummy;
dummy.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED;
m_lastStatus = dummy;
m_stream.resize(0);
}
else
{
m_lastStatus = *statPtr;
int streamOffsetInBytes = 4 + sizeof(SharedMemoryStatus);
int numStreamBytes = packetSizeInBytes - streamOffsetInBytes;
m_stream.resize(numStreamBytes);
for (int i = 0; i < numStreamBytes; i++)
{
m_stream[i] = m_event.packet->data[i + streamOffsetInBytes];
}
}
}
else
{
m_lastStatus = *statPtr;
int streamOffsetInBytes = 4 + sizeof(SharedMemoryStatus);
int numStreamBytes = packetSizeInBytes - streamOffsetInBytes;
m_stream.resize(numStreamBytes);
for (int i = 0; i < numStreamBytes; i++)
{
m_stream[i] = m_event.packet->data[i + streamOffsetInBytes];
}
printf("unknown status message received\n");
}
enet_packet_destroy(m_event.packet);
hasStatus = true;
break;
}
else
case ENET_EVENT_TYPE_DISCONNECT:
{
printf("unknown status message received\n");
}
enet_packet_destroy(m_event.packet);
hasStatus = true;
break;
}
case ENET_EVENT_TYPE_DISCONNECT:
{
printf("%s disconnected.\n", (char*)m_event.peer->data);
printf("%s disconnected.\n", (char*)m_event.peer->data);
break;
}
default:
break;
}
default:
{
printf("unknown event type: %d.\n", m_event.type);
}
@@ -291,7 +279,6 @@ struct UdpNetworkedInternalData
return hasStatus;
}
};
enum UDPThreadEnums
@@ -303,8 +290,6 @@ enum UDPThreadEnums
eUDPHasTerminated
};
enum UDPCommandEnums
{
eUDPIdle = 13,
@@ -316,24 +301,22 @@ enum UDPCommandEnums
};
void UDPThreadFunc(void* userPtr, void* lsMemory)
void UDPThreadFunc(void* userPtr, void* lsMemory)
{
printf("UDPThreadFunc thread started\n");
// UDPThreadLocalStorage* localStorage = (UDPThreadLocalStorage*)lsMemory;
// UDPThreadLocalStorage* localStorage = (UDPThreadLocalStorage*)lsMemory;
UdpNetworkedInternalData* args = (UdpNetworkedInternalData*)userPtr;
// int workLeft = true;
// int workLeft = true;
b3Clock clock;
clock.reset();
bool init = true;
if (init)
{
args->m_cs->lock();
args->m_cs->setSharedParam(0, eUDPIsInitialized);
args->m_cs->unlock();
double deltaTimeInSeconds = 0;
do
@@ -343,7 +326,6 @@ void UDPThreadFunc(void* userPtr, void* lsMemory)
deltaTimeInSeconds += double(clock.getTimeMicroseconds()) / 1000000.;
{
clock.reset();
deltaTimeInSeconds = 0.f;
switch (args->m_cs->getSharedParam(1))
@@ -363,21 +345,19 @@ void UDPThreadFunc(void* userPtr, void* lsMemory)
}
default:
{
}
}
};
if (args->m_isConnected)
{
args->m_cs->lock();
bool hasCommand = args->m_hasCommand;
args->m_cs->unlock();
if (hasCommand)
{
int sz = 0;
ENetPacket *packet = 0;
ENetPacket* packet = 0;
if (args->m_clientCmd.m_type == CMD_STEP_FORWARD_SIMULATION)
{
@@ -396,7 +376,6 @@ void UDPThreadFunc(void* userPtr, void* lsMemory)
args->m_cs->unlock();
}
bool hasNewStatus = args->checkData();
if (hasNewStatus)
{
@@ -414,7 +393,6 @@ void UDPThreadFunc(void* userPtr, void* lsMemory)
}
}
}
}
} while (args->m_cs->getSharedParam(0) != eUDPRequestTerminate);
@@ -426,30 +404,21 @@ void UDPThreadFunc(void* userPtr, void* lsMemory)
args->m_cs->unlock();
}
printf("finished\n");
}
void* UDPlsMemoryFunc()
void* UDPlsMemoryFunc()
{
//don't create local store memory, just return 0
return new UDPThreadLocalStorage;
}
void UDPlsMemoryReleaseFunc(void* ptr)
void UDPlsMemoryReleaseFunc(void* ptr)
{
UDPThreadLocalStorage* p = (UDPThreadLocalStorage*) ptr;
UDPThreadLocalStorage* p = (UDPThreadLocalStorage*)ptr;
delete p;
}
UdpNetworkedPhysicsProcessor::UdpNetworkedPhysicsProcessor(const char* hostName, int port)
{
m_data = new UdpNetworkedInternalData;
@@ -458,7 +427,6 @@ UdpNetworkedPhysicsProcessor::UdpNetworkedPhysicsProcessor(const char* hostName,
m_data->m_hostName = hostName;
}
m_data->m_port = port;
}
UdpNetworkedPhysicsProcessor::~UdpNetworkedPhysicsProcessor()
@@ -469,12 +437,12 @@ UdpNetworkedPhysicsProcessor::~UdpNetworkedPhysicsProcessor()
bool UdpNetworkedPhysicsProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
if(gVerboseNetworkMessagesClient)
if (gVerboseNetworkMessagesClient)
{
printf("PhysicsClientUDP::processCommand\n");
}
// int sz = sizeof(SharedMemoryCommand);
// int sz = sizeof(SharedMemoryCommand);
b3Clock clock;
double startTime = clock.getTimeInSeconds();
double timeOutInSeconds = m_data->m_timeOutInSeconds;
@@ -484,7 +452,7 @@ bool UdpNetworkedPhysicsProcessor::processCommand(const struct SharedMemoryComma
m_data->m_hasCommand = true;
m_data->m_cs->unlock();
while ((m_data->m_hasCommand) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds))
while ((m_data->m_hasCommand) && (clock.getTimeInSeconds() - startTime < timeOutInSeconds))
{
b3Clock::usleep(0);
}
@@ -542,17 +510,14 @@ bool UdpNetworkedPhysicsProcessor::receiveStatus(struct SharedMemoryStatus& serv
m_data->m_cs->unlock();
}
return hasStatus;
}
void UdpNetworkedPhysicsProcessor::renderScene(int renderFlags)
{
}
void UdpNetworkedPhysicsProcessor::physicsDebugDraw(int debugDrawFlags)
void UdpNetworkedPhysicsProcessor::physicsDebugDraw(int debugDrawFlags)
{
}
@@ -565,16 +530,15 @@ bool UdpNetworkedPhysicsProcessor::isConnected() const
return m_data->m_isConnected;
}
bool UdpNetworkedPhysicsProcessor::connect()
{
if (m_data->m_threadSupport==0)
if (m_data->m_threadSupport == 0)
{
m_data->m_threadSupport = createUDPThreadSupport(1);
m_data->m_cs = m_data->m_threadSupport->createCriticalSection();
m_data->m_cs->setSharedParam(0, eUDPIsUnInitialized);
m_data->m_threadSupport->runTask(B3_THREAD_SCHEDULE_TASK, (void*) m_data, 0);
m_data->m_threadSupport->runTask(B3_THREAD_SCHEDULE_TASK, (void*)m_data, 0);
while (m_data->m_cs->getSharedParam(0) == eUDPIsUnInitialized)
{
@@ -589,7 +553,6 @@ bool UdpNetworkedPhysicsProcessor::connect()
{
b3Clock::usleep(1000);
}
}
unsigned int sharedParam = m_data->m_cs->getSharedParam(1);
bool isConnected = (sharedParam == eUDP_Connected);
@@ -626,9 +589,6 @@ void UdpNetworkedPhysicsProcessor::disconnect()
m_data->m_threadSupport = 0;
m_data->m_isConnected = false;
}
}
void UdpNetworkedPhysicsProcessor::setTimeOut(double timeOutInSeconds)

9
examples/SharedMemory/PhysicsClientUDP.h
View File

@@ -6,7 +6,6 @@
class UdpNetworkedPhysicsProcessor : public PhysicsCommandProcessorInterface
{
struct UdpNetworkedInternalData* m_data;
public:
@@ -19,14 +18,14 @@ public:
virtual void disconnect();
virtual bool isConnected() const;
virtual bool processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
virtual bool receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
virtual void renderScene(int renderFlags);
virtual void physicsDebugDraw(int debugDrawFlags);
virtual void physicsDebugDraw(int debugDrawFlags);
virtual void setGuiHelper(struct GUIHelperInterface* guiHelper);
@@ -35,6 +34,4 @@ public:
virtual void reportNotifications() {}
};
#endif //PHYSICS_CLIENT_UDP_H
#endif //PHYSICS_CLIENT_UDP_H

7
examples/SharedMemory/PhysicsClientUDP_C_API.cpp
View File

@@ -5,9 +5,8 @@
#include <stdio.h>
//think more about naming. The b3ConnectPhysicsLoopback
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsUDP(const char* hostName, int port)
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsUDP(const char* hostName, int port)
{
UdpNetworkedPhysicsProcessor* udp = new UdpNetworkedPhysicsProcessor(hostName, port);
PhysicsDirect* direct = new PhysicsDirect(udp, true);
@@ -21,10 +20,6 @@ B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsUDP(const char* hostName, in
else
{
printf("b3ConnectPhysicsUDP connection failed.\n");
}
return (b3PhysicsClientHandle)direct;
}

9
examples/SharedMemory/PhysicsClientUDP_C_API.h
View File

@@ -4,16 +4,15 @@
#include "PhysicsClientC_API.h"
#ifdef __cplusplus
extern "C" {
extern "C"
{
#endif
///send physics commands using UDP networking
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsUDP(const char* hostName, int port);
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsUDP(const char* hostName, int port);
#ifdef __cplusplus
}
#endif
#endif //PHYSICS_CLIENT_UDP_C_API_H
#endif //PHYSICS_CLIENT_UDP_C_API_H

44
examples/SharedMemory/PhysicsCommandProcessorInterface.h
View File

@@ -3,16 +3,15 @@
enum PhysicsCommandRenderFlags
{
COV_DISABLE_SYNC_RENDERING=1
COV_DISABLE_SYNC_RENDERING = 1
};
class PhysicsCommandProcessorInterface
{
public:
virtual ~PhysicsCommandProcessorInterface() {}
virtual bool connect()=0;
virtual bool connect() = 0;
virtual void disconnect() = 0;
@@ -23,46 +22,41 @@ public:
virtual bool receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) = 0;
virtual void renderScene(int renderFlags) = 0;
virtual void physicsDebugDraw(int debugDrawFlags) = 0;
virtual void physicsDebugDraw(int debugDrawFlags) = 0;
virtual void setGuiHelper(struct GUIHelperInterface* guiHelper) = 0;
virtual void setTimeOut(double timeOutInSeconds) = 0;
virtual void reportNotifications() = 0;
};
class btVector3;
class btQuaternion;
class CommandProcessorInterface : public PhysicsCommandProcessorInterface
{
public:
virtual ~CommandProcessorInterface(){}
virtual ~CommandProcessorInterface() {}
virtual void syncPhysicsToGraphics()=0;
virtual void stepSimulationRealTime(double dtInSec,const struct b3VRControllerEvent* vrControllerEvents, int numVRControllerEvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents, const struct b3MouseEvent* mouseEvents, int numMouseEvents)=0;
virtual void enableRealTimeSimulation(bool enableRealTimeSim)=0;
virtual bool isRealTimeSimulationEnabled() const=0;
virtual void syncPhysicsToGraphics() = 0;
virtual void stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrControllerEvents, int numVRControllerEvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents, const struct b3MouseEvent* mouseEvents, int numMouseEvents) = 0;
virtual void enableRealTimeSimulation(bool enableRealTimeSim) = 0;
virtual bool isRealTimeSimulationEnabled() const = 0;
virtual void enableCommandLogging(bool enable, const char* fileName) = 0;
virtual void replayFromLogFile(const char* fileName) = 0;
virtual void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes) = 0;
virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) = 0;
virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) = 0;
virtual void removePickingConstraint() = 0;
virtual void enableCommandLogging(bool enable, const char* fileName)=0;
virtual void replayFromLogFile(const char* fileName)=0;
virtual void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes )=0;
virtual const btVector3& getVRTeleportPosition() const = 0;
virtual void setVRTeleportPosition(const btVector3& vrReleportPos) = 0;
virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)=0;
virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)=0;
virtual void removePickingConstraint()=0;
virtual const btVector3& getVRTeleportPosition() const=0;
virtual void setVRTeleportPosition(const btVector3& vrReleportPos)=0;
virtual const btQuaternion& getVRTeleportOrientation() const=0;
virtual void setVRTeleportOrientation(const btQuaternion& vrReleportOrn)=0;
virtual const btQuaternion& getVRTeleportOrientation() const = 0;
virtual void setVRTeleportOrientation(const btQuaternion& vrReleportOrn) = 0;
virtual void processClientCommands() = 0;
};
#endif //PHYSICS_COMMAND_PROCESSOR_INTERFACE_H
#endif //PHYSICS_COMMAND_PROCESSOR_INTERFACE_H

1392
examples/SharedMemory/PhysicsDirect.cpp
View File

File diff suppressed because it is too large Load Diff

77
examples/SharedMemory/PhysicsDirect.h
View File

@@ -3,28 +3,26 @@
//#include "SharedMemoryCommands.h"
#include "PhysicsClient.h"
#include "LinearMath/btVector3.h"
///PhysicsDirect executes the commands directly, without transporting them or having a separate server executing commands
class PhysicsDirect : public PhysicsClient
class PhysicsDirect : public PhysicsClient
{
protected:
struct PhysicsDirectInternalData* m_data;
bool processDebugLines(const struct SharedMemoryCommand& orgCommand);
bool processCamera(const struct SharedMemoryCommand& orgCommand);
bool processContactPointData(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 processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
void processAddUserData(const struct SharedMemoryStatus& serverCmd);
@@ -35,28 +33,27 @@ protected:
void removeCachedBody(int bodyUniqueId);
public:
PhysicsDirect(class PhysicsCommandProcessorInterface* physSdk, bool passSdkOwnership);
virtual ~PhysicsDirect();
virtual ~PhysicsDirect();
// return true if connection succesfull, can also check 'isConnected'
//it is OK to pass a null pointer for the gui helper
virtual bool connect();
virtual bool connect();
////todo: rename to 'disconnect'
virtual void disconnectSharedMemory();
virtual void disconnectSharedMemory();
virtual bool isConnected() const;
virtual bool isConnected() const;
// return non-null if there is a status, nullptr otherwise
virtual const SharedMemoryStatus* processServerStatus();
// return non-null if there is a status, nullptr otherwise
virtual const SharedMemoryStatus* processServerStatus();
virtual SharedMemoryCommand* getAvailableSharedMemoryCommand();
virtual SharedMemoryCommand* getAvailableSharedMemoryCommand();
virtual bool canSubmitCommand() const;
virtual bool canSubmitCommand() const;
virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
virtual int getNumBodies() const;
@@ -64,37 +61,37 @@ public:
virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const;
virtual int getNumJoints(int bodyIndex) const;
virtual int getNumJoints(int bodyIndex) const;
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) 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 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);
///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 int getNumDebugLines() const;
virtual const float* getDebugLinesFrom() const;
virtual const float* getDebugLinesTo() const;
virtual const float* getDebugLinesColor() 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 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);
@@ -115,13 +112,13 @@ public:
virtual void setTimeOut(double timeOutInSeconds);
virtual double getTimeOut() const;
virtual bool getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const;
virtual bool getCachedUserData(int userDataId, struct b3UserDataValue& valueOut) const;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char* key) const;
virtual int getNumUserData(int bodyUniqueId) const;
virtual void getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const;
virtual void getUserDataInfo(int bodyUniqueId, int userDataIndex, const char** keyOut, int* userDataIdOut, int* linkIndexOut, int* visualShapeIndexOut) const;
virtual void pushProfileTiming(const char* timingName);
virtual void popProfileTiming();
};
#endif //PHYSICS_DIRECT_H
#endif //PHYSICS_DIRECT_H

11
examples/SharedMemory/PhysicsDirectC_API.cpp
View File

@@ -4,20 +4,15 @@
#include "PhysicsServerCommandProcessor.h"
//think more about naming. The b3ConnectPhysicsLoopback
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsDirect()
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsDirect()
{
PhysicsServerCommandProcessor* sdk = new PhysicsServerCommandProcessor;
PhysicsDirect* direct = new PhysicsDirect(sdk,true);
PhysicsDirect* direct = new PhysicsDirect(sdk, true);
bool connected;
connected = direct->connect();
return (b3PhysicsClientHandle )direct;
return (b3PhysicsClientHandle)direct;
}
//

11
examples/SharedMemory/PhysicsDirectC_API.h
View File

@@ -4,16 +4,15 @@
#include "PhysicsClientC_API.h"
#ifdef __cplusplus
extern "C" {
extern "C"
{
#endif
///think more about naming. Directly execute commands without transport (no shared memory, UDP, socket, grpc etc)
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsDirect();
///think more about naming. Directly execute commands without transport (no shared memory, UDP, socket, grpc etc)
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsDirect();
#ifdef __cplusplus
}
#endif
#endif //PHYSICS_DIRECT_C_API_H
#endif //PHYSICS_DIRECT_C_API_H

42
examples/SharedMemory/PhysicsLoopBack.cpp
View File

@@ -10,12 +10,11 @@ struct PhysicsLoopBackInternalData
PhysicsClientSharedMemory* m_physicsClient;
PhysicsServerSharedMemory* m_physicsServer;
DummyGUIHelper m_noGfx;
PhysicsLoopBackInternalData()
:m_commandProcessor(0),
m_physicsClient(0),
m_physicsServer(0)
: m_commandProcessor(0),
m_physicsClient(0),
m_physicsServer(0)
{
}
};
@@ -39,7 +38,7 @@ static Bullet2CommandProcessorCreation2 sB2Proc;
PhysicsLoopBack::PhysicsLoopBack()
{
m_data = new PhysicsLoopBackInternalData;
m_data->m_physicsServer = new PhysicsServerSharedMemory(&sB2Proc, 0,0);
m_data->m_physicsServer = new PhysicsServerSharedMemory(&sB2Proc, 0, 0);
m_data->m_physicsClient = new PhysicsClientSharedMemory();
}
@@ -51,7 +50,6 @@ PhysicsLoopBack::~PhysicsLoopBack()
delete m_data;
}
// return true if connection succesfull, can also check 'isConnected'
bool PhysicsLoopBack::connect()
{
@@ -73,7 +71,7 @@ bool PhysicsLoopBack::isConnected() const
}
// return non-null if there is a status, nullptr otherwise
const SharedMemoryStatus* PhysicsLoopBack::processServerStatus()
const SharedMemoryStatus* PhysicsLoopBack::processServerStatus()
{
m_data->m_physicsServer->processClientCommands();
return m_data->m_physicsClient->processServerStatus();
@@ -91,7 +89,7 @@ bool PhysicsLoopBack::canSubmitCommand() const
bool PhysicsLoopBack::submitClientCommand(const struct SharedMemoryCommand& command)
{
return m_data->m_physicsClient->submitClientCommand(command);
return m_data->m_physicsClient->submitClientCommand(command);
}
int PhysicsLoopBack::getNumBodies() const
@@ -116,16 +114,16 @@ int PhysicsLoopBack::getNumJoints(int bodyIndex) const
bool PhysicsLoopBack::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const
{
return m_data->m_physicsClient->getJointInfo(bodyIndex,jointIndex,info);
return m_data->m_physicsClient->getJointInfo(bodyIndex, jointIndex, info);
}
int PhysicsLoopBack::getNumUserConstraints() const
{
return m_data->m_physicsClient->getNumUserConstraints();
return m_data->m_physicsClient->getNumUserConstraints();
}
int PhysicsLoopBack::getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint&info) const
int PhysicsLoopBack::getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const
{
return m_data->m_physicsClient->getUserConstraintInfo( constraintUniqueId, info);
return m_data->m_physicsClient->getUserConstraintInfo(constraintUniqueId, info);
}
int PhysicsLoopBack::getUserConstraintId(int serialIndex) const
@@ -140,10 +138,9 @@ void PhysicsLoopBack::setSharedMemoryKey(int key)
m_data->m_physicsClient->setSharedMemoryKey(key);
}
void PhysicsLoopBack::uploadBulletFileToSharedMemory(const char* data, int len)
{
m_data->m_physicsClient->uploadBulletFileToSharedMemory(data,len);
m_data->m_physicsClient->uploadBulletFileToSharedMemory(data, len);
}
void PhysicsLoopBack::uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays)
@@ -178,7 +175,7 @@ void PhysicsLoopBack::getCachedCameraImage(struct b3CameraImageData* cameraData)
void PhysicsLoopBack::getCachedContactPointInformation(struct b3ContactInformation* contactPointData)
{
return m_data->m_physicsClient->getCachedContactPointInformation(contactPointData);
return m_data->m_physicsClient->getCachedContactPointInformation(contactPointData);
}
void PhysicsLoopBack::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo)
@@ -191,7 +188,6 @@ void PhysicsLoopBack::getCachedCollisionShapeInformation(struct b3CollisionShape
return m_data->m_physicsClient->getCachedCollisionShapeInformation(collisionShapesInfo);
}
void PhysicsLoopBack::getCachedVREvents(struct b3VREventsData* vrEventsData)
{
return m_data->m_physicsClient->getCachedVREvents(vrEventsData);
@@ -219,7 +215,7 @@ void PhysicsLoopBack::getCachedRaycastHits(struct b3RaycastInformation* raycastH
void PhysicsLoopBack::getCachedMassMatrix(int dofCountCheck, double* massMatrix)
{
m_data->m_physicsClient->getCachedMassMatrix(dofCountCheck,massMatrix);
m_data->m_physicsClient->getCachedMassMatrix(dofCountCheck, massMatrix);
}
void PhysicsLoopBack::setTimeOut(double timeOutInSeconds)
@@ -231,19 +227,23 @@ double PhysicsLoopBack::getTimeOut() const
return m_data->m_physicsClient->getTimeOut();
}
bool PhysicsLoopBack::getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const {
bool PhysicsLoopBack::getCachedUserData(int userDataId, struct b3UserDataValue& valueOut) const
{
return m_data->m_physicsClient->getCachedUserData(userDataId, valueOut);
}
int PhysicsLoopBack::getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const {
int PhysicsLoopBack::getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char* key) const
{
return m_data->m_physicsClient->getCachedUserDataId(bodyUniqueId, linkIndex, visualShapeIndex, key);
}
int PhysicsLoopBack::getNumUserData(int bodyUniqueId) const {
int PhysicsLoopBack::getNumUserData(int bodyUniqueId) const
{
return m_data->m_physicsClient->getNumUserData(bodyUniqueId);
}
void PhysicsLoopBack::getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const {
void PhysicsLoopBack::getUserDataInfo(int bodyUniqueId, int userDataIndex, const char** keyOut, int* userDataIdOut, int* linkIndexOut, int* visualShapeIndexOut) const
{
m_data->m_physicsClient->getUserDataInfo(bodyUniqueId, userDataIndex, keyOut, userDataIdOut, linkIndexOut, visualShapeIndexOut);
}

66
examples/SharedMemory/PhysicsLoopBack.h
View File

@@ -3,40 +3,38 @@
//#include "SharedMemoryCommands.h"
#include "PhysicsClient.h"
#include "LinearMath/btVector3.h"
///todo: the PhysicsClient API was designed with shared memory in mind,
///todo: the PhysicsClient API was designed with shared memory in mind,
///now it become more general we need to move out the shared memory specifics away
///for example naming [disconnectSharedMemory -> disconnect] [ move setSharedMemoryKey to shared memory specific subclass ]
class PhysicsLoopBack : public PhysicsClient
class PhysicsLoopBack : public PhysicsClient
{
struct PhysicsLoopBackInternalData* m_data;
public:
PhysicsLoopBack();
virtual ~PhysicsLoopBack();
// return true if connection succesfull, can also check 'isConnected'
virtual bool connect();
// return true if connection succesfull, can also check 'isConnected'
virtual bool connect();
////todo: rename to 'disconnect'
virtual void disconnectSharedMemory();
virtual void disconnectSharedMemory();
virtual bool isConnected() const;
virtual bool isConnected() const;
// return non-null if there is a status, nullptr otherwise
virtual const SharedMemoryStatus* processServerStatus();
// return non-null if there is a status, nullptr otherwise
virtual const SharedMemoryStatus* processServerStatus();
virtual SharedMemoryCommand* getAvailableSharedMemoryCommand();
virtual SharedMemoryCommand* getAvailableSharedMemoryCommand();
virtual bool canSubmitCommand() const;
virtual bool canSubmitCommand() const;
virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
virtual int getNumBodies() const;
@@ -44,30 +42,30 @@ public:
virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const;
virtual int getNumJoints(int bodyIndex) const;
virtual int getNumJoints(int bodyIndex) const;
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) 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 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);
///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 int getNumDebugLines() const;
virtual const float* getDebugLinesFrom() const;
virtual const float* getDebugLinesTo() const;
virtual const float* getDebugLinesColor() const;
virtual const float* getDebugLinesFrom() const;
virtual const float* getDebugLinesTo() const;
virtual const float* getDebugLinesColor() const;
virtual void getCachedCameraImage(struct b3CameraImageData* cameraData);
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects);
@@ -89,13 +87,13 @@ public:
virtual void setTimeOut(double timeOutInSeconds);
virtual double getTimeOut() const;
virtual bool getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const;
virtual bool getCachedUserData(int userDataId, struct b3UserDataValue& valueOut) const;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char* key) const;
virtual int getNumUserData(int bodyUniqueId) const;
virtual void getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const;
virtual void getUserDataInfo(int bodyUniqueId, int userDataIndex, const char** keyOut, int* userDataIdOut, int* linkIndexOut, int* visualShapeIndexOut) const;
virtual void pushProfileTiming(const char* timingName);
virtual void popProfileTiming();
};
#endif //PHYSICS_LOOP_BACK_H
#endif //PHYSICS_LOOP_BACK_H

5
examples/SharedMemory/PhysicsLoopBackC_API.cpp
View File

@@ -2,8 +2,6 @@
#include "PhysicsLoopBack.h"
//think more about naming. The b3ConnectPhysicsLoopback
b3PhysicsClientHandle b3ConnectPhysicsLoopback(int key)
{
@@ -11,6 +9,5 @@ b3PhysicsClientHandle b3ConnectPhysicsLoopback(int key)
loopBack->setSharedMemoryKey(key);
bool connected;
connected = loopBack->connect();
return (b3PhysicsClientHandle )loopBack;
return (b3PhysicsClientHandle)loopBack;
}

11
examples/SharedMemory/PhysicsLoopBackC_API.h
View File

@@ -4,16 +4,15 @@
#include "PhysicsClientC_API.h"
#ifdef __cplusplus
extern "C" {
extern "C"
{
#endif
///think more about naming. The b3ConnectPhysicsLoopback
b3PhysicsClientHandle b3ConnectPhysicsLoopback(int key);
///think more about naming. The b3ConnectPhysicsLoopback
b3PhysicsClientHandle b3ConnectPhysicsLoopback(int key);
#ifdef __cplusplus
}
#endif
#endif //PHYSICS_LOOPBACK_C_API_H
#endif //PHYSICS_LOOPBACK_C_API_H

39
examples/SharedMemory/PhysicsServer.h
View File

@@ -3,44 +3,35 @@
#include "LinearMath/btVector3.h"
class PhysicsServer
{
public:
virtual ~PhysicsServer();
virtual void setSharedMemoryKey(int key)=0;
virtual bool connectSharedMemory( struct GUIHelperInterface* guiHelper)=0;
virtual void disconnectSharedMemory (bool deInitializeSharedMemory)=0;
virtual void setSharedMemoryKey(int key) = 0;
virtual void processClientCommands()=0;
virtual bool connectSharedMemory(struct GUIHelperInterface* guiHelper) = 0;
// virtual bool supportsJointMotor(class btMultiBody* body, int linkIndex)=0;
virtual void disconnectSharedMemory(bool deInitializeSharedMemory) = 0;
virtual void processClientCommands() = 0;
// virtual bool supportsJointMotor(class btMultiBody* body, int linkIndex)=0;
//@todo(erwincoumans) Should we have shared memory commands for picking objects?
///The pickBody method will try to pick the first body along a ray, return true if succeeds, false otherwise
virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld){return false;}
virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld){return false;}
virtual void removePickingConstraint(){}
virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) { return false; }
virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) { return false; }
virtual void removePickingConstraint() {}
//for physicsDebugDraw and renderScene are mainly for debugging purposes
//and for physics visualization. The idea is that physicsDebugDraw can also send wireframe
//to a physics client, over shared memory
virtual void physicsDebugDraw(int debugDrawFlags){}
virtual void renderScene(int renderFlags){}
virtual void physicsDebugDraw(int debugDrawFlags) {}
virtual void renderScene(int renderFlags) {}
virtual void enableCommandLogging(bool enable, const char* fileName){}
virtual void replayFromLogFile(const char* fileName){}
virtual void enableCommandLogging(bool enable, const char* fileName) {}
virtual void replayFromLogFile(const char* fileName) {}
};
#endif //PHYSICS_SERVER_H
#endif //PHYSICS_SERVER_H

7051
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

File diff suppressed because one or more lines are too long

20
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@@ -12,20 +12,15 @@ struct SharedMemLines
btVector3 m_color;
};
///todo: naming. Perhaps PhysicsSdkCommandprocessor?
class PhysicsServerCommandProcessor : public CommandProcessorInterface
{
struct PhysicsServerCommandProcessorInternalData* m_data;
void resetSimulation();
void createThreadPool();
protected:
bool processStateLoggingCommand(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 processSaveWorldCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
@@ -95,13 +90,13 @@ protected:
bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags, btScalar globalScaling);
bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn,
bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes, int flags, btScalar globalScaling);
bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes, int flags, btScalar globalScaling);
bool loadMjcf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags);
bool processImportedObjects(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags, class URDFImporterInterface& u2b);
bool supportsJointMotor(class btMultiBody* body, int linkIndex);
bool supportsJointMotor(class btMultiBody* body, int linkIndex);
int createBodyInfoStream(int bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes);
void deleteCachedInverseDynamicsBodies();
@@ -112,7 +107,7 @@ public:
PhysicsServerCommandProcessor();
virtual ~PhysicsServerCommandProcessor();
void createJointMotors(class btMultiBody* body);
void createJointMotors(class btMultiBody* body);
virtual void createEmptyDynamicsWorld();
virtual void deleteDynamicsWorld();
@@ -137,11 +132,10 @@ public:
};
virtual void renderScene(int renderFlags);
virtual void physicsDebugDraw(int debugDrawFlags);
virtual void physicsDebugDraw(int debugDrawFlags);
virtual void setGuiHelper(struct GUIHelperInterface* guiHelper);
virtual void syncPhysicsToGraphics();
//@todo(erwincoumans) Should we have shared memory commands for picking objects?
///The pickBody method will try to pick the first body along a ray, return true if succeeds, false otherwise
virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld);
@@ -151,7 +145,7 @@ public:
//logging /playback the shared memory commands
virtual void enableCommandLogging(bool enable, const char* fileName);
virtual void replayFromLogFile(const char* fileName);
virtual void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes );
virtual void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes);
//logging of object states (position etc)
virtual void reportNotifications();
@@ -160,7 +154,7 @@ public:
void logObjectStates(btScalar timeStep);
void processCollisionForces(btScalar timeStep);
virtual void stepSimulationRealTime(double dtInSec,const struct b3VRControllerEvent* vrControllerEvents, int numVRControllerEvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents, const struct b3MouseEvent* mouseEvents, int numMouseEvents);
virtual void stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrControllerEvents, int numVRControllerEvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents, const struct b3MouseEvent* mouseEvents, int numMouseEvents);
virtual void enableRealTimeSimulation(bool enableRealTimeSim);
virtual bool isRealTimeSimulationEnabled() const;
@@ -179,4 +173,4 @@ private:
void addTransformChangedNotifications();
};
#endif //PHYSICS_SERVER_COMMAND_PROCESSOR_H
#endif //PHYSICS_SERVER_COMMAND_PROCESSOR_H

2458
examples/SharedMemory/PhysicsServerExample.cpp
View File

File diff suppressed because it is too large Load Diff

10
examples/SharedMemory/PhysicsServerExample.h
View File

@@ -3,15 +3,13 @@
enum PhysicsServerOptions
{
PHYSICS_SERVER_ENABLE_COMMAND_LOGGING=1,
PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG=2,
PHYSICS_SERVER_ENABLE_COMMAND_LOGGING = 1,
PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG = 2,
PHYSICS_SERVER_USE_RTC_CLOCK = 4,
};
///Don't use PhysicsServerCreateFuncInternal directly
///Use PhysicsServerCreateFuncBullet2 instead, or initialize options.m_commandProcessor
class CommonExampleInterface* PhysicsServerCreateFuncInternal(struct CommonExampleOptions& options);
#endif //PHYSICS_SERVER_EXAMPLE_H
class CommonExampleInterface* PhysicsServerCreateFuncInternal(struct CommonExampleOptions& options);
#endif //PHYSICS_SERVER_EXAMPLE_H

8
examples/SharedMemory/PhysicsServerExampleBullet2.cpp
View File

@@ -18,18 +18,14 @@ struct Bullet2CommandProcessorCreation : public CommandProcessorCreationInterfac
}
};
static Bullet2CommandProcessorCreation sBullet2CommandCreator;
CommonExampleInterface* PhysicsServerCreateFuncBullet2(struct CommonExampleOptions& options)
CommonExampleInterface* PhysicsServerCreateFuncBullet2(struct CommonExampleOptions& options)
{
options.m_commandProcessorCreation = &sBullet2CommandCreator;
CommonExampleInterface* example = PhysicsServerCreateFuncInternal(options);
return example;
}
B3_STANDALONE_EXAMPLE(PhysicsServerCreateFuncBullet2)

6
examples/SharedMemory/PhysicsServerExampleBullet2.h
View File

@@ -2,8 +2,6 @@
#ifndef PHYSICS_SERVER_EXAMPLE_BULLET_2_H
#define PHYSICS_SERVER_EXAMPLE_BULLET_2_H
class CommonExampleInterface* PhysicsServerCreateFuncBullet2(struct CommonExampleOptions& options);
class CommonExampleInterface* PhysicsServerCreateFuncBullet2(struct CommonExampleOptions& options);
#endif //PHYSICS_SERVER_EXAMPLE_BULLET_2_H
#endif //PHYSICS_SERVER_EXAMPLE_BULLET_2_H

303
examples/SharedMemory/PhysicsServerSharedMemory.cpp
View File

@@ -12,7 +12,6 @@
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "SharedMemoryBlock.h"
#include "PhysicsCommandProcessorInterface.h"
//number of shared memory blocks == number of simultaneous connections
@@ -20,14 +19,12 @@
struct PhysicsServerSharedMemoryInternalData
{
///end handle management
SharedMemoryInterface* m_sharedMemory;
bool m_ownsSharedMemory;
SharedMemoryBlock* m_testBlocks[MAX_SHARED_MEMORY_BLOCKS];
SharedMemoryBlock* m_testBlocks[MAX_SHARED_MEMORY_BLOCKS];
int m_sharedMemoryKey;
bool m_areConnected[MAX_SHARED_MEMORY_BLOCKS];
bool m_verboseOutput;
@@ -35,37 +32,34 @@ struct PhysicsServerSharedMemoryInternalData
CommandProcessorCreationInterface* m_commandProcessorCreator;
PhysicsServerSharedMemoryInternalData()
:m_sharedMemory(0),
m_ownsSharedMemory(false),
m_sharedMemoryKey(SHARED_MEMORY_KEY),
m_verboseOutput(false),
m_commandProcessor(0)
: m_sharedMemory(0),
m_ownsSharedMemory(false),
m_sharedMemoryKey(SHARED_MEMORY_KEY),
m_verboseOutput(false),
m_commandProcessor(0)
{
for (int i=0;i<MAX_SHARED_MEMORY_BLOCKS;i++)
{
m_testBlocks[i]=0;
m_areConnected[i]=false;
}
for (int i = 0; i < MAX_SHARED_MEMORY_BLOCKS; i++)
{
m_testBlocks[i] = 0;
m_areConnected[i] = false;
}
}
SharedMemoryStatus& createServerStatus(int statusType, int sequenceNumber, int timeStamp, int blockIndex)
{
SharedMemoryStatus& serverCmd =m_testBlocks[blockIndex]->m_serverCommands[0];
serverCmd .m_type = statusType;
SharedMemoryStatus& serverCmd = m_testBlocks[blockIndex]->m_serverCommands[0];
serverCmd.m_type = statusType;
serverCmd.m_sequenceNumber = sequenceNumber;
serverCmd.m_timeStamp = timeStamp;
return serverCmd;
}
void submitServerStatus(SharedMemoryStatus& status,int blockIndex)
void submitServerStatus(SharedMemoryStatus& status, int blockIndex)
{
m_testBlocks[blockIndex]->m_numServerCommands++;
}
};
PhysicsServerSharedMemory::PhysicsServerSharedMemory(CommandProcessorCreationInterface* commandProcessorCreator, SharedMemoryInterface* sharedMem, int bla)
{
m_data = new PhysicsServerSharedMemoryInternalData();
@@ -74,38 +68,37 @@ PhysicsServerSharedMemory::PhysicsServerSharedMemory(CommandProcessorCreationInt
{
m_data->m_sharedMemory = sharedMem;
m_data->m_ownsSharedMemory = false;
} else
}
else
{
#ifdef _WIN32
m_data->m_sharedMemory = new Win32SharedMemoryServer();
m_data->m_sharedMemory = new Win32SharedMemoryServer();
#else
m_data->m_sharedMemory = new PosixSharedMemory();
m_data->m_sharedMemory = new PosixSharedMemory();
#endif
m_data->m_ownsSharedMemory = true;
m_data->m_ownsSharedMemory = true;
}
m_data->m_commandProcessor = commandProcessorCreator->createCommandProcessor();
}
PhysicsServerSharedMemory::~PhysicsServerSharedMemory()
{
if (m_data->m_sharedMemory)
{
if (m_data->m_verboseOutput)
{
b3Printf("m_sharedMemory\n");
}
if (m_data->m_ownsSharedMemory)
{
delete m_data->m_sharedMemory;
}
m_data->m_sharedMemory = 0;
}
if (m_data->m_sharedMemory)
{
if (m_data->m_verboseOutput)
{
b3Printf("m_sharedMemory\n");
}
if (m_data->m_ownsSharedMemory)
{
delete m_data->m_sharedMemory;
}
m_data->m_sharedMemory = 0;
}
m_data->m_commandProcessorCreator->deleteCommandProcessor(m_data->m_commandProcessor);
delete m_data;
delete m_data;
}
/*void PhysicsServerSharedMemory::resetDynamicsWorld()
@@ -119,73 +112,69 @@ void PhysicsServerSharedMemory::setSharedMemoryKey(int key)
m_data->m_sharedMemoryKey = key;
}
bool PhysicsServerSharedMemory::connectSharedMemory( struct GUIHelperInterface* guiHelper)
bool PhysicsServerSharedMemory::connectSharedMemory(struct GUIHelperInterface* guiHelper)
{
m_data->m_commandProcessor->setGuiHelper(guiHelper);
bool allowCreation = true;
bool allConnected = false;
bool allConnected = false;
int numConnected = 0;
int counter = 0;
for (int block=0;block<MAX_SHARED_MEMORY_BLOCKS;block++)
{
if (m_data->m_areConnected[block])
{
allConnected = true;
numConnected++;
b3Warning("connectSharedMemory, while already connected");
continue;
}
do
{
m_data->m_testBlocks[block] = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(m_data->m_sharedMemoryKey+block, SHARED_MEMORY_SIZE,allowCreation);
if (m_data->m_testBlocks[block])
{
int magicId =m_data->m_testBlocks[block]->m_magicId;
if (m_data->m_verboseOutput)
{
b3Printf("magicId = %d\n", magicId);
}
if (m_data->m_testBlocks[block]->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
{
InitSharedMemoryBlock(m_data->m_testBlocks[block]);
if (m_data->m_verboseOutput)
{
b3Printf("Created and initialized shared memory block\n");
}
m_data->m_areConnected[block] = true;
int counter = 0;
for (int block = 0; block < MAX_SHARED_MEMORY_BLOCKS; block++)
{
if (m_data->m_areConnected[block])
{
allConnected = true;
numConnected++;
b3Warning("connectSharedMemory, while already connected");
continue;
}
do
{
m_data->m_testBlocks[block] = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(m_data->m_sharedMemoryKey + block, SHARED_MEMORY_SIZE, allowCreation);
if (m_data->m_testBlocks[block])
{
int magicId = m_data->m_testBlocks[block]->m_magicId;
if (m_data->m_verboseOutput)
{
b3Printf("magicId = %d\n", magicId);
}
if (m_data->m_testBlocks[block]->m_magicId != SHARED_MEMORY_MAGIC_NUMBER)
{
InitSharedMemoryBlock(m_data->m_testBlocks[block]);
if (m_data->m_verboseOutput)
{
b3Printf("Created and initialized shared memory block\n");
}
m_data->m_areConnected[block] = true;
numConnected++;
} else
{
m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey+block, SHARED_MEMORY_SIZE);
m_data->m_testBlocks[block] = 0;
m_data->m_areConnected[block] = false;
}
} else
{
//b3Error("Cannot connect to shared memory");
m_data->m_areConnected[block] = false;
}
} while (counter++ < 10 && !m_data->m_areConnected[block]);
if (!m_data->m_areConnected[block])
{
b3Error("Server cannot connect to shared memory.\n");
}
}
allConnected = (numConnected==MAX_SHARED_MEMORY_BLOCKS);
}
else
{
m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey + block, SHARED_MEMORY_SIZE);
m_data->m_testBlocks[block] = 0;
m_data->m_areConnected[block] = false;
}
}
else
{
//b3Error("Cannot connect to shared memory");
m_data->m_areConnected[block] = false;
}
} while (counter++ < 10 && !m_data->m_areConnected[block]);
if (!m_data->m_areConnected[block])
{
b3Error("Server cannot connect to shared memory.\n");
}
}
allConnected = (numConnected == MAX_SHARED_MEMORY_BLOCKS);
return allConnected;
}
void PhysicsServerSharedMemory::disconnectSharedMemory(bool deInitializeSharedMemory)
{
//m_data->m_commandProcessor->deleteDynamicsWorld();
@@ -196,40 +185,38 @@ void PhysicsServerSharedMemory::disconnectSharedMemory(bool deInitializeSharedMe
{
b3Printf("releaseSharedMemory1\n");
}
for (int block = 0;block<MAX_SHARED_MEMORY_BLOCKS;block++)
{
if (m_data->m_testBlocks[block])
{
if (m_data->m_verboseOutput)
{
b3Printf("m_testBlock1\n");
}
if (deInitializeSharedMemory)
{
m_data->m_testBlocks[block]->m_magicId = 0;
if (m_data->m_verboseOutput)
{
b3Printf("De-initialized shared memory, magic id = %d\n",m_data->m_testBlocks[block]->m_magicId);
}
}
btAssert(m_data->m_sharedMemory);
m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey+block, SHARED_MEMORY_SIZE);
}
m_data->m_testBlocks[block] = 0;
m_data->m_areConnected[block] = false;
}
for (int block = 0; block < MAX_SHARED_MEMORY_BLOCKS; block++)
{
if (m_data->m_testBlocks[block])
{
if (m_data->m_verboseOutput)
{
b3Printf("m_testBlock1\n");
}
if (deInitializeSharedMemory)
{
m_data->m_testBlocks[block]->m_magicId = 0;
if (m_data->m_verboseOutput)
{
b3Printf("De-initialized shared memory, magic id = %d\n", m_data->m_testBlocks[block]->m_magicId);
}
}
btAssert(m_data->m_sharedMemory);
m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey + block, SHARED_MEMORY_SIZE);
}
m_data->m_testBlocks[block] = 0;
m_data->m_areConnected[block] = false;
}
}
void PhysicsServerSharedMemory::releaseSharedMemory()
{
disconnectSharedMemory(true);
disconnectSharedMemory(true);
}
void PhysicsServerSharedMemory::stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrEvents, int numVREvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents, const struct b3MouseEvent* mouseEvents, int numMouseEvents)
{
m_data->m_commandProcessor->stepSimulationRealTime(dtInSec,vrEvents, numVREvents, keyEvents,numKeyEvents,mouseEvents, numMouseEvents);
m_data->m_commandProcessor->stepSimulationRealTime(dtInSec, vrEvents, numVREvents, keyEvents, numKeyEvents, mouseEvents, numMouseEvents);
}
void PhysicsServerSharedMemory::enableRealTimeSimulation(bool enableRealTimeSim)
@@ -251,64 +238,59 @@ void PhysicsServerSharedMemory::processClientCommands()
{
m_data->m_commandProcessor->processClientCommands();
for (int block = 0;block<MAX_SHARED_MEMORY_BLOCKS;block++)
{
if (m_data->m_areConnected[block] && m_data->m_testBlocks[block])
{
m_data->m_commandProcessor->replayLogCommand(&m_data->m_testBlocks[block]->m_bulletStreamDataServerToClientRefactor[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
///we ignore overflow of integer for now
if (m_data->m_testBlocks[block]->m_numClientCommands> m_data->m_testBlocks[block]->m_numProcessedClientCommands)
{
for (int block = 0; block < MAX_SHARED_MEMORY_BLOCKS; block++)
{
if (m_data->m_areConnected[block] && m_data->m_testBlocks[block])
{
m_data->m_commandProcessor->replayLogCommand(&m_data->m_testBlocks[block]->m_bulletStreamDataServerToClientRefactor[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
///we ignore overflow of integer for now
if (m_data->m_testBlocks[block]->m_numClientCommands > m_data->m_testBlocks[block]->m_numProcessedClientCommands)
{
//BT_PROFILE("processClientCommand");
//until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands
btAssert(m_data->m_testBlocks[block]->m_numClientCommands==m_data->m_testBlocks[block]->m_numProcessedClientCommands+1);
const SharedMemoryCommand& clientCmd =m_data->m_testBlocks[block]->m_clientCommands[0];
//until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands
btAssert(m_data->m_testBlocks[block]->m_numClientCommands == m_data->m_testBlocks[block]->m_numProcessedClientCommands + 1);
m_data->m_testBlocks[block]->m_numProcessedClientCommands++;
//todo, timeStamp
int timeStamp = 0;
SharedMemoryStatus& serverStatusOut = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,clientCmd.m_sequenceNumber,timeStamp,block);
bool hasStatus = m_data->m_commandProcessor->processCommand(clientCmd, serverStatusOut,&m_data->m_testBlocks[block]->m_bulletStreamDataServerToClientRefactor[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
if (hasStatus)
{
m_data->submitServerStatus(serverStatusOut,block);
}
}
}
}
const SharedMemoryCommand& clientCmd = m_data->m_testBlocks[block]->m_clientCommands[0];
m_data->m_testBlocks[block]->m_numProcessedClientCommands++;
//todo, timeStamp
int timeStamp = 0;
SharedMemoryStatus& serverStatusOut = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED, clientCmd.m_sequenceNumber, timeStamp, block);
bool hasStatus = m_data->m_commandProcessor->processCommand(clientCmd, serverStatusOut, &m_data->m_testBlocks[block]->m_bulletStreamDataServerToClientRefactor[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
if (hasStatus)
{
m_data->submitServerStatus(serverStatusOut, block);
}
}
}
}
}
void PhysicsServerSharedMemory::renderScene(int renderFlags)
{
m_data->m_commandProcessor->renderScene(renderFlags);
}
void PhysicsServerSharedMemory::syncPhysicsToGraphics()
void PhysicsServerSharedMemory::syncPhysicsToGraphics()
{
m_data->m_commandProcessor->syncPhysicsToGraphics();
}
void PhysicsServerSharedMemory::physicsDebugDraw(int debugDrawFlags)
void PhysicsServerSharedMemory::physicsDebugDraw(int debugDrawFlags)
{
m_data->m_commandProcessor->physicsDebugDraw(debugDrawFlags);
}
bool PhysicsServerSharedMemory::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
{
return m_data->m_commandProcessor->pickBody(rayFromWorld,rayToWorld);
return m_data->m_commandProcessor->pickBody(rayFromWorld, rayToWorld);
}
bool PhysicsServerSharedMemory::movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
{
return m_data->m_commandProcessor->movePickedBody(rayFromWorld,rayToWorld);
return m_data->m_commandProcessor->movePickedBody(rayFromWorld, rayToWorld);
}
void PhysicsServerSharedMemory::removePickingConstraint()
{
@@ -317,7 +299,7 @@ void PhysicsServerSharedMemory::removePickingConstraint()
void PhysicsServerSharedMemory::enableCommandLogging(bool enable, const char* fileName)
{
m_data->m_commandProcessor->enableCommandLogging(enable,fileName);
m_data->m_commandProcessor->enableCommandLogging(enable, fileName);
}
void PhysicsServerSharedMemory::replayFromLogFile(const char* fileName)
@@ -337,11 +319,8 @@ void PhysicsServerSharedMemory::setVRTeleportPosition(const btVector3& vrTelepor
const btQuaternion& PhysicsServerSharedMemory::getVRTeleportOrientation() const
{
return m_data->m_commandProcessor->getVRTeleportOrientation();
}
void PhysicsServerSharedMemory::setVRTeleportOrientation(const btQuaternion& vrTeleportOrn)
{
m_data->m_commandProcessor->setVRTeleportOrientation(vrTeleportOrn);
}

31
examples/SharedMemory/PhysicsServerSharedMemory.h
View File

@@ -9,25 +9,22 @@ class PhysicsServerSharedMemory : public PhysicsServer
struct PhysicsServerSharedMemoryInternalData* m_data;
protected:
void releaseSharedMemory();
void releaseSharedMemory();
public:
PhysicsServerSharedMemory(struct CommandProcessorCreationInterface* commandProcessorCreator, class SharedMemoryInterface* sharedMem, int bla);
virtual ~PhysicsServerSharedMemory();
virtual void setSharedMemoryKey(int key);
//todo: implement option to allocated shared memory from client
virtual bool connectSharedMemory( struct GUIHelperInterface* guiHelper);
virtual void disconnectSharedMemory (bool deInitializeSharedMemory);
//todo: implement option to allocated shared memory from client
virtual bool connectSharedMemory(struct GUIHelperInterface* guiHelper);
virtual void disconnectSharedMemory(bool deInitializeSharedMemory);
virtual void processClientCommands();
virtual void stepSimulationRealTime(double dtInSec,const struct b3VRControllerEvent* vrEvents, int numVREvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents, const struct b3MouseEvent* mouseEvents, int numMouseEvents);
virtual void stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrEvents, int numVREvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents, const struct b3MouseEvent* mouseEvents, int numMouseEvents);
virtual void enableRealTimeSimulation(bool enableRealTimeSim);
virtual bool isRealTimeSimulationEnabled() const;
@@ -36,7 +33,6 @@ public:
//bool supportsJointMotor(class btMultiBody* body, int linkIndex);
///The pickBody method will try to pick the first body along a ray, return true if succeeds, false otherwise
virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld);
virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld);
@@ -48,22 +44,15 @@ public:
virtual const btQuaternion& getVRTeleportOrientation() const;
virtual void setVRTeleportOrientation(const btQuaternion& vrTeleportOrn);
//for physicsDebugDraw and renderScene are mainly for debugging purposes
//and for physics visualization. The idea is that physicsDebugDraw can also send wireframe
//to a physics client, over shared memory
void physicsDebugDraw(int debugDrawFlags);
void renderScene(int renderFlags);
void syncPhysicsToGraphics();
void physicsDebugDraw(int debugDrawFlags);
void renderScene(int renderFlags);
void syncPhysicsToGraphics();
void enableCommandLogging(bool enable, const char* fileName);
void replayFromLogFile(const char* fileName);
};
#endif //PHYSICS_SERVER_EXAMPLESHARED_MEMORY_H
#endif //PHYSICS_SERVER_EXAMPLESHARED_MEMORY_H

201
examples/SharedMemory/PosixSharedMemory.cpp
View File

@@ -1,12 +1,12 @@
#include "PosixSharedMemory.h"
#include "Bullet3Common/b3Logging.h"
#include "LinearMath/btScalar.h" //for btAssert
#include "LinearMath/btScalar.h" //for btAssert
#include "LinearMath/btAlignedObjectArray.h"
//Windows implementation is in Win32SharedMemory.cpp
#ifndef _WIN32
#define TEST_SHARED_MEMORY
#endif//_WIN32
#endif //_WIN32
#include <stddef.h>
@@ -19,25 +19,23 @@
struct btSharedMemorySegment
{
int m_key;
int m_sharedMemoryId;
void* m_sharedMemoryPtr;
bool m_createdSharedMemory;
btSharedMemorySegment()
: m_sharedMemoryId(-1),
m_sharedMemoryPtr(0),
m_createdSharedMemory(true)
{
}
int m_key;
int m_sharedMemoryId;
void* m_sharedMemoryPtr;
bool m_createdSharedMemory;
btSharedMemorySegment()
: m_sharedMemoryId(-1),
m_sharedMemoryPtr(0),
m_createdSharedMemory(true)
{
}
};
struct PosixSharedMemoryInteralData
{
btAlignedObjectArray<btSharedMemorySegment> m_segments;
btAlignedObjectArray<btSharedMemorySegment> m_segments;
PosixSharedMemoryInteralData()
{
}
@@ -45,111 +43,112 @@ struct PosixSharedMemoryInteralData
PosixSharedMemory::PosixSharedMemory()
{
m_internalData = new PosixSharedMemoryInteralData;
m_internalData = new PosixSharedMemoryInteralData;
}
PosixSharedMemory::~PosixSharedMemory()
{
delete m_internalData;
delete m_internalData;
}
struct btPointerCaster
{
union
{
void* ptr;
ptrdiff_t integer;
};
union {
void* ptr;
ptrdiff_t integer;
};
};
void* PosixSharedMemory::allocateSharedMemory(int key, int size, bool allowCreation)
void* PosixSharedMemory::allocateSharedMemory(int key, int size, bool allowCreation)
{
#ifdef TEST_SHARED_MEMORY
{
btSharedMemorySegment* seg = 0;
int i=0;
for (i=0;i<m_internalData->m_segments.size();i++)
{
if (m_internalData->m_segments[i].m_key == key)
{
seg = &m_internalData->m_segments[i];
break;
}
}
if (seg)
{
b3Error("already created shared memory segment using same key");
return seg->m_sharedMemoryPtr;
}
}
int flags = (allowCreation ? IPC_CREAT : 0) | 0666;
int id = shmget((key_t) key, (size_t) size,flags);
if (id < 0)
{
//b3Warning("shmget error1");
} else
{
btPointerCaster result;
result.ptr = shmat(id,0,0);
if (result.integer == -1)
{
b3Error("shmat returned -1");
} else
{
btSharedMemorySegment seg;
seg.m_key = key;
seg.m_createdSharedMemory = allowCreation;
seg.m_sharedMemoryId = id;
seg.m_sharedMemoryPtr = result.ptr;
m_internalData->m_segments.push_back(seg);
return result.ptr;
}
}
{
btSharedMemorySegment* seg = 0;
int i = 0;
for (i = 0; i < m_internalData->m_segments.size(); i++)
{
if (m_internalData->m_segments[i].m_key == key)
{
seg = &m_internalData->m_segments[i];
break;
}
}
if (seg)
{
b3Error("already created shared memory segment using same key");
return seg->m_sharedMemoryPtr;
}
}
int flags = (allowCreation ? IPC_CREAT : 0) | 0666;
int id = shmget((key_t)key, (size_t)size, flags);
if (id < 0)
{
//b3Warning("shmget error1");
}
else
{
btPointerCaster result;
result.ptr = shmat(id, 0, 0);
if (result.integer == -1)
{
b3Error("shmat returned -1");
}
else
{
btSharedMemorySegment seg;
seg.m_key = key;
seg.m_createdSharedMemory = allowCreation;
seg.m_sharedMemoryId = id;
seg.m_sharedMemoryPtr = result.ptr;
m_internalData->m_segments.push_back(seg);
return result.ptr;
}
}
#else
//not implemented yet
btAssert(0);
//not implemented yet
btAssert(0);
#endif
return 0;
return 0;
}
void PosixSharedMemory::releaseSharedMemory(int key, int size)
{
#ifdef TEST_SHARED_MEMORY
btSharedMemorySegment* seg = 0;
int i=0;
for (i=0;i<m_internalData->m_segments.size();i++)
{
if (m_internalData->m_segments[i].m_key == key)
{
seg = &m_internalData->m_segments[i];
break;
}
}
if (0==seg)
{
b3Error("PosixSharedMemory::releaseSharedMemory: shared memory key not found");
return;
}
if (seg->m_sharedMemoryId < 0)
{
b3Error("PosixSharedMemory::releaseSharedMemory: shared memory id is not set");
} else
{
btSharedMemorySegment* seg = 0;
int i = 0;
for (i = 0; i < m_internalData->m_segments.size(); i++)
{
if (m_internalData->m_segments[i].m_key == key)
{
seg = &m_internalData->m_segments[i];
break;
}
}
if (0 == seg)
{
b3Error("PosixSharedMemory::releaseSharedMemory: shared memory key not found");
return;
}
if (seg->m_sharedMemoryId < 0)
{
b3Error("PosixSharedMemory::releaseSharedMemory: shared memory id is not set");
}
else
{
if (seg->m_createdSharedMemory)
{
int result = shmctl(seg->m_sharedMemoryId,IPC_RMID,0);
int result = shmctl(seg->m_sharedMemoryId, IPC_RMID, 0);
if (result == -1)
{
b3Error("PosixSharedMemory::releaseSharedMemory: shmat returned -1");
} else
}
else
{
b3Printf("PosixSharedMemory::releaseSharedMemory removed shared memory");
}
@@ -159,12 +158,12 @@ void PosixSharedMemory::releaseSharedMemory(int key, int size)
if (seg->m_sharedMemoryPtr)
{
shmdt(seg->m_sharedMemoryPtr);
seg->m_sharedMemoryPtr = 0;
seg->m_sharedMemoryPtr = 0;
b3Printf("PosixSharedMemory::releaseSharedMemory detached shared memory\n");
}
}
}
m_internalData->m_segments.removeAtIndex(i);
m_internalData->m_segments.removeAtIndex(i);
#endif
}

17
examples/SharedMemory/PosixSharedMemory.h
View File

@@ -3,19 +3,16 @@
#include "SharedMemoryInterface.h"
class PosixSharedMemory : public SharedMemoryInterface
{
struct PosixSharedMemoryInteralData* m_internalData;
public:
PosixSharedMemory();
virtual ~PosixSharedMemory();
virtual void* allocateSharedMemory(int key, int size, bool allowCreation);
virtual void releaseSharedMemory(int key, int size);
public:
PosixSharedMemory();
virtual ~PosixSharedMemory();
virtual void* allocateSharedMemory(int key, int size, bool allowCreation);
virtual void releaseSharedMemory(int key, int size);
};
#endif //
#endif //

3
examples/SharedMemory/RobotControlExample.cpp
View File

@@ -1,6 +1,5 @@
#include "RobotControlExample.h"
#if 0
@@ -665,5 +664,3 @@ class CommonExampleInterface* RobotControlExampleCreateFunc(struct CommonExam
return example;
}
#endif

8
examples/SharedMemory/RobotControlExample.h
View File

@@ -3,13 +3,11 @@
enum EnumRobotControls
{
ROBOT_VELOCITY_CONTROL=0,
ROBOT_VELOCITY_CONTROL = 0,
ROBOT_PD_CONTROL,
ROBOT_PING_PONG_JOINT_FEEDBACK,
};
class CommonExampleInterface* RobotControlExampleCreateFunc(struct CommonExampleOptions& options);
#endif //ROBOT_CONTROL_EXAMPLE_H
class CommonExampleInterface* RobotControlExampleCreateFunc(struct CommonExampleOptions& options);
#endif //ROBOT_CONTROL_EXAMPLE_H

27
examples/SharedMemory/SharedMemoryBlock.h
View File

@@ -3,7 +3,6 @@
#define SHARED_MEMORY_MAX_COMMANDS 4
#include "SharedMemoryCommands.h"
struct SharedMemoryBlock
@@ -20,35 +19,29 @@ struct SharedMemoryBlock
//m_bulletStreamDataClientToServer is a way for the client to create collision shapes, rigid bodies and constraints
//the Bullet data structures are more general purpose than the capabilities of a URDF file.
char m_bulletStreamDataClientToServer[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];
char m_bulletStreamDataClientToServer[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];
//m_bulletStreamDataServerToClient is used to send (debug) data from server to client, for
//example to provide all details of a multibody including joint/link names, after loading a URDF file.
char m_bulletStreamDataServerToClientRefactor[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];
char m_bulletStreamDataServerToClientRefactor[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];
};
//http://stackoverflow.com/questions/24736304/unable-to-use-inline-in-declaration-get-error-c2054
#ifdef _WIN32
__inline
#else
inline
#endif
void InitSharedMemoryBlock(struct SharedMemoryBlock* sharedMemoryBlock)
void
InitSharedMemoryBlock(struct SharedMemoryBlock* sharedMemoryBlock)
{
sharedMemoryBlock->m_numClientCommands = 0;
sharedMemoryBlock->m_numServerCommands = 0;
sharedMemoryBlock->m_numProcessedClientCommands=0;
sharedMemoryBlock->m_numProcessedServerCommands=0;
sharedMemoryBlock->m_magicId = SHARED_MEMORY_MAGIC_NUMBER;
sharedMemoryBlock->m_numClientCommands = 0;
sharedMemoryBlock->m_numServerCommands = 0;
sharedMemoryBlock->m_numProcessedClientCommands = 0;
sharedMemoryBlock->m_numProcessedServerCommands = 0;
sharedMemoryBlock->m_magicId = SHARED_MEMORY_MAGIC_NUMBER;
}
#define SHARED_MEMORY_SIZE sizeof(SharedMemoryBlock)
#endif //SHARED_MEMORY_BLOCK_H
#endif //SHARED_MEMORY_BLOCK_H

91
examples/SharedMemory/SharedMemoryCommandProcessor.cpp
View File

@@ -7,29 +7,26 @@
#include "SharedMemoryBlock.h"
struct SharedMemoryCommandProcessorInternalData
{
int m_sharedMemoryKey;
bool m_isConnected;
SharedMemoryInterface* m_sharedMemory;
bool m_ownsSharedMemory;
bool m_ownsSharedMemory;
bool m_verboseOutput;
bool m_waitingForServer;
SharedMemoryStatus m_lastServerStatus;
SharedMemoryBlock* m_testBlock1;
SharedMemoryCommandProcessorInternalData()
:m_sharedMemoryKey(SHARED_MEMORY_KEY),
m_isConnected(false),
m_sharedMemory(0),
m_ownsSharedMemory(false),
m_verboseOutput(false),
m_waitingForServer(false),
m_testBlock1(0)
: m_sharedMemoryKey(SHARED_MEMORY_KEY),
m_isConnected(false),
m_sharedMemory(0),
m_ownsSharedMemory(false),
m_verboseOutput(false),
m_waitingForServer(false),
m_testBlock1(0)
{
}
};
@@ -43,13 +40,11 @@ SharedMemoryCommandProcessor::SharedMemoryCommandProcessor()
m_data->m_sharedMemory = new PosixSharedMemory();
#endif
m_data->m_ownsSharedMemory = true;
}
SharedMemoryCommandProcessor::~SharedMemoryCommandProcessor()
{
if (m_data->m_isConnected)
if (m_data->m_isConnected)
{
disconnect();
}
@@ -58,12 +53,10 @@ SharedMemoryCommandProcessor::~SharedMemoryCommandProcessor()
delete m_data->m_sharedMemory;
}
delete m_data;
}
bool SharedMemoryCommandProcessor::connect()
{
if (m_data->m_isConnected)
return true;
@@ -71,45 +64,48 @@ bool SharedMemoryCommandProcessor::connect()
m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(
m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE, allowCreation);
if (m_data->m_testBlock1) {
if (m_data->m_testBlock1->m_magicId != SHARED_MEMORY_MAGIC_NUMBER) {
if ((m_data->m_testBlock1->m_magicId < 211705023) &&
(m_data->m_testBlock1->m_magicId >=201705023))
if (m_data->m_testBlock1)
{
if (m_data->m_testBlock1->m_magicId != SHARED_MEMORY_MAGIC_NUMBER)
{
if ((m_data->m_testBlock1->m_magicId < 211705023) &&
(m_data->m_testBlock1->m_magicId >= 201705023))
{
b3Error("Error: physics server version mismatch (expected %d got %d)\n",SHARED_MEMORY_MAGIC_NUMBER, m_data->m_testBlock1->m_magicId);
} else
b3Error("Error: physics server version mismatch (expected %d got %d)\n", SHARED_MEMORY_MAGIC_NUMBER, m_data->m_testBlock1->m_magicId);
}
else
{
b3Error("Error connecting to shared memory: please start server before client\n");
}
m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey,
SHARED_MEMORY_SIZE);
SHARED_MEMORY_SIZE);
m_data->m_testBlock1 = 0;
return false;
}
else {
if (m_data->m_verboseOutput) {
else
{
if (m_data->m_verboseOutput)
{
b3Printf("Connected to existing shared memory, status OK.\n");
}
m_data->m_isConnected = true;
}
}
else {
else
{
b3Error("Cannot connect to shared memory");
return false;
}
return true;
}
void SharedMemoryCommandProcessor::disconnect()
{
if (m_data->m_isConnected && m_data->m_sharedMemory)
if (m_data->m_isConnected && m_data->m_sharedMemory)
{
m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE);
}
m_data->m_isConnected = false;
}
bool SharedMemoryCommandProcessor::isConnected() const
@@ -117,11 +113,12 @@ bool SharedMemoryCommandProcessor::isConnected() const
return m_data->m_isConnected;
}
bool SharedMemoryCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
if (!m_data->m_waitingForServer) {
if (&m_data->m_testBlock1->m_clientCommands[0] != &clientCmd) {
if (!m_data->m_waitingForServer)
{
if (&m_data->m_testBlock1->m_clientCommands[0] != &clientCmd)
{
m_data->m_testBlock1->m_clientCommands[0] = clientCmd;
}
m_data->m_testBlock1->m_numClientCommands++;
@@ -133,11 +130,10 @@ bool SharedMemoryCommandProcessor::processCommand(const struct SharedMemoryComma
bool SharedMemoryCommandProcessor::receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
m_data->m_lastServerStatus.m_dataStream = 0;
m_data->m_lastServerStatus.m_numDataStreamBytes = 0;
if (!m_data->m_testBlock1)
if (!m_data->m_testBlock1)
{
//m_data->m_lastServerStatus.m_type = CMD_SHARED_MEMORY_NOT_INITIALIZED;
//return &m_data->m_lastServerStatus;
@@ -145,7 +141,8 @@ bool SharedMemoryCommandProcessor::receiveStatus(struct SharedMemoryStatus& serv
return false;
}
if (!m_data->m_waitingForServer) {
if (!m_data->m_waitingForServer)
{
return false;
}
@@ -157,10 +154,10 @@ bool SharedMemoryCommandProcessor::receiveStatus(struct SharedMemoryStatus& serv
}
if (m_data->m_testBlock1->m_numServerCommands >
m_data->m_testBlock1->m_numProcessedServerCommands)
m_data->m_testBlock1->m_numProcessedServerCommands)
{
b3Assert(m_data->m_testBlock1->m_numServerCommands ==
m_data->m_testBlock1->m_numProcessedServerCommands + 1);
m_data->m_testBlock1->m_numProcessedServerCommands + 1);
const SharedMemoryStatus& serverCmd = m_data->m_testBlock1->m_serverCommands[0];
m_data->m_lastServerStatus = serverCmd;
@@ -174,13 +171,15 @@ bool SharedMemoryCommandProcessor::receiveStatus(struct SharedMemoryStatus& serv
m_data->m_testBlock1->m_numProcessedServerCommands++;
// we don't have more than 1 command outstanding (in total, either server or client)
b3Assert(m_data->m_testBlock1->m_numProcessedServerCommands ==
m_data->m_testBlock1->m_numServerCommands);
m_data->m_testBlock1->m_numServerCommands);
if (m_data->m_testBlock1->m_numServerCommands ==
m_data->m_testBlock1->m_numProcessedServerCommands) {
m_data->m_testBlock1->m_numProcessedServerCommands)
{
m_data->m_waitingForServer = false;
}
else {
else
{
m_data->m_waitingForServer = true;
}
@@ -195,7 +194,7 @@ void SharedMemoryCommandProcessor::renderScene(int renderFlags)
{
}
void SharedMemoryCommandProcessor::physicsDebugDraw(int debugDrawFlags)
void SharedMemoryCommandProcessor::physicsDebugDraw(int debugDrawFlags)
{
}
@@ -211,13 +210,11 @@ void SharedMemoryCommandProcessor::setSharedMemoryInterface(class SharedMemoryIn
}
m_data->m_ownsSharedMemory = false;
m_data->m_sharedMemory = sharedMem;
}
void SharedMemoryCommandProcessor::setSharedMemoryKey(int key)
{
m_data->m_sharedMemoryKey = key;
void SharedMemoryCommandProcessor::setSharedMemoryKey(int key)
{
m_data->m_sharedMemoryKey = key;
}
void SharedMemoryCommandProcessor::setTimeOut(double /*timeOutInSeconds*/)

6
examples/SharedMemory/SharedMemoryCommandProcessor.h
View File

@@ -5,7 +5,6 @@
class SharedMemoryCommandProcessor : public PhysicsCommandProcessorInterface
{
struct SharedMemoryCommandProcessorInternalData* m_data;
public:
@@ -24,7 +23,7 @@ public:
virtual bool receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
virtual void renderScene(int renderFlags);
virtual void physicsDebugDraw(int debugDrawFlags);
virtual void physicsDebugDraw(int debugDrawFlags);
virtual void setGuiHelper(struct GUIHelperInterface* guiHelper);
void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem);
@@ -34,5 +33,4 @@ public:
virtual void reportNotifications() {}
};
#endif //SHARED_MEMORY_COMMAND_PROCESSOR_H
#endif //SHARED_MEMORY_COMMAND_PROCESSOR_H

700
examples/SharedMemory/SharedMemoryCommands.h
View File

File diff suppressed because it is too large Load Diff

14
examples/SharedMemory/SharedMemoryCommon.h
View File

@@ -6,18 +6,16 @@
class SharedMemoryCommon : public CommonExampleInterface
{
protected:
struct GUIHelperInterface* m_guiHelper;
public:
SharedMemoryCommon(GUIHelperInterface* helper)
:m_guiHelper(helper)
: m_guiHelper(helper)
{
}
virtual void setSharedMemoryKey(int key)=0;
virtual bool wantsTermination()=0;
virtual bool isConnected()=0;
virtual void setSharedMemoryKey(int key) = 0;
virtual bool wantsTermination() = 0;
virtual bool isConnected() = 0;
};
#endif//
#endif //

267
examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
View File

@@ -3,7 +3,7 @@
#include "../Utils/b3Clock.h"
#include "PhysicsClientSharedMemory.h"
#include"../ExampleBrowser/InProcessExampleBrowser.h"
#include "../ExampleBrowser/InProcessExampleBrowser.h"
#include <stdio.h>
#include <string.h>
#include "PhysicsServerExampleBullet2.h"
@@ -14,52 +14,50 @@
#include "Bullet3Common/b3Logging.h"
class InProcessPhysicsClientSharedMemoryMainThread : public PhysicsClientSharedMemory
{
btInProcessExampleBrowserMainThreadInternalData* m_data;
b3Clock m_clock;
btInProcessExampleBrowserMainThreadInternalData* m_data;
b3Clock m_clock;
public:
InProcessPhysicsClientSharedMemoryMainThread(int argc, char* argv[], bool useInProcessMemory)
{
int newargc = argc+3;
char** newargv = (char**)malloc(sizeof(void*)*newargc);
InProcessPhysicsClientSharedMemoryMainThread(int argc, char* argv[], bool useInProcessMemory)
{
int newargc = argc + 3;
char** newargv = (char**)malloc(sizeof(void*) * newargc);
char* t0 = (char*)"--unused";
newargv[0] = t0;
for (int i=0;i<argc;i++)
newargv[i+1] = argv[i];
newargv[argc+1]=(char*)"--logtostderr";
newargv[argc+2]=(char*)"--start_demo_name=Physics Server";
m_data = btCreateInProcessExampleBrowserMainThread(newargc,newargv, useInProcessMemory);
SharedMemoryInterface* shMem = btGetSharedMemoryInterfaceMainThread(m_data);
setSharedMemoryInterface(shMem);
}
virtual ~InProcessPhysicsClientSharedMemoryMainThread()
{
setSharedMemoryInterface(0);
btShutDownExampleBrowserMainThread(m_data);
}
// return non-null if there is a status, nullptr otherwise
virtual const struct SharedMemoryStatus* processServerStatus()
{
newargv[0] = t0;
for (int i = 0; i < argc; i++)
newargv[i + 1] = argv[i];
newargv[argc + 1] = (char*)"--logtostderr";
newargv[argc + 2] = (char*)"--start_demo_name=Physics Server";
m_data = btCreateInProcessExampleBrowserMainThread(newargc, newargv, useInProcessMemory);
SharedMemoryInterface* shMem = btGetSharedMemoryInterfaceMainThread(m_data);
setSharedMemoryInterface(shMem);
}
virtual ~InProcessPhysicsClientSharedMemoryMainThread()
{
setSharedMemoryInterface(0);
btShutDownExampleBrowserMainThread(m_data);
}
// return non-null if there is a status, nullptr otherwise
virtual const struct SharedMemoryStatus* processServerStatus()
{
{
if (btIsExampleBrowserMainThreadTerminated(m_data))
{
PhysicsClientSharedMemory::disconnectSharedMemory();
}
}
{
unsigned long int ms = m_clock.getTimeMilliseconds();
if (ms>2)
{
{
unsigned long int ms = m_clock.getTimeMilliseconds();
if (ms > 2)
{
B3_PROFILE("m_clock.reset()");
btUpdateInProcessExampleBrowserMainThread(m_data);
m_clock.reset();
m_clock.reset();
}
}
{
@@ -72,55 +70,50 @@ public:
}
return stat;
}
virtual bool submitClientCommand(const struct SharedMemoryCommand& command)
{
// btUpdateInProcessExampleBrowserMainThread(m_data);
return PhysicsClientSharedMemory::submitClientCommand(command);
}
}
virtual bool submitClientCommand(const struct SharedMemoryCommand& command)
{
// btUpdateInProcessExampleBrowserMainThread(m_data);
return PhysicsClientSharedMemory::submitClientCommand(command);
}
};
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[])
{
InProcessPhysicsClientSharedMemoryMainThread* cl = new InProcessPhysicsClientSharedMemoryMainThread(argc, argv, 1);
cl->setSharedMemoryKey(SHARED_MEMORY_KEY+1);
cl->connect();
return (b3PhysicsClientHandle ) cl;
InProcessPhysicsClientSharedMemoryMainThread* cl = new InProcessPhysicsClientSharedMemoryMainThread(argc, argv, 1);
cl->setSharedMemoryKey(SHARED_MEMORY_KEY + 1);
cl->connect();
return (b3PhysicsClientHandle)cl;
}
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThreadSharedMemory(int argc, char* argv[])
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThreadSharedMemory(int argc, char* argv[])
{
InProcessPhysicsClientSharedMemoryMainThread* cl = new InProcessPhysicsClientSharedMemoryMainThread(argc, argv, 0);
cl->setSharedMemoryKey(SHARED_MEMORY_KEY+1);
cl->connect();
return (b3PhysicsClientHandle ) cl;
InProcessPhysicsClientSharedMemoryMainThread* cl = new InProcessPhysicsClientSharedMemoryMainThread(argc, argv, 0);
cl->setSharedMemoryKey(SHARED_MEMORY_KEY + 1);
cl->connect();
return (b3PhysicsClientHandle)cl;
}
class InProcessPhysicsClientSharedMemory : public PhysicsClientSharedMemory
{
btInProcessExampleBrowserInternalData* m_data;
char** m_newargv;
public:
InProcessPhysicsClientSharedMemory(int argc, char* argv[], bool useInProcessMemory)
{
int newargc = argc+2;
m_newargv = (char**)malloc(sizeof(void*)*newargc);
int newargc = argc + 2;
m_newargv = (char**)malloc(sizeof(void*) * newargc);
char* t0 = (char*)"--unused";
m_newargv[0] = t0;
for (int i=0;i<argc;i++)
m_newargv[i+1] = argv[i];
for (int i = 0; i < argc; i++)
m_newargv[i + 1] = argv[i];
char* t1 = (char*)"--start_demo_name=Physics Server";
m_newargv[argc+1] = t1;
m_data = btCreateInProcessExampleBrowser(newargc,m_newargv, useInProcessMemory);
m_newargv[argc + 1] = t1;
m_data = btCreateInProcessExampleBrowser(newargc, m_newargv, useInProcessMemory);
SharedMemoryInterface* shMem = btGetSharedMemoryInterface(m_data);
setSharedMemoryInterface(shMem);
}
@@ -131,52 +124,49 @@ public:
btShutDownExampleBrowser(m_data);
free(m_newargv);
}
};
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* argv[])
{
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* argv[])
{
InProcessPhysicsClientSharedMemory* cl = new InProcessPhysicsClientSharedMemory(argc, argv, 1);
cl->setSharedMemoryKey(SHARED_MEMORY_KEY+1);
cl->connect();
return (b3PhysicsClientHandle ) cl;
cl->setSharedMemoryKey(SHARED_MEMORY_KEY + 1);
cl->connect();
return (b3PhysicsClientHandle)cl;
}
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectSharedMemory(int argc, char* argv[])
{
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectSharedMemory(int argc, char* argv[])
{
InProcessPhysicsClientSharedMemory* cl = new InProcessPhysicsClientSharedMemory(argc, argv, 0);
cl->setSharedMemoryKey(SHARED_MEMORY_KEY+1);
cl->connect();
return (b3PhysicsClientHandle ) cl;
cl->setSharedMemoryKey(SHARED_MEMORY_KEY + 1);
cl->connect();
return (b3PhysicsClientHandle)cl;
}
class InProcessPhysicsClientExistingExampleBrowser : public PhysicsClientSharedMemory
{
CommonExampleInterface* m_physicsServerExample;
CommonExampleInterface* m_physicsServerExample;
SharedMemoryInterface* m_sharedMem;
b3Clock m_clock;
unsigned long long int m_prevTime;
public:
InProcessPhysicsClientExistingExampleBrowser (struct GUIHelperInterface* guiHelper, bool useInProcessMemory, bool skipGraphicsUpdate)
InProcessPhysicsClientExistingExampleBrowser(struct GUIHelperInterface* guiHelper, bool useInProcessMemory, bool skipGraphicsUpdate)
{
m_sharedMem=0;
m_sharedMem = 0;
CommonExampleOptions options(guiHelper);
if (useInProcessMemory)
{
m_sharedMem = new InProcessMemory;
options.m_sharedMem = m_sharedMem;
}
options.m_skipGraphicsUpdate = skipGraphicsUpdate;
m_physicsServerExample = PhysicsServerCreateFuncBullet2(options);
m_physicsServerExample ->initPhysics();
m_physicsServerExample ->resetCamera();
if (useInProcessMemory)
{
m_sharedMem = new InProcessMemory;
options.m_sharedMem = m_sharedMem;
}
options.m_skipGraphicsUpdate = skipGraphicsUpdate;
m_physicsServerExample = PhysicsServerCreateFuncBullet2(options);
m_physicsServerExample->initPhysics();
m_physicsServerExample->resetCamera();
setSharedMemoryInterface(m_sharedMem);
m_clock.reset();
m_prevTime = m_clock.getTimeMicroseconds();
}
virtual ~InProcessPhysicsClientExistingExampleBrowser()
{
@@ -186,16 +176,16 @@ public:
delete m_sharedMem;
}
// return non-null if there is a status, nullptr otherwise
virtual const struct SharedMemoryStatus* processServerStatus()
{
// return non-null if there is a status, nullptr otherwise
virtual const struct SharedMemoryStatus* processServerStatus()
{
m_physicsServerExample->updateGraphics();
unsigned long long int curTime = m_clock.getTimeMicroseconds();
unsigned long long int dtMicro = curTime - m_prevTime;
m_prevTime = curTime;
double dt = double(dtMicro)/1000000.;
double dt = double(dtMicro) / 1000000.;
m_physicsServerExample->stepSimulation(dt);
{
@@ -208,8 +198,7 @@ public:
}
return stat;
}
}
virtual void renderScene()
{
@@ -221,81 +210,79 @@ public:
}
virtual bool mouseMoveCallback(float x, float y)
{
return m_physicsServerExample->mouseMoveCallback(x,y);
return m_physicsServerExample->mouseMoveCallback(x, y);
}
virtual bool mouseButtonCallback(int button, int state, float x, float y)
{
return m_physicsServerExample->mouseButtonCallback(button,state,x,y);
return m_physicsServerExample->mouseButtonCallback(button, state, x, y);
}
};
void b3InProcessDebugDrawInternal(b3PhysicsClientHandle clientHandle, int debugDrawMode)
{
InProcessPhysicsClientExistingExampleBrowser* cl = (InProcessPhysicsClientExistingExampleBrowser*) clientHandle;
InProcessPhysicsClientExistingExampleBrowser* cl = (InProcessPhysicsClientExistingExampleBrowser*)clientHandle;
cl->debugDraw(debugDrawMode);
}
void b3InProcessRenderSceneInternal(b3PhysicsClientHandle clientHandle)
{
InProcessPhysicsClientExistingExampleBrowser* cl = (InProcessPhysicsClientExistingExampleBrowser*) clientHandle;
InProcessPhysicsClientExistingExampleBrowser* cl = (InProcessPhysicsClientExistingExampleBrowser*)clientHandle;
cl->renderScene();
}
int b3InProcessMouseMoveCallback(b3PhysicsClientHandle clientHandle,float x,float y)
int b3InProcessMouseMoveCallback(b3PhysicsClientHandle clientHandle, float x, float y)
{
InProcessPhysicsClientExistingExampleBrowser* cl = (InProcessPhysicsClientExistingExampleBrowser*) clientHandle;
return cl->mouseMoveCallback(x,y);
InProcessPhysicsClientExistingExampleBrowser* cl = (InProcessPhysicsClientExistingExampleBrowser*)clientHandle;
return cl->mouseMoveCallback(x, y);
}
int b3InProcessMouseButtonCallback(b3PhysicsClientHandle clientHandle, int button, int state, float x, float y)
{
InProcessPhysicsClientExistingExampleBrowser* cl = (InProcessPhysicsClientExistingExampleBrowser*) clientHandle;
return cl->mouseButtonCallback(button,state, x,y);
InProcessPhysicsClientExistingExampleBrowser* cl = (InProcessPhysicsClientExistingExampleBrowser*)clientHandle;
return cl->mouseButtonCallback(button, state, x, y);
}
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(void* guiHelperPtr)
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(void* guiHelperPtr)
{
static DummyGUIHelper noGfx;
GUIHelperInterface* guiHelper = (GUIHelperInterface*) guiHelperPtr;
if (!guiHelper)
{
guiHelper = &noGfx;
}
bool useInprocessMemory = true;
bool skipGraphicsUpdate = false;
InProcessPhysicsClientExistingExampleBrowser* cl = new InProcessPhysicsClientExistingExampleBrowser(guiHelper,useInprocessMemory,skipGraphicsUpdate);
cl->connect();
return (b3PhysicsClientHandle ) cl;
static DummyGUIHelper noGfx;
GUIHelperInterface* guiHelper = (GUIHelperInterface*)guiHelperPtr;
if (!guiHelper)
{
guiHelper = &noGfx;
}
bool useInprocessMemory = true;
bool skipGraphicsUpdate = false;
InProcessPhysicsClientExistingExampleBrowser* cl = new InProcessPhysicsClientExistingExampleBrowser(guiHelper, useInprocessMemory, skipGraphicsUpdate);
cl->connect();
return (b3PhysicsClientHandle)cl;
}
extern int gSharedMemoryKey;
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect3(void* guiHelperPtr, int sharedMemoryKey)
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect3(void* guiHelperPtr, int sharedMemoryKey)
{
static DummyGUIHelper noGfx;
gSharedMemoryKey = sharedMemoryKey;
GUIHelperInterface* guiHelper = (GUIHelperInterface*) guiHelperPtr;
if (!guiHelper)
{
guiHelper = &noGfx;
}
bool useInprocessMemory = false;
bool skipGraphicsUpdate = true;
InProcessPhysicsClientExistingExampleBrowser* cl = new InProcessPhysicsClientExistingExampleBrowser(guiHelper, useInprocessMemory, skipGraphicsUpdate);
static DummyGUIHelper noGfx;
cl->setSharedMemoryKey(sharedMemoryKey+1);
cl->connect();
gSharedMemoryKey = sharedMemoryKey;
GUIHelperInterface* guiHelper = (GUIHelperInterface*)guiHelperPtr;
if (!guiHelper)
{
guiHelper = &noGfx;
}
bool useInprocessMemory = false;
bool skipGraphicsUpdate = true;
InProcessPhysicsClientExistingExampleBrowser* cl = new InProcessPhysicsClientExistingExampleBrowser(guiHelper, useInprocessMemory, skipGraphicsUpdate);
cl->setSharedMemoryKey(sharedMemoryKey + 1);
cl->connect();
//backward compatiblity
gSharedMemoryKey = SHARED_MEMORY_KEY;
return (b3PhysicsClientHandle ) cl;
return (b3PhysicsClientHandle)cl;
}
//backward compatiblity
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect2(void* guiHelperPtr)
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect2(void* guiHelperPtr)
{
return b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect3(guiHelperPtr, SHARED_MEMORY_KEY);
}

36
examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h
View File

@@ -5,31 +5,31 @@
#include "PhysicsClientC_API.h"
#ifdef __cplusplus
extern "C" {
extern "C"
{
#endif
///think more about naming. The b3ConnectPhysicsLoopback
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* argv[]);
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectSharedMemory(int argc, char* argv[]);
///think more about naming. The b3ConnectPhysicsLoopback
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* argv[]);
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectSharedMemory(int argc, char* argv[]);
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[]);
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThreadSharedMemory(int argc, char* argv[]);
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[]);
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThreadSharedMemory(int argc, char* argv[]);
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(void* guiHelperPtr);
//create a shared memory physics server, with a DummyGUIHelper (no graphics)
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect2(void* guiHelperPtr);
//create a shared memory physics server, with a DummyGUIHelper (no graphics) and allow to set shared memory key
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect3(void* guiHelperPtr, int sharedMemoryKey);
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(void* guiHelperPtr);
//create a shared memory physics server, with a DummyGUIHelper (no graphics)
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect2(void* guiHelperPtr);
//create a shared memory physics server, with a DummyGUIHelper (no graphics) and allow to set shared memory key
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect3(void* guiHelperPtr, int sharedMemoryKey);
///ignore the following APIs, they are for internal use for example browser
void b3InProcessRenderSceneInternal(b3PhysicsClientHandle clientHandle);
void b3InProcessDebugDrawInternal(b3PhysicsClientHandle clientHandle, int debugDrawMode);
int b3InProcessMouseMoveCallback(b3PhysicsClientHandle clientHandle,float x,float y);
int b3InProcessMouseButtonCallback(b3PhysicsClientHandle clientHandle, int button, int state, float x, float y);
///ignore the following APIs, they are for internal use for example browser
void b3InProcessRenderSceneInternal(b3PhysicsClientHandle clientHandle);
void b3InProcessDebugDrawInternal(b3PhysicsClientHandle clientHandle, int debugDrawMode);
int b3InProcessMouseMoveCallback(b3PhysicsClientHandle clientHandle, float x, float y);
int b3InProcessMouseButtonCallback(b3PhysicsClientHandle clientHandle, int button, int state, float x, float y);
#ifdef __cplusplus
}
#endif
#endif //IN_PROCESS_PHYSICS_C_API_H
#endif //IN_PROCESS_PHYSICS_C_API_H

9
examples/SharedMemory/SharedMemoryInterface.h
View File

@@ -3,14 +3,13 @@
class SharedMemoryInterface
{
public:
public:
virtual ~SharedMemoryInterface()
{
}
virtual void* allocateSharedMemory(int key, int size, bool allowCreation) =0;
virtual void releaseSharedMemory(int key, int size) =0;
virtual void* allocateSharedMemory(int key, int size, bool allowCreation) = 0;
virtual void releaseSharedMemory(int key, int size) = 0;
};
#endif

687
examples/SharedMemory/SharedMemoryPublic.h
View File

File diff suppressed because it is too large Load Diff

42
examples/SharedMemory/SharedMemoryUserData.h
View File

@@ -6,7 +6,7 @@
#include "LinearMath/btHashMap.h"
#include "SharedMemoryPublic.h"
struct SharedMemoryUserData
struct SharedMemoryUserData
{
std::string m_key;
int m_type;
@@ -16,14 +16,14 @@ struct SharedMemoryUserData
int m_visualShapeIndex;
btAlignedObjectArray<char> m_bytes;
SharedMemoryUserData()
:m_type(-1), m_bodyUniqueId(-1), m_linkIndex(-1), m_visualShapeIndex(-1)
: m_type(-1), m_bodyUniqueId(-1), m_linkIndex(-1), m_visualShapeIndex(-1)
{
}
SharedMemoryUserData(const char* key, int bodyUniqueId, int linkIndex, int visualShapeIndex)
:m_key(key), m_type(-1), m_bodyUniqueId(bodyUniqueId), m_linkIndex(linkIndex), m_visualShapeIndex(visualShapeIndex)
: m_key(key), m_type(-1), m_bodyUniqueId(bodyUniqueId), m_linkIndex(linkIndex), m_visualShapeIndex(visualShapeIndex)
{
}
@@ -31,7 +31,7 @@ struct SharedMemoryUserData
{
m_type = type;
m_bytes.resize(len);
for (int i=0;i<len;i++)
for (int i = 0; i < len; i++)
{
m_bytes[i] = bytes[i];
}
@@ -48,7 +48,8 @@ struct SharedMemoryUserData
}
};
struct SharedMemoryUserDataHashKey {
struct SharedMemoryUserDataHashKey
{
unsigned int m_hash;
btHashString m_key;
@@ -56,35 +57,40 @@ struct SharedMemoryUserDataHashKey {
btHashInt m_linkIndex;
btHashInt m_visualShapeIndex;
SIMD_FORCE_INLINE unsigned int getHash()const {
SIMD_FORCE_INLINE unsigned int getHash() const
{
return m_hash;
}
SharedMemoryUserDataHashKey() : m_hash(0) {}
SharedMemoryUserDataHashKey(const struct SharedMemoryUserData *userData)
: m_key(userData->m_key.c_str()),
SharedMemoryUserDataHashKey(const struct SharedMemoryUserData* userData)
: m_key(userData->m_key.c_str()),
m_bodyUniqueId(userData->m_bodyUniqueId),
m_linkIndex(userData->m_linkIndex),
m_visualShapeIndex(userData->m_visualShapeIndex) {
m_visualShapeIndex(userData->m_visualShapeIndex)
{
calculateHash();
}
SharedMemoryUserDataHashKey(const char *key, int bodyUniqueId, int linkIndex, int visualShapeIndex)
: m_key(key), m_bodyUniqueId(bodyUniqueId), m_linkIndex(linkIndex), m_visualShapeIndex(visualShapeIndex) {
SharedMemoryUserDataHashKey(const char* key, int bodyUniqueId, int linkIndex, int visualShapeIndex)
: m_key(key), m_bodyUniqueId(bodyUniqueId), m_linkIndex(linkIndex), m_visualShapeIndex(visualShapeIndex)
{
calculateHash();
}
void calculateHash() {
void calculateHash()
{
m_hash = m_key.getHash() ^ m_bodyUniqueId.getHash() ^ m_linkIndex.getHash() ^ m_visualShapeIndex.getHash();
}
bool equals(const SharedMemoryUserDataHashKey& other) const {
bool equals(const SharedMemoryUserDataHashKey& other) const
{
return m_bodyUniqueId.equals(other.m_bodyUniqueId) &&
m_linkIndex.equals(other.m_linkIndex) &&
m_visualShapeIndex.equals(other.m_visualShapeIndex) &&
m_key.equals(other.m_key);
m_linkIndex.equals(other.m_linkIndex) &&
m_visualShapeIndex.equals(other.m_visualShapeIndex) &&
m_key.equals(other.m_key);
}
};
#endif //SHARED_MEMORY_USER_DATA_H
#endif //SHARED_MEMORY_USER_DATA_H

141
examples/SharedMemory/Win32SharedMemory.cpp
View File

@@ -8,27 +8,24 @@
#include <stdio.h>
//see also https://msdn.microsoft.com/en-us/library/windows/desktop/aa366551%28v=vs.85%29.aspx
struct Win32SharedMemorySegment
{
int m_key;
HANDLE m_hMapFile;
void* m_buf;
void* m_buf;
TCHAR m_szName[1024];
Win32SharedMemorySegment()
:m_hMapFile(0),
m_buf(0),
m_key(-1)
: m_hMapFile(0),
m_buf(0),
m_key(-1)
{
m_szName[0] = 0;
}
};
struct Win32SharedMemoryInteralData
{
b3AlignedObjectArray<Win32SharedMemorySegment> m_segments;
Win32SharedMemoryInteralData()
@@ -45,95 +42,91 @@ Win32SharedMemory::~Win32SharedMemory()
delete m_internalData;
}
void* Win32SharedMemory::allocateSharedMemory(int key, int size, bool allowCreation)
void* Win32SharedMemory::allocateSharedMemory(int key, int size, bool allowCreation)
{
{
Win32SharedMemorySegment* seg = 0;
int i=0;
for (i=0;i<m_internalData->m_segments.size();i++)
{
if (m_internalData->m_segments[i].m_key == key)
{
seg = &m_internalData->m_segments[i];
break;
}
}
if (seg)
{
b3Error("already created shared memory segment using same key");
return seg->m_buf;
}
Win32SharedMemorySegment* seg = 0;
int i = 0;
}
for (i = 0; i < m_internalData->m_segments.size(); i++)
{
if (m_internalData->m_segments[i].m_key == key)
{
seg = &m_internalData->m_segments[i];
break;
}
}
if (seg)
{
b3Error("already created shared memory segment using same key");
return seg->m_buf;
}
}
Win32SharedMemorySegment seg;
seg.m_key = key;
#ifdef UNICODE
swprintf_s (seg.m_szName,TEXT("MyFileMappingObject%d"),key);
swprintf_s(seg.m_szName, TEXT("MyFileMappingObject%d"), key);
#else
sprintf(seg.m_szName,"MyFileMappingObject%d",key);
sprintf(seg.m_szName, "MyFileMappingObject%d", key);
#endif
seg.m_hMapFile = OpenFileMapping(
FILE_MAP_ALL_ACCESS, // read/write access
FALSE, // do not inherit the name
seg.m_szName); // name of mapping object
FILE_MAP_ALL_ACCESS, // read/write access
FALSE, // do not inherit the name
seg.m_szName); // name of mapping object
if (seg.m_hMapFile==NULL)
if (seg.m_hMapFile == NULL)
{
if (allowCreation)
{
seg.m_hMapFile = CreateFileMapping(
INVALID_HANDLE_VALUE, // use paging file
NULL, // default security
PAGE_READWRITE, // read/write access
0, // maximum object size (high-order DWORD)
size, // maximum object size (low-order DWORD)
seg.m_szName); // name of mapping object
} else
{
b3Warning("Could not create file mapping object (%d).\n",GetLastError());
return 0;
}
if (allowCreation)
{
seg.m_hMapFile = CreateFileMapping(
INVALID_HANDLE_VALUE, // use paging file
NULL, // default security
PAGE_READWRITE, // read/write access
0, // maximum object size (high-order DWORD)
size, // maximum object size (low-order DWORD)
seg.m_szName); // name of mapping object
}
else
{
b3Warning("Could not create file mapping object (%d).\n", GetLastError());
return 0;
}
}
seg.m_buf = MapViewOfFile(seg.m_hMapFile, // handle to map object
FILE_MAP_ALL_ACCESS, // read/write permission
0,
0,
size);
seg.m_buf = MapViewOfFile(seg.m_hMapFile, // handle to map object
FILE_MAP_ALL_ACCESS, // read/write permission
0,
0,
size);
if (seg.m_buf == NULL)
if (seg.m_buf == NULL)
{
b3Warning("Could not map view of file (%d).\n",GetLastError());
b3Warning("Could not map view of file (%d).\n", GetLastError());
CloseHandle(seg.m_hMapFile);
return 0;
}
}
m_internalData->m_segments.push_back(seg);
return seg.m_buf;
m_internalData->m_segments.push_back(seg);
return seg.m_buf;
}
void Win32SharedMemory::releaseSharedMemory(int key, int size)
{
Win32SharedMemorySegment* seg = 0;
int i = 0;
Win32SharedMemorySegment* seg = 0;
int i=0;
for (i=0;i<m_internalData->m_segments.size();i++)
{
if (m_internalData->m_segments[i].m_key == key)
{
seg = &m_internalData->m_segments[i];
break;
}
}
for (i = 0; i < m_internalData->m_segments.size(); i++)
{
if (m_internalData->m_segments[i].m_key == key)
{
seg = &m_internalData->m_segments[i];
break;
}
}
if (seg==0)
if (seg == 0)
{
b3Error("Couldn't find shared memory segment");
return;
@@ -142,7 +135,7 @@ void Win32SharedMemory::releaseSharedMemory(int key, int size)
if (seg->m_buf)
{
UnmapViewOfFile(seg->m_buf);
seg->m_buf=0;
seg->m_buf = 0;
}
if (seg->m_hMapFile)
@@ -164,8 +157,8 @@ Win32SharedMemoryServer::~Win32SharedMemoryServer()
Win32SharedMemoryClient::Win32SharedMemoryClient()
{
}
Win32SharedMemoryClient:: ~Win32SharedMemoryClient()
Win32SharedMemoryClient::~Win32SharedMemoryClient()
{
}
#endif //_WIN32
#endif //_WIN32

21
examples/SharedMemory/Win32SharedMemory.h
View File

@@ -3,20 +3,16 @@
#include "SharedMemoryInterface.h"
class Win32SharedMemory : public SharedMemoryInterface
{
struct Win32SharedMemoryInteralData* m_internalData;
public:
Win32SharedMemory();
virtual ~Win32SharedMemory();
virtual void* allocateSharedMemory(int key, int size, bool allowCreation);
virtual void releaseSharedMemory(int key, int size);
public:
Win32SharedMemory();
virtual ~Win32SharedMemory();
virtual void* allocateSharedMemory(int key, int size, bool allowCreation);
virtual void releaseSharedMemory(int key, int size);
};
class Win32SharedMemoryServer : public Win32SharedMemory
@@ -24,7 +20,6 @@ class Win32SharedMemoryServer : public Win32SharedMemory
public:
Win32SharedMemoryServer();
virtual ~Win32SharedMemoryServer();
};
class Win32SharedMemoryClient : public Win32SharedMemory
@@ -32,8 +27,6 @@ class Win32SharedMemoryClient : public Win32SharedMemory
public:
Win32SharedMemoryClient();
virtual ~Win32SharedMemoryClient();
};
#endif //WIN32_SHARED_MEMORY_H
#endif //WIN32_SHARED_MEMORY_H

130
examples/SharedMemory/b3PluginManager.cpp
View File

@@ -7,27 +7,27 @@
#include "plugins/b3PluginContext.h"
#ifdef _WIN32
#define WIN32_LEAN_AND_MEAN
#define VC_EXTRALEAN
#include <windows.h>
#define WIN32_LEAN_AND_MEAN
#define VC_EXTRALEAN
#include <windows.h>
typedef HMODULE B3_DYNLIB_HANDLE;
typedef HMODULE B3_DYNLIB_HANDLE;
#define B3_DYNLIB_OPEN LoadLibraryA
#define B3_DYNLIB_CLOSE FreeLibrary
#define B3_DYNLIB_IMPORT GetProcAddress
#define B3_DYNLIB_OPEN LoadLibraryA
#define B3_DYNLIB_CLOSE FreeLibrary
#define B3_DYNLIB_IMPORT GetProcAddress
#else
#include <dlfcn.h>
#include <dlfcn.h>
typedef void* B3_DYNLIB_HANDLE;
typedef void* B3_DYNLIB_HANDLE;
#ifdef B3_USE_DLMOPEN
#define B3_DYNLIB_OPEN(path) dlmopen(LM_ID_NEWLM, path, RTLD_LAZY)
#define B3_DYNLIB_OPEN(path) dlmopen(LM_ID_NEWLM, path, RTLD_LAZY)
#else
#define B3_DYNLIB_OPEN(path) dlopen(path, RTLD_NOW | RTLD_GLOBAL)
#define B3_DYNLIB_OPEN(path) dlopen(path, RTLD_NOW | RTLD_GLOBAL)
#endif
#define B3_DYNLIB_CLOSE dlclose
#define B3_DYNLIB_IMPORT dlsym
#define B3_DYNLIB_CLOSE dlclose
#define B3_DYNLIB_IMPORT dlsym
#endif
struct b3Plugin
@@ -52,20 +52,20 @@ struct b3Plugin
void* m_userPointer;
b3Plugin()
:m_pluginHandle(0),
m_ownsPluginHandle(false),
m_isInitialized(false),
m_pluginUniqueId(-1),
m_initFunc(0),
m_exitFunc(0),
m_executeCommandFunc(0),
m_preTickFunc(0),
m_postTickFunc(0),
m_processNotificationsFunc(0),
m_processClientCommandsFunc(0),
m_getRendererFunc(0),
m_getCollisionFunc(0),
m_userPointer(0)
: m_pluginHandle(0),
m_ownsPluginHandle(false),
m_isInitialized(false),
m_pluginUniqueId(-1),
m_initFunc(0),
m_exitFunc(0),
m_executeCommandFunc(0),
m_preTickFunc(0),
m_postTickFunc(0),
m_processNotificationsFunc(0),
m_processClientCommandsFunc(0),
m_getRendererFunc(0),
m_getCollisionFunc(0),
m_userPointer(0)
{
}
void clear()
@@ -107,8 +107,7 @@ struct b3PluginManagerInternalData
int m_numNotificationPlugins;
b3PluginManagerInternalData()
:m_rpcCommandProcessorInterface(0), m_activeNotificationsBufferIndex(0), m_activeRendererPluginUid(-1),
m_activeCollisionPluginUid(-1), m_numNotificationPlugins(0)
: m_rpcCommandProcessorInterface(0), m_activeNotificationsBufferIndex(0), m_activeRendererPluginUid(-1), m_activeCollisionPluginUid(-1), m_numNotificationPlugins(0)
{
}
};
@@ -117,8 +116,7 @@ b3PluginManager::b3PluginManager(class PhysicsCommandProcessorInterface* physSdk
{
m_data = new b3PluginManagerInternalData;
m_data->m_rpcCommandProcessorInterface = physSdk;
m_data->m_physicsDirect = new PhysicsDirect(physSdk,false);
m_data->m_physicsDirect = new PhysicsDirect(physSdk, false);
}
b3PluginManager::~b3PluginManager()
@@ -140,7 +138,6 @@ b3PluginManager::~b3PluginManager()
void b3PluginManager::addEvents(const struct b3VRControllerEvent* vrControllerEvents, int numVRControllerEvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents, const struct b3MouseEvent* mouseEvents, int numMouseEvents)
{
for (int i = 0; i < numKeyEvents; i++)
{
m_data->m_keyEvents.push_back(keyEvents[i]);
@@ -165,7 +162,7 @@ void b3PluginManager::clearEvents()
void b3PluginManager::addNotification(const struct b3Notification& notification)
{
if (m_data->m_numNotificationPlugins>0)
if (m_data->m_numNotificationPlugins > 0)
{
m_data->m_notifications[m_data->m_activeNotificationsBufferIndex].push_back(notification);
}
@@ -178,7 +175,6 @@ int b3PluginManager::loadPlugin(const char* pluginPath, const char* postFixStr)
int* pluginUidPtr = m_data->m_pluginMap.find(pluginPath);
if (pluginUidPtr)
{
//already loaded
pluginUniqueId = *pluginUidPtr;
b3PluginHandle* plugin = m_data->m_plugins.getHandle(pluginUniqueId);
@@ -186,7 +182,7 @@ int b3PluginManager::loadPlugin(const char* pluginPath, const char* postFixStr)
{
b3PluginContext context = {0};
context.m_userPointer = 0;
context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
context.m_physClient = (b3PhysicsClientHandle)m_data->m_physicsDirect;
context.m_rpcCommandProcessorInterface = m_data->m_rpcCommandProcessorInterface;
int result = plugin->m_initFunc(&context);
plugin->m_isInitialized = true;
@@ -212,7 +208,6 @@ int b3PluginManager::loadPlugin(const char* pluginPath, const char* postFixStr)
std::string processClientCommandsStr = std::string("processClientCommands") + postFix;
std::string getRendererStr = std::string("getRenderInterface") + postFix;
std::string getCollisionStr = std::string("getCollisionInterface") + postFix;
plugin->m_initFunc = (PFN_INIT)B3_DYNLIB_IMPORT(pluginHandle, initStr.c_str());
plugin->m_exitFunc = (PFN_EXIT)B3_DYNLIB_IMPORT(pluginHandle, exitStr.c_str());
@@ -227,16 +222,14 @@ int b3PluginManager::loadPlugin(const char* pluginPath, const char* postFixStr)
}
plugin->m_processClientCommandsFunc = (PFN_TICK)B3_DYNLIB_IMPORT(pluginHandle, processClientCommandsStr.c_str());
plugin->m_getRendererFunc = (PFN_GET_RENDER_INTERFACE)B3_DYNLIB_IMPORT(pluginHandle, getRendererStr.c_str());
plugin->m_getRendererFunc = (PFN_GET_RENDER_INTERFACE)B3_DYNLIB_IMPORT(pluginHandle, getRendererStr.c_str());
plugin->m_getCollisionFunc = (PFN_GET_COLLISION_INTERFACE)B3_DYNLIB_IMPORT(pluginHandle, getCollisionStr.c_str());
if (plugin->m_initFunc && plugin->m_exitFunc && plugin->m_executeCommandFunc)
{
b3PluginContext context;
context.m_userPointer = plugin->m_userPointer;
context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
context.m_physClient = (b3PhysicsClientHandle)m_data->m_physicsDirect;
context.m_rpcCommandProcessorInterface = m_data->m_rpcCommandProcessorInterface;
int version = plugin->m_initFunc(&context);
plugin->m_isInitialized = true;
@@ -244,7 +237,6 @@ int b3PluginManager::loadPlugin(const char* pluginPath, const char* postFixStr)
plugin->m_userPointer = context.m_userPointer;
if (version == SHARED_MEMORY_MAGIC_NUMBER)
{
ok = true;
plugin->m_ownsPluginHandle = true;
plugin->m_pluginHandle = pluginHandle;
@@ -269,13 +261,11 @@ int b3PluginManager::loadPlugin(const char* pluginPath, const char* postFixStr)
}
else
{
b3Warning("Warning: couldn't load plugin %s\n", pluginPath);
#ifdef _WIN32
#else
b3Warning("Error: %s\n", dlerror() );
#endif
#ifdef _WIN32
#else
b3Warning("Error: %s\n", dlerror());
#endif
}
if (!ok)
{
@@ -285,7 +275,7 @@ int b3PluginManager::loadPlugin(const char* pluginPath, const char* postFixStr)
}
//for now, automatically select the loaded plugin as active renderer.
if (pluginUniqueId>=0)
if (pluginUniqueId >= 0)
{
b3PluginHandle* plugin = m_data->m_plugins.getHandle(pluginUniqueId);
if (plugin && plugin->m_getRendererFunc)
@@ -293,9 +283,9 @@ int b3PluginManager::loadPlugin(const char* pluginPath, const char* postFixStr)
selectPluginRenderer(pluginUniqueId);
}
}
//for now, automatically select the loaded plugin as active collision plugin.
if (pluginUniqueId>=0)
if (pluginUniqueId >= 0)
{
b3PluginHandle* plugin = m_data->m_plugins.getHandle(pluginUniqueId);
if (plugin && plugin->m_getCollisionFunc)
@@ -304,7 +294,6 @@ int b3PluginManager::loadPlugin(const char* pluginPath, const char* postFixStr)
}
}
return pluginUniqueId;
}
@@ -319,7 +308,7 @@ void b3PluginManager::unloadPlugin(int pluginUniqueId)
}
b3PluginContext context = {0};
context.m_userPointer = plugin->m_userPointer;
context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
context.m_physClient = (b3PhysicsClientHandle)m_data->m_physicsDirect;
if (plugin->m_isInitialized)
{
@@ -332,11 +321,9 @@ void b3PluginManager::unloadPlugin(int pluginUniqueId)
}
}
void b3PluginManager::tickPlugins(double timeStep, b3PluginManagerTickMode tickMode)
{
for (int i=0;i<m_data->m_pluginMap.size();i++)
for (int i = 0; i < m_data->m_pluginMap.size(); i++)
{
int* pluginUidPtr = m_data->m_pluginMap.getAtIndex(i);
b3PluginHandle* plugin = 0;
@@ -351,7 +338,7 @@ void b3PluginManager::tickPlugins(double timeStep, b3PluginManagerTickMode tickM
continue;
}
PFN_TICK tick = 0;
PFN_TICK tick = 0;
switch (tickMode)
{
case B3_PRE_TICK_MODE:
@@ -376,7 +363,7 @@ void b3PluginManager::tickPlugins(double timeStep, b3PluginManagerTickMode tickM
if (tick)
{
b3PluginContext context = { 0 };
b3PluginContext context = {0};
context.m_userPointer = plugin->m_userPointer;
context.m_physClient = (b3PhysicsClientHandle)m_data->m_physicsDirect;
context.m_numMouseEvents = m_data->m_mouseEvents.size();
@@ -397,7 +384,7 @@ void b3PluginManager::tickPlugins(double timeStep, b3PluginManagerTickMode tickM
void b3PluginManager::reportNotifications()
{
b3AlignedObjectArray<b3Notification> &notifications = m_data->m_notifications[m_data->m_activeNotificationsBufferIndex];
b3AlignedObjectArray<b3Notification>& notifications = m_data->m_notifications[m_data->m_activeNotificationsBufferIndex];
if (notifications.size() == 0)
{
return;
@@ -406,7 +393,7 @@ void b3PluginManager::reportNotifications()
// Swap notification buffers.
m_data->m_activeNotificationsBufferIndex = 1 - m_data->m_activeNotificationsBufferIndex;
for (int i=0;i<m_data->m_pluginMap.size();i++)
for (int i = 0; i < m_data->m_pluginMap.size(); i++)
{
int* pluginUidPtr = m_data->m_pluginMap.getAtIndex(i);
b3PluginHandle* plugin = 0;
@@ -421,10 +408,11 @@ void b3PluginManager::reportNotifications()
continue;
}
if (plugin->m_processNotificationsFunc) {
if (plugin->m_processNotificationsFunc)
{
b3PluginContext context = {0};
context.m_userPointer = plugin->m_userPointer;
context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
context.m_physClient = (b3PhysicsClientHandle)m_data->m_physicsDirect;
context.m_numNotifications = notifications.size();
context.m_notifications = notifications.size() ? &notifications[0] : 0;
plugin->m_processNotificationsFunc(&context);
@@ -442,7 +430,7 @@ int b3PluginManager::executePluginCommand(int pluginUniqueId, const b3PluginArgu
{
b3PluginContext context = {0};
context.m_userPointer = plugin->m_userPointer;
context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
context.m_physClient = (b3PhysicsClientHandle)m_data->m_physicsDirect;
context.m_rpcCommandProcessorInterface = m_data->m_rpcCommandProcessorInterface;
result = plugin->m_executeCommandFunc(&context, arguments);
plugin->m_userPointer = context.m_userPointer;
@@ -450,16 +438,14 @@ int b3PluginManager::executePluginCommand(int pluginUniqueId, const b3PluginArgu
return result;
}
int b3PluginManager::registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT initFunc,PFN_EXIT exitFunc, PFN_EXECUTE executeCommandFunc, PFN_TICK preTickFunc, PFN_TICK postTickFunc, PFN_GET_RENDER_INTERFACE getRendererFunc, PFN_TICK processClientCommandsFunc, PFN_GET_COLLISION_INTERFACE getCollisionFunc, bool initPlugin)
int b3PluginManager::registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT initFunc, PFN_EXIT exitFunc, PFN_EXECUTE executeCommandFunc, PFN_TICK preTickFunc, PFN_TICK postTickFunc, PFN_GET_RENDER_INTERFACE getRendererFunc, PFN_TICK processClientCommandsFunc, PFN_GET_COLLISION_INTERFACE getCollisionFunc, bool initPlugin)
{
b3Plugin orgPlugin;
int pluginUniqueId = m_data->m_plugins.allocHandle();
b3PluginHandle* pluginHandle = m_data->m_plugins.getHandle(pluginUniqueId);
pluginHandle->m_pluginHandle = 0;
pluginHandle->m_ownsPluginHandle =false;
pluginHandle->m_ownsPluginHandle = false;
pluginHandle->m_pluginUniqueId = pluginUniqueId;
pluginHandle->m_executeCommandFunc = executeCommandFunc;
pluginHandle->m_exitFunc = exitFunc;
@@ -473,7 +459,6 @@ int b3PluginManager::registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT
pluginHandle->m_pluginPath = pluginPath;
pluginHandle->m_userPointer = 0;
if (pluginHandle->m_processNotificationsFunc)
{
m_data->m_numNotificationPlugins++;
@@ -485,7 +470,7 @@ int b3PluginManager::registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT
{
b3PluginContext context = {0};
context.m_userPointer = 0;
context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
context.m_physClient = (b3PhysicsClientHandle)m_data->m_physicsDirect;
context.m_rpcCommandProcessorInterface = m_data->m_rpcCommandProcessorInterface;
int result = pluginHandle->m_initFunc(&context);
pluginHandle->m_isInitialized = true;
@@ -503,14 +488,14 @@ UrdfRenderingInterface* b3PluginManager::getRenderInterface()
{
UrdfRenderingInterface* renderer = 0;
if (m_data->m_activeRendererPluginUid>=0)
if (m_data->m_activeRendererPluginUid >= 0)
{
b3PluginHandle* plugin = m_data->m_plugins.getHandle(m_data->m_activeRendererPluginUid);
if (plugin && plugin->m_getRendererFunc)
{
b3PluginContext context = {0};
context.m_userPointer = plugin->m_userPointer;
context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
context.m_physClient = (b3PhysicsClientHandle)m_data->m_physicsDirect;
renderer = plugin->m_getRendererFunc(&context);
}
}
@@ -525,17 +510,16 @@ void b3PluginManager::selectCollisionPlugin(int pluginUniqueId)
struct b3PluginCollisionInterface* b3PluginManager::getCollisionInterface()
{
b3PluginCollisionInterface* collisionInterface = 0;
if (m_data->m_activeCollisionPluginUid>=0)
if (m_data->m_activeCollisionPluginUid >= 0)
{
b3PluginHandle* plugin = m_data->m_plugins.getHandle(m_data->m_activeCollisionPluginUid);
if (plugin && plugin->m_getCollisionFunc)
{
b3PluginContext context = {0};
context.m_userPointer = plugin->m_userPointer;
context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
context.m_physClient = (b3PhysicsClientHandle)m_data->m_physicsDirect;
collisionInterface = plugin->m_getCollisionFunc(&context);
}
}
return collisionInterface;
}

43
examples/SharedMemory/b3PluginManager.h
View File

@@ -5,7 +5,7 @@
enum b3PluginManagerTickMode
{
B3_PRE_TICK_MODE=1,
B3_PRE_TICK_MODE = 1,
B3_POST_TICK_MODE,
B3_PROCESS_CLIENT_COMMANDS_TICK,
};
@@ -14,29 +14,28 @@ class b3PluginManager
{
struct b3PluginManagerInternalData* m_data;
public:
b3PluginManager(class PhysicsCommandProcessorInterface* physSdk);
virtual ~b3PluginManager();
int loadPlugin(const char* pluginPath, const char* postFixStr ="");
void unloadPlugin(int pluginUniqueId);
int executePluginCommand(int pluginUniqueId, const struct b3PluginArguments* arguments);
void addEvents(const struct b3VRControllerEvent* vrControllerEvents, int numVRControllerEvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents, const struct b3MouseEvent* mouseEvents, int numMouseEvents);
void clearEvents();
public:
b3PluginManager(class PhysicsCommandProcessorInterface* physSdk);
virtual ~b3PluginManager();
void addNotification(const struct b3Notification& notification);
void reportNotifications();
void tickPlugins(double timeStep, b3PluginManagerTickMode tickMode);
int loadPlugin(const char* pluginPath, const char* postFixStr = "");
void unloadPlugin(int pluginUniqueId);
int executePluginCommand(int pluginUniqueId, const struct b3PluginArguments* arguments);
void addEvents(const struct b3VRControllerEvent* vrControllerEvents, int numVRControllerEvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents, const struct b3MouseEvent* mouseEvents, int numMouseEvents);
void clearEvents();
int registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT initFunc, PFN_EXIT exitFunc, PFN_EXECUTE executeCommandFunc, PFN_TICK preTickFunc, PFN_TICK postTickFunc, PFN_GET_RENDER_INTERFACE getRendererFunc, PFN_TICK processClientCommandsFunc, PFN_GET_COLLISION_INTERFACE getCollisionFunc, bool initPlugin=true);
void selectPluginRenderer(int pluginUniqueId);
struct UrdfRenderingInterface* getRenderInterface();
void addNotification(const struct b3Notification& notification);
void reportNotifications();
void selectCollisionPlugin(int pluginUniqueId);
struct b3PluginCollisionInterface* getCollisionInterface();
void tickPlugins(double timeStep, b3PluginManagerTickMode tickMode);
int registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT initFunc, PFN_EXIT exitFunc, PFN_EXECUTE executeCommandFunc, PFN_TICK preTickFunc, PFN_TICK postTickFunc, PFN_GET_RENDER_INTERFACE getRendererFunc, PFN_TICK processClientCommandsFunc, PFN_GET_COLLISION_INTERFACE getCollisionFunc, bool initPlugin = true);
void selectPluginRenderer(int pluginUniqueId);
struct UrdfRenderingInterface* getRenderInterface();
void selectCollisionPlugin(int pluginUniqueId);
struct b3PluginCollisionInterface* getCollisionInterface();
};
#endif //B3_PLUGIN_MANAGER_H
#endif //B3_PLUGIN_MANAGER_H

4
examples/SharedMemory/b3RobotSimulatorClientAPI_InternalData.h
View File

@@ -10,9 +10,9 @@ struct b3RobotSimulatorClientAPI_InternalData
b3RobotSimulatorClientAPI_InternalData()
: m_physicsClientHandle(0),
m_guiHelper(0)
m_guiHelper(0)
{
}
};
#endif //B3_ROBOT_SIMULATOR_CLIENT_API_INTERNAL_DATA_H
#endif //B3_ROBOT_SIMULATOR_CLIENT_API_INTERNAL_DATA_H

858
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
View File

File diff suppressed because it is too large Load Diff

331
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
View File

@@ -21,21 +21,21 @@ struct b3RobotSimulatorLoadUrdfFileArgs
bool m_useMultiBody;
int m_flags;
b3RobotSimulatorLoadUrdfFileArgs(const btVector3& startPos, const btQuaternion& startOrn)
b3RobotSimulatorLoadUrdfFileArgs(const btVector3 &startPos, const btQuaternion &startOrn)
: m_startPosition(startPos),
m_startOrientation(startOrn),
m_forceOverrideFixedBase(false),
m_useMultiBody(true),
m_flags(0)
m_startOrientation(startOrn),
m_forceOverrideFixedBase(false),
m_useMultiBody(true),
m_flags(0)
{
}
b3RobotSimulatorLoadUrdfFileArgs()
: m_startPosition(btVector3(0, 0, 0)),
m_startOrientation(btQuaternion(0, 0, 0, 1)),
m_forceOverrideFixedBase(false),
m_useMultiBody(true),
m_flags(0)
m_startOrientation(btQuaternion(0, 0, 0, 1)),
m_forceOverrideFixedBase(false),
m_useMultiBody(true),
m_flags(0)
{
}
};
@@ -47,7 +47,7 @@ struct b3RobotSimulatorLoadSdfFileArgs
b3RobotSimulatorLoadSdfFileArgs()
: m_forceOverrideFixedBase(false),
m_useMultiBody(true)
m_useMultiBody(true)
{
}
};
@@ -62,26 +62,26 @@ struct b3RobotSimulatorLoadFileResults
struct b3RobotSimulatorChangeVisualShapeArgs
{
int m_objectUniqueId;
int m_linkIndex;
int m_shapeIndex;
int m_textureUniqueId;
btVector4 m_rgbaColor;
bool m_hasRgbaColor;
btVector3 m_specularColor;
bool m_hasSpecularColor;
b3RobotSimulatorChangeVisualShapeArgs()
:m_objectUniqueId(-1),
m_linkIndex(-1),
m_shapeIndex(-1),
m_textureUniqueId(-1),
m_rgbaColor(0,0,0,1),
m_hasRgbaColor(false),
m_specularColor(1,1,1),
m_hasSpecularColor(false)
{
}
int m_objectUniqueId;
int m_linkIndex;
int m_shapeIndex;
int m_textureUniqueId;
btVector4 m_rgbaColor;
bool m_hasRgbaColor;
btVector3 m_specularColor;
bool m_hasSpecularColor;
b3RobotSimulatorChangeVisualShapeArgs()
: m_objectUniqueId(-1),
m_linkIndex(-1),
m_shapeIndex(-1),
m_textureUniqueId(-1),
m_rgbaColor(0, 0, 0, 1),
m_hasRgbaColor(false),
m_specularColor(1, 1, 1),
m_hasSpecularColor(false)
{
}
};
struct b3RobotSimulatorJointMotorArgs
@@ -98,11 +98,11 @@ struct b3RobotSimulatorJointMotorArgs
b3RobotSimulatorJointMotorArgs(int controlMode)
: m_controlMode(controlMode),
m_targetPosition(0),
m_kp(0.1),
m_targetVelocity(0),
m_kd(0.9),
m_maxTorqueValue(1000)
m_targetPosition(0),
m_kp(0.1),
m_targetVelocity(0),
m_kd(0.9),
m_maxTorqueValue(1000)
{
}
};
@@ -112,7 +112,7 @@ enum b3RobotSimulatorInverseKinematicsFlags
B3_HAS_IK_TARGET_ORIENTATION = 1,
B3_HAS_NULL_SPACE_VELOCITY = 2,
B3_HAS_JOINT_DAMPING = 4,
B3_HAS_CURRENT_POSITIONS = 8,
B3_HAS_CURRENT_POSITIONS = 8,
};
struct b3RobotSimulatorInverseKinematicArgs
@@ -128,12 +128,12 @@ struct b3RobotSimulatorInverseKinematicArgs
btAlignedObjectArray<double> m_jointRanges;
btAlignedObjectArray<double> m_restPoses;
btAlignedObjectArray<double> m_jointDamping;
btAlignedObjectArray<double> m_currentJointPositions;
btAlignedObjectArray<double> m_currentJointPositions;
b3RobotSimulatorInverseKinematicArgs()
: m_bodyUniqueId(-1),
m_endEffectorLinkIndex(-1),
m_flags(0)
m_endEffectorLinkIndex(-1),
m_flags(0)
{
m_endEffectorTargetPosition[0] = 0;
m_endEffectorTargetPosition[1] = 0;
@@ -163,7 +163,6 @@ struct b3JointStates2
btAlignedObjectArray<double> m_jointReactionForces;
};
struct b3RobotSimulatorJointMotorArrayArgs
{
int m_controlMode;
@@ -209,40 +208,35 @@ struct b3RobotSimulatorGetCameraImageArgs
b3RobotSimulatorGetCameraImageArgs(int width, int height)
: m_width(width),
m_height(height),
m_viewMatrix(NULL),
m_projectionMatrix(NULL),
m_lightDirection(NULL),
m_lightColor(NULL),
m_lightDistance(-1),
m_hasShadow(-1),
m_lightAmbientCoeff(-1),
m_lightDiffuseCoeff(-1),
m_lightSpecularCoeff(-1),
m_renderer(-1)
m_height(height),
m_viewMatrix(NULL),
m_projectionMatrix(NULL),
m_lightDirection(NULL),
m_lightColor(NULL),
m_lightDistance(-1),
m_hasShadow(-1),
m_lightAmbientCoeff(-1),
m_lightDiffuseCoeff(-1),
m_lightSpecularCoeff(-1),
m_renderer(-1)
{
}
};
struct b3RobotSimulatorSetPhysicsEngineParameters : b3PhysicsSimulationParameters
{
b3RobotSimulatorSetPhysicsEngineParameters()
{
m_deltaTime=-1;
m_deltaTime = -1;
m_gravityAcceleration[0] = 0;
m_gravityAcceleration[1] = 0;
m_gravityAcceleration[2] = 0;
m_numSimulationSubSteps=-1;
m_numSolverIterations=-1;
m_numSimulationSubSteps = -1;
m_numSolverIterations = -1;
m_useRealTimeSimulation = -1;
m_useSplitImpulse = -1;
m_splitImpulsePenetrationThreshold =-1;
m_splitImpulsePenetrationThreshold = -1;
m_contactBreakingThreshold = -1;
m_internalSimFlags = -1;
m_defaultContactERP = -1;
@@ -259,18 +253,17 @@ struct b3RobotSimulatorSetPhysicsEngineParameters : b3PhysicsSimulationParameter
m_jointFeedbackMode = -1;
m_solverResidualThreshold = -1;
m_contactSlop = -1;
m_collisionFilterMode=-1;
m_contactBreakingThreshold=-1;
m_enableFileCaching=-1;
m_restitutionVelocityThreshold=-1;
m_frictionERP=-1;
m_solverResidualThreshold=-1;
m_collisionFilterMode = -1;
m_contactBreakingThreshold = -1;
m_enableFileCaching = -1;
m_restitutionVelocityThreshold = -1;
m_frictionERP = -1;
m_solverResidualThreshold = -1;
m_constraintSolverType = -1;
m_minimumSolverIslandSize = -1;
}
};
@@ -290,17 +283,18 @@ struct b3RobotSimulatorChangeDynamicsArgs
b3RobotSimulatorChangeDynamicsArgs()
: m_mass(-1),
m_lateralFriction(-1),
m_spinningFriction(-1),
m_rollingFriction(-1),
m_restitution(-1),
m_linearDamping(-1),
m_angularDamping(-1),
m_contactStiffness(-1),
m_contactDamping(-1),
m_frictionAnchor(-1),
m_activationState(-1)
{}
m_lateralFriction(-1),
m_spinningFriction(-1),
m_rollingFriction(-1),
m_restitution(-1),
m_linearDamping(-1),
m_angularDamping(-1),
m_contactStiffness(-1),
m_contactDamping(-1),
m_frictionAnchor(-1),
m_activationState(-1)
{
}
};
struct b3RobotSimulatorAddUserDebugLineArgs
@@ -312,11 +306,10 @@ struct b3RobotSimulatorAddUserDebugLineArgs
int m_parentLinkIndex;
b3RobotSimulatorAddUserDebugLineArgs()
:
m_lineWidth(1),
m_lifeTime(0),
m_parentObjectUniqueId(-1),
m_parentLinkIndex(-1)
: m_lineWidth(1),
m_lifeTime(0),
m_parentObjectUniqueId(-1),
m_parentLinkIndex(-1)
{
m_colorRGB[0] = 1;
m_colorRGB[1] = 1;
@@ -324,7 +317,8 @@ struct b3RobotSimulatorAddUserDebugLineArgs
}
};
enum b3AddUserDebugTextFlags {
enum b3AddUserDebugTextFlags
{
DEBUG_TEXT_HAS_ORIENTATION = 1
};
@@ -340,10 +334,10 @@ struct b3RobotSimulatorAddUserDebugTextArgs
b3RobotSimulatorAddUserDebugTextArgs()
: m_size(1),
m_lifeTime(0),
m_parentObjectUniqueId(-1),
m_parentLinkIndex(-1),
m_flags(0)
m_lifeTime(0),
m_parentObjectUniqueId(-1),
m_parentLinkIndex(-1),
m_flags(0)
{
m_colorRGB[0] = 1;
m_colorRGB[1] = 1;
@@ -353,7 +347,6 @@ struct b3RobotSimulatorAddUserDebugTextArgs
m_textOrientation[1] = 0;
m_textOrientation[2] = 0;
m_textOrientation[3] = 1;
}
};
@@ -366,10 +359,11 @@ struct b3RobotSimulatorGetContactPointsArgs
b3RobotSimulatorGetContactPointsArgs()
: m_bodyUniqueIdA(-1),
m_bodyUniqueIdB(-1),
m_linkIndexA(-2),
m_linkIndexB(-2)
{}
m_bodyUniqueIdB(-1),
m_linkIndexA(-2),
m_linkIndexB(-2)
{
}
};
struct b3RobotSimulatorCreateCollisionShapeArgs
@@ -378,16 +372,16 @@ struct b3RobotSimulatorCreateCollisionShapeArgs
double m_radius;
btVector3 m_halfExtents;
double m_height;
char* m_fileName;
char *m_fileName;
btVector3 m_meshScale;
btVector3 m_planeNormal;
int m_flags;
b3RobotSimulatorCreateCollisionShapeArgs()
: m_shapeType(-1),
m_radius(0.5),
m_height(1),
m_fileName(NULL),
m_flags(0)
m_radius(0.5),
m_height(1),
m_fileName(NULL),
m_flags(0)
{
m_halfExtents.m_floats[0] = 1;
m_halfExtents.m_floats[1] = 1;
@@ -401,7 +395,6 @@ struct b3RobotSimulatorCreateCollisionShapeArgs
m_planeNormal.m_floats[1] = 0;
m_planeNormal.m_floats[2] = 1;
}
};
struct b3RobotSimulatorCreateMultiBodyArgs
@@ -429,28 +422,15 @@ struct b3RobotSimulatorCreateMultiBodyArgs
int m_useMaximalCoordinates;
b3RobotSimulatorCreateMultiBodyArgs()
: m_baseMass(0), m_baseCollisionShapeIndex(-1), m_baseVisualShapeIndex(-1),
m_numLinks(0),
m_linkMasses(NULL),
m_linkCollisionShapeIndices(NULL),
m_linkVisualShapeIndices(NULL),
m_linkPositions(NULL),
m_linkOrientations(NULL),
m_linkInertialFramePositions(NULL),
m_linkInertialFrameOrientations(NULL),
m_linkParentIndices(NULL),
m_linkJointTypes(NULL),
m_linkJointAxes(NULL),
m_useMaximalCoordinates(0)
: m_baseMass(0), m_baseCollisionShapeIndex(-1), m_baseVisualShapeIndex(-1), m_numLinks(0), m_linkMasses(NULL), m_linkCollisionShapeIndices(NULL), m_linkVisualShapeIndices(NULL), m_linkPositions(NULL), m_linkOrientations(NULL), m_linkInertialFramePositions(NULL), m_linkInertialFrameOrientations(NULL), m_linkParentIndices(NULL), m_linkJointTypes(NULL), m_linkJointAxes(NULL), m_useMaximalCoordinates(0)
{
m_basePosition.setValue(0,0,0);
m_baseOrientation.setValue(0,0,0,1);
m_baseInertialFramePosition.setValue(0,0,0);
m_baseInertialFrameOrientation.setValue(0,0,0,1);
m_basePosition.setValue(0, 0, 0);
m_baseOrientation.setValue(0, 0, 0, 1);
m_baseInertialFramePosition.setValue(0, 0, 0);
m_baseInertialFrameOrientation.setValue(0, 0, 0, 1);
}
};
struct b3RobotJointInfo : public b3JointInfo
{
b3RobotJointInfo()
@@ -480,7 +460,7 @@ struct b3RobotJointInfo : public b3JointInfo
m_parentFrame[5] = 0;
m_parentFrame[6] = 1;
//position
//position
m_childFrame[0] = 0;
m_childFrame[1] = 0;
m_childFrame[2] = 0;
@@ -490,28 +470,23 @@ struct b3RobotJointInfo : public b3JointInfo
m_childFrame[5] = 0;
m_childFrame[6] = 1;
m_jointAxis[0] = 0;
m_jointAxis[1] = 0;
m_jointAxis[2] = 1;
}
};
class b3RobotSimulatorClientAPI_NoDirect
{
protected:
struct b3RobotSimulatorClientAPI_InternalData* m_data;
struct b3RobotSimulatorClientAPI_InternalData *m_data;
public:
b3RobotSimulatorClientAPI_NoDirect();
virtual ~b3RobotSimulatorClientAPI_NoDirect();
//No 'connect', use setInternalData to bypass the connect method, pass an existing client
virtual void setInternalData(struct b3RobotSimulatorClientAPI_InternalData* data);
virtual void setInternalData(struct b3RobotSimulatorClientAPI_InternalData *data);
void disconnect();
@@ -523,52 +498,52 @@ public:
void resetSimulation();
btQuaternion getQuaternionFromEuler(const btVector3& rollPitchYaw);
btVector3 getEulerFromQuaternion(const btQuaternion& quat);
btQuaternion getQuaternionFromEuler(const btVector3 &rollPitchYaw);
btVector3 getEulerFromQuaternion(const btQuaternion &quat);
int loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args = b3RobotSimulatorLoadUrdfFileArgs());
bool loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args = b3RobotSimulatorLoadSdfFileArgs());
bool loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results);
bool loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results);
bool saveBullet(const std::string& fileName);
int loadTexture(const std::string& fileName);
bool changeVisualShape(const struct b3RobotSimulatorChangeVisualShapeArgs& args);
bool savePythonWorld(const std::string& fileName);
bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo* bodyInfo);
int loadURDF(const std::string &fileName, const struct b3RobotSimulatorLoadUrdfFileArgs &args = b3RobotSimulatorLoadUrdfFileArgs());
bool loadSDF(const std::string &fileName, b3RobotSimulatorLoadFileResults &results, const struct b3RobotSimulatorLoadSdfFileArgs &args = b3RobotSimulatorLoadSdfFileArgs());
bool loadMJCF(const std::string &fileName, b3RobotSimulatorLoadFileResults &results);
bool loadBullet(const std::string &fileName, b3RobotSimulatorLoadFileResults &results);
bool saveBullet(const std::string &fileName);
bool getBasePositionAndOrientation(int bodyUniqueId, btVector3& basePosition, btQuaternion& baseOrientation) const;
bool resetBasePositionAndOrientation(int bodyUniqueId, btVector3& basePosition, btQuaternion& baseOrientation);
int loadTexture(const std::string &fileName);
bool getBaseVelocity(int bodyUniqueId, btVector3& baseLinearVelocity, btVector3& baseAngularVelocity) const;
bool resetBaseVelocity(int bodyUniqueId, const btVector3& linearVelocity, const btVector3& angularVelocity) const;
bool changeVisualShape(const struct b3RobotSimulatorChangeVisualShapeArgs &args);
bool savePythonWorld(const std::string &fileName);
bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo *bodyInfo);
bool getBasePositionAndOrientation(int bodyUniqueId, btVector3 &basePosition, btQuaternion &baseOrientation) const;
bool resetBasePositionAndOrientation(int bodyUniqueId, btVector3 &basePosition, btQuaternion &baseOrientation);
bool getBaseVelocity(int bodyUniqueId, btVector3 &baseLinearVelocity, btVector3 &baseAngularVelocity) const;
bool resetBaseVelocity(int bodyUniqueId, const btVector3 &linearVelocity, const btVector3 &angularVelocity) const;
int getNumJoints(int bodyUniqueId) const;
bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo);
bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo *jointInfo);
int createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo);
int createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo *jointInfo);
int changeConstraint(int constraintId, b3JointInfo *jointInfo);
int changeConstraint(int constraintId, b3JointInfo* jointInfo);
void removeConstraint(int constraintId);
bool getConstraintInfo(int constraintUniqueId, struct b3UserConstraint& constraintInfo);
bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state);
bool getConstraintInfo(int constraintUniqueId, struct b3UserConstraint &constraintInfo);
bool getJointStates(int bodyUniqueId, b3JointStates2& state);
bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState *state);
bool getJointStates(int bodyUniqueId, b3JointStates2 &state);
bool resetJointState(int bodyUniqueId, int jointIndex, double targetValue);
void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3RobotSimulatorJointMotorArgs& args);
void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3RobotSimulatorJointMotorArgs &args);
bool setJointMotorControlArray(int bodyUniqueId, int controlMode, int numControlledDofs,
int *jointIndices, double *targetVelocities, double *targetPositions,
double *forces, double *kps, double *kds);
int *jointIndices, double *targetVelocities, double *targetPositions,
double *forces, double *kps, double *kds);
void stepSimulation();
@@ -578,33 +553,33 @@ public:
void setInternalSimFlags(int flags);
void setGravity(const btVector3& gravityAcceleration);
void setGravity(const btVector3 &gravityAcceleration);
void setTimeStep(double timeStepInSeconds);
void setNumSimulationSubSteps(int numSubSteps);
void setNumSolverIterations(int numIterations);
void setContactBreakingThreshold(double threshold);
bool calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results);
bool calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs &args, struct b3RobotSimulatorInverseKinematicsResults &results);
bool getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian);
bool getBodyJacobian(int bodyUniqueId, int linkIndex, const double *localPosition, const double *jointPositions, const double *jointVelocities, const double *jointAccelerations, double *linearJacobian, double *angularJacobian);
void configureDebugVisualizer(enum b3ConfigureDebugVisualizerEnum flag, int enable);
void resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const btVector3& targetPos);
void resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const btVector3 &targetPos);
int startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const btAlignedObjectArray<int>& objectUniqueIds=btAlignedObjectArray<int>(), int maxLogDof = -1);
int startStateLogging(b3StateLoggingType loggingType, const std::string &fileName, const btAlignedObjectArray<int> &objectUniqueIds = btAlignedObjectArray<int>(), int maxLogDof = -1);
void stopStateLogging(int stateLoggerUniqueId);
void getVREvents(b3VREventsData* vrEventsData, int deviceTypeFilter);
void getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData);
void getVREvents(b3VREventsData *vrEventsData, int deviceTypeFilter);
void getKeyboardEvents(b3KeyboardEventsData *keyboardEventsData);
void submitProfileTiming(const std::string& profileName, int durationInMicroSeconds=1);
void submitProfileTiming(const std::string &profileName, int durationInMicroSeconds = 1);
// JFC: added these 24 methods
void getMouseEvents(b3MouseEventsData* mouseEventsData);
void getMouseEvents(b3MouseEventsData *mouseEventsData);
bool getLinkState(int bodyUniqueId, int linkIndex, int computeLinkVelocity, int computeForwardKinematics, b3LinkState* linkState);
bool getLinkState(int bodyUniqueId, int linkIndex, int computeLinkVelocity, int computeForwardKinematics, b3LinkState *linkState);
bool getCameraImage(int width, int height, struct b3RobotSimulatorGetCameraImageArgs args, b3CameraImageData &imageData);
@@ -636,9 +611,9 @@ public:
bool setJointMotorControlArray(int bodyUniqueId, struct b3RobotSimulatorJointMotorArrayArgs &args);
bool setPhysicsEngineParameter(const struct b3RobotSimulatorSetPhysicsEngineParameters&args);
bool getPhysicsEngineParameters(struct b3RobotSimulatorSetPhysicsEngineParameters&args);
bool setPhysicsEngineParameter(const struct b3RobotSimulatorSetPhysicsEngineParameters &args);
bool getPhysicsEngineParameters(struct b3RobotSimulatorSetPhysicsEngineParameters &args);
bool applyExternalForce(int objectUniqueId, int linkIndex, double *force, double *position, int flags);
@@ -672,10 +647,10 @@ public:
int getConstraintUniqueId(int serialIndex);
void loadSoftBody(const std::string& fileName, double scale, double mass, double collisionMargin);
virtual void setGuiHelper(struct GUIHelperInterface* guiHelper);
virtual struct GUIHelperInterface* getGuiHelper();
void loadSoftBody(const std::string &fileName, double scale, double mass, double collisionMargin);
virtual void setGuiHelper(struct GUIHelperInterface *guiHelper);
virtual struct GUIHelperInterface *getGuiHelper();
bool getCollisionShapeData(int bodyUniqueId, int linkIndex, b3CollisionShapeInformation &collisionShapeInfo);
@@ -688,9 +663,7 @@ public:
{
return SHARED_MEMORY_MAGIC_NUMBER;
}
void setAdditionalSearchPath(const std::string& path);
void setAdditionalSearchPath(const std::string &path);
};
#endif //B3_ROBOT_SIMULATOR_CLIENT_API_NO_DIRECT_H

18
examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp
View File

@@ -13,13 +13,11 @@
#ifndef BT_DISABLE_PHYSICS_DIRECT
#include "PhysicsDirectC_API.h"
#endif //BT_DISABLE_PHYSICS_DIRECT
#endif //BT_DISABLE_PHYSICS_DIRECT
#include "SharedMemoryPublic.h"
#include "Bullet3Common/b3Logging.h"
b3RobotSimulatorClientAPI_NoGUI::b3RobotSimulatorClientAPI_NoGUI()
{
}
@@ -28,7 +26,6 @@ b3RobotSimulatorClientAPI_NoGUI::~b3RobotSimulatorClientAPI_NoGUI()
{
}
bool b3RobotSimulatorClientAPI_NoGUI::connect(int mode, const std::string& hostName, int portOrKey)
{
if (m_data->m_physicsClientHandle)
@@ -43,16 +40,15 @@ bool b3RobotSimulatorClientAPI_NoGUI::connect(int mode, const std::string& hostN
switch (mode)
{
case eCONNECT_DIRECT:
case eCONNECT_DIRECT:
{
#ifndef BT_DISABLE_PHYSICS_DIRECT
sm = b3ConnectPhysicsDirect();
#endif //BT_DISABLE_PHYSICS_DIRECT
#endif //BT_DISABLE_PHYSICS_DIRECT
break;
}
case eCONNECT_SHARED_MEMORY:
case eCONNECT_SHARED_MEMORY:
{
if (portOrKey >= 0)
{
@@ -61,7 +57,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::connect(int mode, const std::string& hostN
sm = b3ConnectSharedMemory(key);
break;
}
case eCONNECT_UDP:
case eCONNECT_UDP:
{
if (portOrKey >= 0)
{
@@ -76,7 +72,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::connect(int mode, const std::string& hostN
break;
}
case eCONNECT_TCP:
case eCONNECT_TCP:
{
if (portOrKey >= 0)
{
@@ -91,7 +87,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::connect(int mode, const std::string& hostN
break;
}
default:
default:
{
b3Warning("connectPhysicsServer unexpected argument");
}

4
examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.h
View File

@@ -3,20 +3,16 @@
#include "b3RobotSimulatorClientAPI_NoDirect.h"
///The b3RobotSimulatorClientAPI is pretty much the C++ version of pybullet
///as documented in the pybullet Quickstart Guide
///https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA
class b3RobotSimulatorClientAPI_NoGUI : public b3RobotSimulatorClientAPI_NoDirect
{
public:
b3RobotSimulatorClientAPI_NoGUI();
virtual ~b3RobotSimulatorClientAPI_NoGUI();
bool connect(int mode, const std::string& hostName = "localhost", int portOrKey = -1);
};
#endif //B3_ROBOT_SIMULATOR_CLIENT_API_H

8
examples/SharedMemory/dart/DARTPhysicsC_API.cpp
View File

@@ -4,13 +4,13 @@
#include "DARTPhysicsClient.h"
//think more about naming. The b3ConnectPhysicsLoopback
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsDART()
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsDART()
{
DARTPhysicsServerCommandProcessor* sdk = new DARTPhysicsServerCommandProcessor;
DARTPhysicsClient* direct = new DARTPhysicsClient(sdk,true);
DARTPhysicsClient* direct = new DARTPhysicsClient(sdk, true);
bool connected;
connected = direct->connect();
return (b3PhysicsClientHandle )direct;
return (b3PhysicsClientHandle)direct;
}
#endif//BT_ENABLE_DART
#endif //BT_ENABLE_DART

11
examples/SharedMemory/dart/DARTPhysicsC_API.h
View File

@@ -6,15 +6,16 @@
#include "../PhysicsClientC_API.h"
#ifdef __cplusplus
extern "C" {
extern "C"
{
#endif
//think more about naming. The b3ConnectPhysicsLoopback
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsDART();
//think more about naming. The b3ConnectPhysicsLoopback
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsDART();
#ifdef __cplusplus
}
#endif
#endif //BT_ENABLE_DART
#endif //DART_PHYSICS_C_API_H
#endif //BT_ENABLE_DART
#endif //DART_PHYSICS_C_API_H

1412
examples/SharedMemory/dart/DARTPhysicsClient.cpp
View File

File diff suppressed because it is too large Load Diff

79
examples/SharedMemory/dart/DARTPhysicsClient.h
View File

@@ -1,27 +1,25 @@
#ifndef DART_PHYSICS_CLIENT_H
#define DART_PHYSICS_CLIENT_H
#include "../PhysicsClient.h"
///PhysicsDirect executes the commands directly, without transporting them or having a separate server executing commands
class DARTPhysicsClient : public PhysicsClient
class DARTPhysicsClient : public PhysicsClient
{
protected:
struct DARTPhysicsDirectInternalData* m_data;
bool processDebugLines(const struct SharedMemoryCommand& orgCommand);
bool processCamera(const struct SharedMemoryCommand& orgCommand);
bool processContactPointData(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 processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
void processAddUserData(const struct SharedMemoryStatus& serverCmd);
@@ -32,28 +30,27 @@ protected:
void removeCachedBody(int bodyUniqueId);
public:
DARTPhysicsClient(class PhysicsCommandProcessorInterface* physSdk, bool passSdkOwnership);
virtual ~DARTPhysicsClient();
virtual ~DARTPhysicsClient();
// return true if connection succesfull, can also check 'isConnected'
//it is OK to pass a null pointer for the gui helper
virtual bool connect();
virtual bool connect();
////todo: rename to 'disconnect'
virtual void disconnectSharedMemory();
virtual void disconnectSharedMemory();
virtual bool isConnected() const;
virtual bool isConnected() const;
// return non-null if there is a status, nullptr otherwise
virtual const SharedMemoryStatus* processServerStatus();
// return non-null if there is a status, nullptr otherwise
virtual const SharedMemoryStatus* processServerStatus();
virtual SharedMemoryCommand* getAvailableSharedMemoryCommand();
virtual SharedMemoryCommand* getAvailableSharedMemoryCommand();
virtual bool canSubmitCommand() const;
virtual bool canSubmitCommand() const;
virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
virtual int getNumBodies() const;
@@ -61,37 +58,37 @@ public:
virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const;
virtual int getNumJoints(int bodyIndex) const;
virtual int getNumJoints(int bodyIndex) const;
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) 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 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);
///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 int getNumDebugLines() const;
virtual const float* getDebugLinesFrom() const;
virtual const float* getDebugLinesTo() const;
virtual const float* getDebugLinesColor() 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 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);
@@ -112,13 +109,13 @@ public:
virtual void setTimeOut(double timeOutInSeconds);
virtual double getTimeOut() const;
virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const;
virtual int getNumUserData(int bodyUniqueId, int linkIndex) const;
virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const;
virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue& valueOut) const;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char* key) const;
virtual int getNumUserData(int bodyUniqueId, int linkIndex) const;
virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char** keyOut, int* userDataIdOut) const;
virtual void pushProfileTiming(const char* timingName);
virtual void popProfileTiming();
};
#endif //DART_PHYSICS__H
#endif //DART_PHYSICS__H

8
examples/SharedMemory/dart/DARTPhysicsServerCommandProcessor.cpp
View File

@@ -1,13 +1,11 @@
#include "DARTPhysicsServerCommandProcessor.h"
DARTPhysicsServerCommandProcessor::DARTPhysicsServerCommandProcessor()
{
}
DARTPhysicsServerCommandProcessor::~DARTPhysicsServerCommandProcessor()
{
}
bool DARTPhysicsServerCommandProcessor::connect()
@@ -17,7 +15,6 @@ bool DARTPhysicsServerCommandProcessor::connect()
void DARTPhysicsServerCommandProcessor::disconnect()
{
}
bool DARTPhysicsServerCommandProcessor::isConnected() const
@@ -34,6 +31,3 @@ bool DARTPhysicsServerCommandProcessor::receiveStatus(struct SharedMemoryStatus&
{
return false;
}

14
examples/SharedMemory/dart/DARTPhysicsServerCommandProcessor.h
View File

@@ -5,10 +5,9 @@
class DARTPhysicsServerCommandProcessor : public PhysicsCommandProcessorInterface
{
public:
DARTPhysicsServerCommandProcessor();
virtual ~DARTPhysicsServerCommandProcessor();
virtual bool connect();
@@ -21,11 +20,10 @@ public:
virtual bool receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
virtual void renderScene(int renderFlags){}
virtual void physicsDebugDraw(int debugDrawFlags){}
virtual void setGuiHelper(struct GUIHelperInterface* guiHelper){}
virtual void setTimeOut(double timeOutInSeconds){}
virtual void renderScene(int renderFlags) {}
virtual void physicsDebugDraw(int debugDrawFlags) {}
virtual void setGuiHelper(struct GUIHelperInterface* guiHelper) {}
virtual void setTimeOut(double timeOutInSeconds) {}
};
#endif //DART_PHYSICS_COMMAND_PROCESSOR_H
#endif //DART_PHYSICS_COMMAND_PROCESSOR_H

1960
examples/SharedMemory/grpc/ConvertGRPCBullet.cpp
View File

File diff suppressed because it is too large Load Diff

8
examples/SharedMemory/grpc/ConvertGRPCBullet.h
View File

@@ -6,9 +6,9 @@
namespace pybullet_grpc
{
class PyBulletCommand;
class PyBulletStatus;
};
class PyBulletCommand;
class PyBulletStatus;
}; // namespace pybullet_grpc
struct SharedMemoryCommand* convertGRPCToBulletCommand(const pybullet_grpc::PyBulletCommand& grpcCommand, struct SharedMemoryCommand& cmd);
@@ -18,4 +18,4 @@ bool convertGRPCToStatus(const pybullet_grpc::PyBulletStatus& grpcReply, struct
bool convertStatusToGRPC(const struct SharedMemoryStatus& serverStatus, char* bufferServerToClient, int bufferSizeInBytes, pybullet_grpc::PyBulletStatus& grpcReply);
#endif //BT_CONVERT_GRPC_BULLET_H
#endif //BT_CONVERT_GRPC_BULLET_H

87
examples/SharedMemory/grpc/main.cpp
View File

@@ -12,14 +12,13 @@ typedef PhysicsServerCommandProcessor MyCommandProcessor;
#else
#include "SharedMemoryCommandProcessor.h"
typedef SharedMemoryCommandProcessor MyCommandProcessor;
#endif //NO_SHARED_MEMORY
#endif //NO_SHARED_MEMORY
#include "SharedMemoryCommands.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "PhysicsServerCommandProcessor.h"
#include "../Utils/b3Clock.h"
#include <memory>
#include <iostream>
#include <string>
@@ -30,32 +29,31 @@ typedef SharedMemoryCommandProcessor MyCommandProcessor;
#include "SharedMemory/grpc/proto/pybullet.grpc.pb.h"
using grpc::Server;
using grpc::ServerAsyncResponseWriter;
using grpc::ServerBuilder;
using grpc::ServerContext;
using grpc::ServerCompletionQueue;
using grpc::ServerContext;
using grpc::Status;
using pybullet_grpc::PyBulletAPI;
using pybullet_grpc::PyBulletCommand;
using pybullet_grpc::PyBulletStatus;
using pybullet_grpc::PyBulletAPI;
bool gVerboseNetworkMessagesServer = true;
#include "ConvertGRPCBullet.h"
class ServerImpl final {
class ServerImpl final
{
public:
~ServerImpl() {
~ServerImpl()
{
server_->Shutdown();
// Always shutdown the completion queue after the server.
cq_->Shutdown();
}
void Run(MyCommandProcessor* comProc, const std::string& hostNamePort) {
void Run(MyCommandProcessor* comProc, const std::string& hostNamePort)
{
ServerBuilder builder;
// Listen on the given address without any authentication mechanism.
builder.AddListeningPort(hostNamePort, grpc::InsecureServerCredentials());
@@ -75,21 +73,31 @@ public:
private:
// Class encompasing the state and logic needed to serve a request.
class CallData {
class CallData
{
public:
// Take in the "service" instance (in this case representing an asynchronous
// server) and the completion queue "cq" used for asynchronous communication
// with the gRPC runtime.
CallData(PyBulletAPI::AsyncService* service, ServerCompletionQueue* cq, MyCommandProcessor* comProc)
: service_(service), cq_(cq), responder_(&ctx_), status_(CREATE) , m_finished(false), m_comProc(comProc){
: service_(service), cq_(cq), responder_(&ctx_), status_(CREATE), m_finished(false), m_comProc(comProc)
{
// Invoke the serving logic right away.
Proceed();
}
enum CallStatus { CREATE, PROCESS, FINISH, TERMINATE };
enum CallStatus
{
CREATE,
PROCESS,
FINISH,
TERMINATE
};
CallStatus Proceed() {
if (status_ == CREATE) {
CallStatus Proceed()
{
if (status_ == CREATE)
{
// Make this instance progress to the PROCESS state.
status_ = PROCESS;
@@ -98,12 +106,12 @@ private:
// the tag uniquely identifying the request (so that different CallData
// instances can serve different requests concurrently), in this case
// the memory address of this CallData instance.
service_->RequestSubmitCommand(&ctx_, &m_command, &responder_, cq_, cq_,
this);
this);
}
else if (status_ == PROCESS) {
else if (status_ == PROCESS)
{
// Spawn a new CallData instance to serve new clients while we process
// the one for this CallData. The instance will deallocate itself as
// part of its FINISH state.
@@ -118,10 +126,9 @@ private:
buffer.resize(SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
SharedMemoryCommand cmd;
SharedMemoryCommand* cmdPtr = 0;
m_status.set_statustype(CMD_UNKNOWN_COMMAND_FLUSHED);
if (m_command.has_checkversioncommand())
{
m_status.set_statustype(CMD_CLIENT_COMMAND_COMPLETED);
@@ -131,7 +138,6 @@ private:
{
cmdPtr = convertGRPCToBulletCommand(m_command, cmd);
if (cmdPtr)
{
bool hasStatus = m_comProc->processCommand(*cmdPtr, serverStatus, &buffer[0], buffer.size());
@@ -165,14 +171,15 @@ private:
status_ = TERMINATE;
}
}
// And we are done! Let the gRPC runtime know we've finished, using the
// memory address of this instance as the uniquely identifying tag for
// the event.
responder_.Finish(m_status, Status::OK, this);
}
else {
else
{
GPR_ASSERT(status_ == FINISH);
// Once in the FINISH state, deallocate ourselves (CallData).
delete this;
@@ -200,16 +207,17 @@ private:
ServerAsyncResponseWriter<PyBulletStatus> responder_;
// Let's implement a tiny state machine with the following states.
CallStatus status_; // The current serving state.
bool m_finished;
MyCommandProcessor* m_comProc; //physics server command processor
MyCommandProcessor* m_comProc; //physics server command processor
};
// This can be run in multiple threads if needed.
void HandleRpcs(MyCommandProcessor* comProc) {
void HandleRpcs(MyCommandProcessor* comProc)
{
// Spawn a new CallData instance to serve new clients.
new CallData(&service_, cq_.get(), comProc);
void* tag; // uniquely identifies a request.
@@ -218,13 +226,14 @@ private:
CallData::CallStatus status = CallData::CallStatus::CREATE;
while (status!= CallData::CallStatus::TERMINATE) {
while (status != CallData::CallStatus::TERMINATE)
{
// Block waiting to read the next event from the completion queue. The
// event is uniquely identified by its tag, which in this case is the
// memory address of a CallData instance.
// The return value of Next should always be checked. This return value
// tells us whether there is any kind of event or cq_ is shutting down.
grpc::CompletionQueue::NextStatus nextStatus = cq_->AsyncNext(&tag, &ok, gpr_now(GPR_CLOCK_MONOTONIC));
if (nextStatus == grpc::CompletionQueue::NextStatus::GOT_EVENT)
{
@@ -235,15 +244,13 @@ private:
}
}
std::unique_ptr<ServerCompletionQueue> cq_;
PyBulletAPI::AsyncService service_;
std::unique_ptr<Server> server_;
};
int main(int argc, char** argv)
int main(int argc, char** argv)
{
b3CommandLineArgs parseArgs(argc, argv);
b3Clock clock;
double timeOutInSeconds = 10;
@@ -256,7 +263,7 @@ int main(int argc, char** argv)
parseArgs.GetCmdLineArgument("port", port);
std::string hostName = "localhost";
std::string hostNamePort = hostName;
if (port>=0)
if (port >= 0)
{
hostNamePort += ":" + std::to_string(port);
}
@@ -269,7 +276,7 @@ int main(int argc, char** argv)
{
sm->setSharedMemoryKey(key);
}
#endif//NO_SHARED_MEMORY
#endif //NO_SHARED_MEMORY
bool isPhysicsClientConnected = sm->connect();
bool exitRequested = false;
@@ -289,5 +296,3 @@ int main(int argc, char** argv)
return 0;
}

72
examples/SharedMemory/main.cpp
View File

@@ -13,8 +13,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "PhysicsServerExampleBullet2.h"
#include "Bullet3Common/b3CommandLineArgs.h"
@@ -23,12 +21,11 @@ subject to the following restrictions:
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "SharedMemoryCommon.h"
#include <stdlib.h>
int gSharedMemoryKey = -1;
static SharedMemoryCommon* example = NULL;
static SharedMemoryCommon* example = NULL;
static bool interrupted = false;
#ifndef _WIN32
@@ -37,57 +34,57 @@ static bool interrupted = false;
#include <unistd.h>
static void cleanup(int signo)
{
if (interrupted) { // this is the second time, we're hanging somewhere
// if (example) {
// example->abort();
// }
b3Printf("Aborting and deleting SharedMemoryCommon object");
sleep(1);
delete example;
errx(EXIT_FAILURE, "aborted example on signal %d", signo);
}
interrupted = true;
warnx("caught signal %d", signo);
if (interrupted)
{ // this is the second time, we're hanging somewhere
// if (example) {
// example->abort();
// }
b3Printf("Aborting and deleting SharedMemoryCommon object");
sleep(1);
delete example;
errx(EXIT_FAILURE, "aborted example on signal %d", signo);
}
interrupted = true;
warnx("caught signal %d", signo);
}
#endif//_WIN32
#endif //_WIN32
int main(int argc, char* argv[])
{
#ifndef _WIN32
struct sigaction action;
memset(&action, 0x0, sizeof(action));
action.sa_handler = cleanup;
static const int signos[] = { SIGHUP, SIGINT, SIGQUIT, SIGABRT, SIGSEGV, SIGPIPE, SIGTERM };
for (int ii(0); ii < sizeof(signos) / sizeof(*signos); ++ii) {
if (0 != sigaction(signos[ii], &action, NULL)) {
err(EXIT_FAILURE, "signal %d", signos[ii]);
}
}
struct sigaction action;
memset(&action, 0x0, sizeof(action));
action.sa_handler = cleanup;
static const int signos[] = {SIGHUP, SIGINT, SIGQUIT, SIGABRT, SIGSEGV, SIGPIPE, SIGTERM};
for (int ii(0); ii < sizeof(signos) / sizeof(*signos); ++ii)
{
if (0 != sigaction(signos[ii], &action, NULL))
{
err(EXIT_FAILURE, "signal %d", signos[ii]);
}
}
#endif
b3CommandLineArgs args(argc, argv);
DummyGUIHelper noGfx;
CommonExampleOptions options(&noGfx);
args.GetCmdLineArgument("shared_memory_key", gSharedMemoryKey);
args.GetCmdLineArgument("sharedMemoryKey", gSharedMemoryKey);
// options.m_option |= PHYSICS_SERVER_ENABLE_COMMAND_LOGGING;
// options.m_option |= PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG;
// options.m_option |= PHYSICS_SERVER_ENABLE_COMMAND_LOGGING;
// options.m_option |= PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG;
example = (SharedMemoryCommon*)PhysicsServerCreateFuncBullet2(options);
example->initPhysics();
while (example->isConnected() && !(example->wantsTermination() || interrupted))
{
example->stepSimulation(1.f/60.f);
}
example->stepSimulation(1.f / 60.f);
}
example->exitPhysics();
@@ -95,4 +92,3 @@ int main(int argc, char* argv[])
return 0;
}

8
examples/SharedMemory/mujoco/MuJoCoPhysicsC_API.cpp
View File

@@ -3,13 +3,13 @@
#include "MuJoCoPhysicsServerCommandProcessor.h"
#include "MuJoCoPhysicsClient.h"
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsMuJoCo()
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsMuJoCo()
{
MuJoCoPhysicsServerCommandProcessor* sdk = new MuJoCoPhysicsServerCommandProcessor;
MuJoCoPhysicsClient* direct = new MuJoCoPhysicsClient(sdk,true);
MuJoCoPhysicsClient* direct = new MuJoCoPhysicsClient(sdk, true);
bool connected;
connected = direct->connect();
return (b3PhysicsClientHandle )direct;
return (b3PhysicsClientHandle)direct;
}
#endif//BT_ENABLE_MUJOCO
#endif //BT_ENABLE_MUJOCO

11
examples/SharedMemory/mujoco/MuJoCoPhysicsC_API.h
View File

@@ -6,15 +6,16 @@
#include "../PhysicsClientC_API.h"
#ifdef __cplusplus
extern "C" {
extern "C"
{
#endif
//think more about naming. The b3ConnectPhysicsLoopback
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsMuJoCo();
//think more about naming. The b3ConnectPhysicsLoopback
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsMuJoCo();
#ifdef __cplusplus
}
#endif
#endif //BT_ENABLE_MUJOCO
#endif //MUJOCO_PHYSICS_C_API_H
#endif //BT_ENABLE_MUJOCO
#endif //MUJOCO_PHYSICS_C_API_H

1413
examples/SharedMemory/mujoco/MuJoCoPhysicsClient.cpp
View File

File diff suppressed because it is too large Load Diff

78
examples/SharedMemory/mujoco/MuJoCoPhysicsClient.h
View File

@@ -4,23 +4,22 @@
#include "../PhysicsClient.h"
///PhysicsDirect executes the commands directly, without transporting them or having a separate server executing commands
class MuJoCoPhysicsClient : public PhysicsClient
class MuJoCoPhysicsClient : public PhysicsClient
{
protected:
struct MuJoCoPhysicsDirectInternalData* m_data;
bool processDebugLines(const struct SharedMemoryCommand& orgCommand);
bool processCamera(const struct SharedMemoryCommand& orgCommand);
bool processContactPointData(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 processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
void processAddUserData(const struct SharedMemoryStatus& serverCmd);
@@ -31,28 +30,27 @@ protected:
void removeCachedBody(int bodyUniqueId);
public:
MuJoCoPhysicsClient(class PhysicsCommandProcessorInterface* physSdk, bool passSdkOwnership);
virtual ~MuJoCoPhysicsClient();
virtual ~MuJoCoPhysicsClient();
// return true if connection succesfull, can also check 'isConnected'
//it is OK to pass a null pointer for the gui helper
virtual bool connect();
virtual bool connect();
////todo: rename to 'disconnect'
virtual void disconnectSharedMemory();
virtual void disconnectSharedMemory();
virtual bool isConnected() const;
virtual bool isConnected() const;
// return non-null if there is a status, nullptr otherwise
virtual const SharedMemoryStatus* processServerStatus();
// return non-null if there is a status, nullptr otherwise
virtual const SharedMemoryStatus* processServerStatus();
virtual SharedMemoryCommand* getAvailableSharedMemoryCommand();
virtual SharedMemoryCommand* getAvailableSharedMemoryCommand();
virtual bool canSubmitCommand() const;
virtual bool canSubmitCommand() const;
virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
virtual int getNumBodies() const;
@@ -60,37 +58,37 @@ public:
virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const;
virtual int getNumJoints(int bodyIndex) const;
virtual int getNumJoints(int bodyIndex) const;
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) 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 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);
///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 int getNumDebugLines() const;
virtual const float* getDebugLinesFrom() const;
virtual const float* getDebugLinesTo() const;
virtual const float* getDebugLinesColor() 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 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);
@@ -111,13 +109,13 @@ public:
virtual void setTimeOut(double timeOutInSeconds);
virtual double getTimeOut() const;
virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const;
virtual int getNumUserData(int bodyUniqueId, int linkIndex) const;
virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const;
virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue& valueOut) const;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char* key) const;
virtual int getNumUserData(int bodyUniqueId, int linkIndex) const;
virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char** keyOut, int* userDataIdOut) const;
virtual void pushProfileTiming(const char* timingName);
virtual void popProfileTiming();
};
#endif //MUJOCO_PHYSICS_CLIENT_H
#endif //MUJOCO_PHYSICS_CLIENT_H

278
examples/SharedMemory/mujoco/MuJoCoPhysicsServerCommandProcessor.cpp
View File

@@ -9,7 +9,6 @@
#include "Bullet3Common/b3FileUtils.h"
#include "../../Utils/b3ResourcePath.h"
struct MuJoCoPhysicsServerCommandProcessorInternalData
{
bool m_isConnected;
@@ -19,15 +18,15 @@ struct MuJoCoPhysicsServerCommandProcessorInternalData
mjModel* m_mujocoModel;
mjData* m_mujocoData;
b3AlignedObjectArray<int> m_mjcfRecentLoadedBodies;
MuJoCoPhysicsServerCommandProcessorInternalData()
:m_isConnected(false),
m_verboseOutput(false),
m_mujocoModel(0),
m_mujocoData(0),
m_physicsDeltaTime(1./240.),
m_numSimulationSubSteps(0)
: m_isConnected(false),
m_verboseOutput(false),
m_mujocoModel(0),
m_mujocoData(0),
m_physicsDeltaTime(1. / 240.),
m_numSimulationSubSteps(0)
{
}
};
@@ -36,11 +35,10 @@ MuJoCoPhysicsServerCommandProcessor::MuJoCoPhysicsServerCommandProcessor()
{
m_data = new MuJoCoPhysicsServerCommandProcessorInternalData;
}
MuJoCoPhysicsServerCommandProcessor::~MuJoCoPhysicsServerCommandProcessor()
{
delete m_data;
}
bool MuJoCoPhysicsServerCommandProcessor::connect()
@@ -51,13 +49,13 @@ bool MuJoCoPhysicsServerCommandProcessor::connect()
return true;
}
printf("MuJoCo Pro library version %.2lf\n", 0.01*mj_version());
if( mjVERSION_HEADER!=mj_version() )
mju_error("Headers and library have different versions");
printf("MuJoCo Pro library version %.2lf\n", 0.01 * mj_version());
if (mjVERSION_HEADER != mj_version())
mju_error("Headers and library have different versions");
// activate MuJoCo license
int result = mj_activate("mjkey.txt");
if (result==1)
// activate MuJoCo license
int result = mj_activate("mjkey.txt");
if (result == 1)
{
m_data->m_isConnected = true;
return true;
@@ -71,7 +69,7 @@ void MuJoCoPhysicsServerCommandProcessor::resetSimulation()
if (m_data->m_mujocoModel)
{
mj_deleteModel(m_data->m_mujocoModel);
m_data->m_mujocoModel=0;
m_data->m_mujocoModel = 0;
}
if (m_data->m_mujocoData)
{
@@ -83,7 +81,7 @@ void MuJoCoPhysicsServerCommandProcessor::resetSimulation()
void MuJoCoPhysicsServerCommandProcessor::disconnect()
{
resetSimulation();
m_data->m_isConnected = false;
}
@@ -101,7 +99,6 @@ bool MuJoCoPhysicsServerCommandProcessor::processCommand(const struct SharedMemo
bool hasStatus = false;
serverStatusOut.m_type = CMD_INVALID_STATUS;
serverStatusOut.m_numDataStreamBytes = 0;
serverStatusOut.m_dataStream = 0;
@@ -110,61 +107,60 @@ bool MuJoCoPhysicsServerCommandProcessor::processCommand(const struct SharedMemo
switch (clientCmd.m_type)
{
case CMD_REQUEST_INTERNAL_DATA:
{
hasStatus = processRequestInternalDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
};
case CMD_SYNC_BODY_INFO:
{
hasStatus = processSyncBodyInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_SYNC_USER_DATA:
{
hasStatus = processSyncUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_LOAD_MJCF:
{
hasStatus = processLoadMJCFCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_REQUEST_BODY_INFO:
{
hasStatus = processRequestBodyInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_STEP_FORWARD_SIMULATION:
{
hasStatus = processForwardDynamicsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
hasStatus = processRequestInternalDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
};
case CMD_SYNC_BODY_INFO:
{
hasStatus = processSyncBodyInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_SYNC_USER_DATA:
{
hasStatus = processSyncUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_LOAD_MJCF:
{
hasStatus = processLoadMJCFCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_REQUEST_BODY_INFO:
{
hasStatus = processRequestBodyInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_STEP_FORWARD_SIMULATION:
{
hasStatus = processForwardDynamicsCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS:
{
hasStatus = processSendPhysicsParametersCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
hasStatus = processSendPhysicsParametersCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
};
case CMD_REQUEST_ACTUAL_STATE:
{
hasStatus = processRequestActualStateCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_RESET_SIMULATION:
{
hasStatus = processResetSimulationCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
hasStatus = processRequestActualStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
default:
case CMD_RESET_SIMULATION:
{
hasStatus = processResetSimulationCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
default:
{
BT_PROFILE("CMD_UNKNOWN");
printf("Unknown command encountered: %d",clientCmd.m_type);
SharedMemoryStatus& serverCmd =serverStatusOut;
printf("Unknown command encountered: %d", clientCmd.m_type);
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_UNKNOWN_COMMAND_FLUSHED;
hasStatus = true;
}
@@ -467,7 +463,6 @@ bool MuJoCoPhysicsServerCommandProcessor::processCommand(const struct SharedMemo
break;
}
#endif
};
return hasStatus;
@@ -483,8 +478,6 @@ bool MuJoCoPhysicsServerCommandProcessor::processRequestInternalDataCommand(cons
return hasStatus;
}
bool MuJoCoPhysicsServerCommandProcessor::processSyncBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
@@ -513,37 +506,39 @@ bool MuJoCoPhysicsServerCommandProcessor::processLoadMJCFCommand(const struct Sh
BT_PROFILE("CMD_LOAD_MJCF");
serverStatusOut.m_type = CMD_MJCF_LOADING_FAILED;
const MjcfArgs& mjcfArgs = clientCmd.m_mjcfArguments;
if (m_data->m_verboseOutput)
{
printf("Processed CMD_LOAD_MJCF:%s", mjcfArgs.m_mjcfFileName);
}
bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (mjcfArgs.m_useMultiBody!=0) : true;
if (m_data->m_verboseOutput)
{
printf("Processed CMD_LOAD_MJCF:%s", mjcfArgs.m_mjcfFileName);
}
bool useMultiBody = (clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (mjcfArgs.m_useMultiBody != 0) : true;
int flags = 0;
if (clientCmd.m_updateFlags&URDF_ARGS_HAS_CUSTOM_URDF_FLAGS)
if (clientCmd.m_updateFlags & URDF_ARGS_HAS_CUSTOM_URDF_FLAGS)
{
flags |= clientCmd.m_mjcfArguments.m_flags;
}
const char* fileName = mjcfArgs.m_mjcfFileName;
if (strlen(fileName)>0)
if (strlen(fileName) > 0)
{
char relativeFileName[1024];
b3FileUtils fu;
//bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
bool fileFound = (b3ResourcePath::findResourcePath(fileName,relativeFileName,1024)>0);
if (!fileFound){
bool fileFound = (b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024) > 0);
if (!fileFound)
{
printf("MJCF file not found: %s\n", fileName);
} else
}
else
{
int maxPathLen = 1024;
char pathPrefix[1024];
fu.extractPath(relativeFileName,pathPrefix,maxPathLen);
fu.extractPath(relativeFileName, pathPrefix, maxPathLen);
{
char error[1000] = "could not load binary model";
mjModel* mnew = 0;
if( strlen(relativeFileName)>4 && !strcmp(relativeFileName+strlen(relativeFileName)-4, ".mjb") )
char error[1000] = "could not load binary model";
mjModel* mnew = 0;
if (strlen(relativeFileName) > 4 && !strcmp(relativeFileName + strlen(relativeFileName) - 4, ".mjb"))
{
mnew = mj_loadModel(relativeFileName, 0);
}
@@ -566,34 +561,29 @@ bool MuJoCoPhysicsServerCommandProcessor::processLoadMJCFCommand(const struct Sh
mj_forward(m_data->m_mujocoModel, m_data->m_mujocoData);
}
}
if( !mnew )
if (!mnew)
{
printf("%s\n", error);
} else
}
else
{
int maxBodies = btMin(MAX_SDF_BODIES, mnew->nbody);
serverStatusOut.m_sdfLoadedArgs.m_numBodies = maxBodies;
for (int i=0;i<maxBodies;i++)
for (int i = 0; i < maxBodies; i++)
{
serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[i]=i;
serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[i] = i;
}
serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0;
serverStatusOut.m_type = CMD_MJCF_LOADING_COMPLETED;
}
}
}
}
return hasStatus;
}
bool MuJoCoPhysicsServerCommandProcessor::processRequestBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
@@ -601,31 +591,29 @@ bool MuJoCoPhysicsServerCommandProcessor::processRequestBodyInfoCommand(const st
const SdfRequestInfoArgs& sdfInfoArgs = clientCmd.m_sdfRequestInfoArgs;
//stream info into memory
int streamSizeInBytes = 0;//createBodyInfoStream(sdfInfoArgs.m_bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
int streamSizeInBytes = 0; //createBodyInfoStream(sdfInfoArgs.m_bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
serverStatusOut.m_type = CMD_BODY_INFO_COMPLETED;
serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = sdfInfoArgs.m_bodyUniqueId;
serverStatusOut.m_dataStreamArguments.m_bodyName[0] = 0;
if (m_data->m_mujocoModel && sdfInfoArgs.m_bodyUniqueId>=0 && sdfInfoArgs.m_bodyUniqueId<m_data->m_mujocoModel->nbody)
if (m_data->m_mujocoModel && sdfInfoArgs.m_bodyUniqueId >= 0 && sdfInfoArgs.m_bodyUniqueId < m_data->m_mujocoModel->nbody)
{
const char* name = m_data->m_mujocoModel->names+m_data->m_mujocoModel->name_bodyadr[sdfInfoArgs.m_bodyUniqueId];
strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName,name);
const char* name = m_data->m_mujocoModel->names + m_data->m_mujocoModel->name_bodyadr[sdfInfoArgs.m_bodyUniqueId];
strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, name);
}
serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
return hasStatus;
}
bool MuJoCoPhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
BT_PROFILE("CMD_STEP_FORWARD_SIMULATION");
if (m_data->m_mujocoModel)
{
if (m_data->m_verboseOutput)
@@ -633,39 +621,37 @@ bool MuJoCoPhysicsServerCommandProcessor::processForwardDynamicsCommand(const st
b3Printf("Step simulation request");
b3Printf("CMD_STEP_FORWARD_SIMULATION clientCmd = %d\n", clientCmd.m_sequenceNumber);
}
btScalar deltaTimeScaled = m_data->m_physicsDeltaTime;
if (m_data->m_numSimulationSubSteps > 0)
{
for (int i=0;i<m_data->m_numSimulationSubSteps;i++)
for (int i = 0; i < m_data->m_numSimulationSubSteps; i++)
{
m_data->m_mujocoModel->opt.timestep = m_data->m_physicsDeltaTime/m_data->m_numSimulationSubSteps;
mj_step(m_data->m_mujocoModel,m_data->m_mujocoData);
mj_forward(m_data->m_mujocoModel,m_data->m_mujocoData);
m_data->m_mujocoModel->opt.timestep = m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps;
mj_step(m_data->m_mujocoModel, m_data->m_mujocoData);
mj_forward(m_data->m_mujocoModel, m_data->m_mujocoData);
}
}
else
{
m_data->m_mujocoModel->opt.timestep = m_data->m_physicsDeltaTime;
mj_step(m_data->m_mujocoModel,m_data->m_mujocoData);
mj_forward(m_data->m_mujocoModel,m_data->m_mujocoData);
mj_step(m_data->m_mujocoModel, m_data->m_mujocoData);
mj_forward(m_data->m_mujocoModel, m_data->m_mujocoData);
}
}
SharedMemoryStatus& serverCmd =serverStatusOut;
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED;
return hasStatus;
}
bool MuJoCoPhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
BT_PROFILE("CMD_SEND_PHYSICS_SIMULATION_PARAMETERS");
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DELTA_TIME)
if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_DELTA_TIME)
{
m_data->m_physicsDeltaTime = clientCmd.m_physSimParamArgs.m_deltaTime;
}
@@ -718,7 +704,7 @@ bool MuJoCoPhysicsServerCommandProcessor::processSendPhysicsParametersCommand(co
this->m_data->m_dynamicsWorld->setGravity(grav);
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
m_data->m_dynamicsWorld->getWorldInfo().m_gravity=grav;
#endif
if (m_data->m_verboseOutput)
{
@@ -812,7 +798,7 @@ bool MuJoCoPhysicsServerCommandProcessor::processSendPhysicsParametersCommand(co
#endif
SharedMemoryStatus& serverCmd =serverStatusOut;
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
return hasStatus;
}
@@ -821,7 +807,7 @@ bool MuJoCoPhysicsServerCommandProcessor::processRequestActualStateCommand(const
{
bool hasStatus = true;
serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED;
if (m_data->m_mujocoModel)
{
BT_PROFILE("CMD_REQUEST_ACTUAL_STATE");
@@ -831,26 +817,26 @@ bool MuJoCoPhysicsServerCommandProcessor::processRequestActualStateCommand(const
}
int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId;
if (bodyUniqueId>=0 && bodyUniqueId<m_data->m_mujocoModel->nbody)
if (bodyUniqueId >= 0 && bodyUniqueId < m_data->m_mujocoModel->nbody)
{
SharedMemoryStatus& serverCmd = serverStatusOut;
serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED;
serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId;
serverCmd.m_sendActualStateArgs.m_numLinks = 0;//todo body->m_multiBody->getNumLinks();
serverCmd.m_sendActualStateArgs.m_numLinks = 0; //todo body->m_multiBody->getNumLinks();
int totalDegreeOfFreedomQ = 0;
int totalDegreeOfFreedomU = 0;
if (serverCmd.m_sendActualStateArgs.m_numLinks>= MAX_DEGREE_OF_FREEDOM)
if (serverCmd.m_sendActualStateArgs.m_numLinks >= MAX_DEGREE_OF_FREEDOM)
{
serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED;
hasStatus = true;
return hasStatus;
}
bool computeForwardKinematics = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS)!=0);
bool computeLinkVelocities = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_LINKVELOCITY)!=0);
bool computeForwardKinematics = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS) != 0);
bool computeLinkVelocities = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_LINKVELOCITY) != 0);
if (computeForwardKinematics || computeLinkVelocities)
{
@@ -862,18 +848,18 @@ bool MuJoCoPhysicsServerCommandProcessor::processRequestActualStateCommand(const
//so that we can easily move the 'fixed' base when needed
//do we don't use this conditional "if (!mb->hasFixedBase())"
{
int rootLink = 0;//todo check
int type=(m_data->m_mujocoModel->jnt_type+m_data->m_mujocoModel->body_jntnum[bodyUniqueId])[rootLink];
int rootLink = 0; //todo check
int type = (m_data->m_mujocoModel->jnt_type + m_data->m_mujocoModel->body_jntnum[bodyUniqueId])[rootLink];
//assume mjJNT_FREE?
int qposAdr = (m_data->m_mujocoModel->jnt_qposadr+m_data->m_mujocoModel->body_jntnum[bodyUniqueId])[rootLink];
mjtNum* pos = m_data->m_mujocoData->xipos+bodyUniqueId*3;
int qposAdr = (m_data->m_mujocoModel->jnt_qposadr + m_data->m_mujocoModel->body_jntnum[bodyUniqueId])[rootLink];
mjtNum* pos = m_data->m_mujocoData->xipos + bodyUniqueId * 3;
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] = 0;
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] = 0;
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[2] = 0;
mjtNum* orn= m_data->m_mujocoData->xquat+bodyUniqueId*4;
mjtNum* cvel=m_data->m_mujocoData->cvel+bodyUniqueId*6;
mjtNum* orn = m_data->m_mujocoData->xquat + bodyUniqueId * 4;
mjtNum* cvel = m_data->m_mujocoData->cvel + bodyUniqueId * 6;
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[3] = 0;
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[4] = 0;
@@ -890,31 +876,30 @@ bool MuJoCoPhysicsServerCommandProcessor::processRequestActualStateCommand(const
serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = orn[1];
serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = orn[2];
serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = orn[3];
totalDegreeOfFreedomQ +=7;//pos + quaternion
totalDegreeOfFreedomQ += 7; //pos + quaternion
//base linear velocity (in world space, carthesian)
serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = cvel[3];//mb->getBaseVel()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = cvel[4];//mb->getBaseVel()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = cvel[5];//mb->getBaseVel()[2];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = cvel[3]; //mb->getBaseVel()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = cvel[4]; //mb->getBaseVel()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = cvel[5]; //mb->getBaseVel()[2];
//base angular velocity (in world space, carthesian)
serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = cvel[0];//mb->getBaseOmega()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = cvel[1];//mb->getBaseOmega()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = cvel[2];//mb->getBaseOmega()[2];
totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF
serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = cvel[0]; //mb->getBaseOmega()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = cvel[1]; //mb->getBaseOmega()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = cvel[2]; //mb->getBaseOmega()[2];
totalDegreeOfFreedomU += 6; //3 linear and 3 angular DOF
}
//btAlignedObjectArray<btVector3> omega;
//btAlignedObjectArray<btVector3> linVel;
int numLinks = m_data->m_mujocoModel->body_jntnum[bodyUniqueId];
for (int l=0;l<numLinks ;l++)
for (int l = 0; l < numLinks; l++)
{
int type=(m_data->m_mujocoModel->jnt_type+m_data->m_mujocoModel->body_jntnum[bodyUniqueId])[l];
int type = (m_data->m_mujocoModel->jnt_type + m_data->m_mujocoModel->body_jntnum[bodyUniqueId])[l];
//int type=(m_data->m_mujocoModel->jnt_type+m_data->m_mujocoModel->body_jntnum[bodyUniqueId])[l];
#if 0
#if 0
mjtNum* xpos =
for (int d=0;d<mb->getLink(l).m_posVarCount;d++)
{
@@ -944,7 +929,7 @@ bool MuJoCoPhysicsServerCommandProcessor::processRequestActualStateCommand(const
}
serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] = 0;
#if 0
#if 0
if (supportsJointMotor(mb,l))
{
if (motor && m_data->m_physicsDeltaTime>btScalar(0))
@@ -952,7 +937,7 @@ bool MuJoCoPhysicsServerCommandProcessor::processRequestActualStateCommand(const
serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] = 0;
}
}
#endif
#endif
//btVector3 linkLocalInertialOrigin = body->m_linkLocalInertialFrames[l].getOrigin();
//btQuaternion linkLocalInertialRotation = body->m_linkLocalInertialFrames[l].getRotation();
@@ -966,9 +951,8 @@ bool MuJoCoPhysicsServerCommandProcessor::processRequestActualStateCommand(const
serverCmd.m_sendActualStateArgs.m_linkState[l*7+4] = 0;//linkCOMRotation.y();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = 0;//linkCOMRotation.z();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = 1;//linkCOMRotation.w();
#if 0
#if 0
btVector3 worldLinVel(0,0,0);
btVector3 worldAngVel(0,0,0);
@@ -978,7 +962,7 @@ bool MuJoCoPhysicsServerCommandProcessor::processRequestActualStateCommand(const
worldLinVel = linkRotMat * linVel[l+1];
worldAngVel = linkRotMat * omega[l+1];
}
#endif
#endif
serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+0] = 0;//worldLinVel[0];
serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+1] = 0;//worldLinVel[1];
serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+2] = 0;//worldLinVel[2];
@@ -994,39 +978,33 @@ bool MuJoCoPhysicsServerCommandProcessor::processRequestActualStateCommand(const
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+4] = 0;//linkLocalInertialRotation.y();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+5] = 0;//linkLocalInertialRotation.z();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+6] = 1;//linkLocalInertialRotation.w();
#endif
#endif
}
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ;
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU;
hasStatus = true;
}
}
}
return hasStatus;
}
bool MuJoCoPhysicsServerCommandProcessor::processResetSimulationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
BT_PROFILE("CMD_RESET_SIMULATION");
resetSimulation();
SharedMemoryStatus& serverCmd =serverStatusOut;
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_RESET_SIMULATION_COMPLETED;
return hasStatus;
}
bool MuJoCoPhysicsServerCommandProcessor::receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
return false;
}
#endif //BT_ENABLE_MUJOCO
#endif //BT_ENABLE_MUJOCO

14
examples/SharedMemory/mujoco/MuJoCoPhysicsServerCommandProcessor.h
View File

@@ -5,7 +5,6 @@
class MuJoCoPhysicsServerCommandProcessor : public PhysicsCommandProcessorInterface
{
struct MuJoCoPhysicsServerCommandProcessorInternalData* m_data;
bool processSyncBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
@@ -22,7 +21,7 @@ class MuJoCoPhysicsServerCommandProcessor : public PhysicsCommandProcessorInterf
public:
MuJoCoPhysicsServerCommandProcessor();
virtual ~MuJoCoPhysicsServerCommandProcessor();
virtual bool connect();
@@ -35,11 +34,10 @@ public:
virtual bool receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
virtual void renderScene(int renderFlags){}
virtual void physicsDebugDraw(int debugDrawFlags){}
virtual void setGuiHelper(struct GUIHelperInterface* guiHelper){}
virtual void setTimeOut(double timeOutInSeconds){}
virtual void renderScene(int renderFlags) {}
virtual void physicsDebugDraw(int debugDrawFlags) {}
virtual void setGuiHelper(struct GUIHelperInterface* guiHelper) {}
virtual void setTimeOut(double timeOutInSeconds) {}
};
#endif //MUJOCO_PHYSICS_COMMAND_PROCESSOR_H
#endif //MUJOCO_PHYSICS_COMMAND_PROCESSOR_H

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

@@ -3,40 +3,37 @@
#ifdef _WIN32
#define B3_SHARED_API __declspec(dllexport)
#elif defined (__GNUC__)
#elif defined(__GNUC__)
#define B3_SHARED_API __attribute__((visibility("default")))
#else
#define B3_SHARED_API
#endif
#if defined(_WIN32)
#define B3_API_ENTRY
#define B3_API_CALL __cdecl
#define B3_CALLBACK __cdecl
#define B3_API_CALL __cdecl
#define B3_CALLBACK __cdecl
#else
#define B3_API_ENTRY
#define B3_API_CALL
#define B3_CALLBACK
#endif
#ifdef __cplusplus
extern "C" {
extern "C"
{
#endif
/* Plugin API */
typedef B3_API_ENTRY int (B3_API_CALL * PFN_INIT)(struct b3PluginContext* context);
typedef B3_API_ENTRY void (B3_API_CALL * PFN_EXIT)(struct b3PluginContext* context);
typedef B3_API_ENTRY int (B3_API_CALL * PFN_EXECUTE)(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
typedef B3_API_ENTRY int (B3_API_CALL * PFN_TICK)(struct b3PluginContext* context);
typedef B3_API_ENTRY int(B3_API_CALL* PFN_INIT)(struct b3PluginContext* context);
typedef B3_API_ENTRY void(B3_API_CALL* PFN_EXIT)(struct b3PluginContext* context);
typedef B3_API_ENTRY int(B3_API_CALL* PFN_EXECUTE)(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
typedef B3_API_ENTRY int(B3_API_CALL* PFN_TICK)(struct b3PluginContext* context);
typedef B3_API_ENTRY struct UrdfRenderingInterface* (B3_API_CALL * PFN_GET_RENDER_INTERFACE)(struct b3PluginContext* context);
typedef B3_API_ENTRY struct b3PluginCollisionInterface* (B3_API_CALL * PFN_GET_COLLISION_INTERFACE)(struct b3PluginContext* context);
typedef B3_API_ENTRY struct UrdfRenderingInterface*(B3_API_CALL* PFN_GET_RENDER_INTERFACE)(struct b3PluginContext* context);
typedef B3_API_ENTRY struct b3PluginCollisionInterface*(B3_API_CALL* PFN_GET_COLLISION_INTERFACE)(struct b3PluginContext* context);
#ifdef __cplusplus
}
#endif
#endif //B3_PLUGIN_API_H
#endif //B3_PLUGIN_API_H

23
examples/SharedMemory/plugins/b3PluginCollisionInterface.h
View File

@@ -3,7 +3,7 @@
enum b3PluginCollisionFilterModes
{
B3_FILTER_GROUPAMASKB_AND_GROUPBMASKA=0,
B3_FILTER_GROUPAMASKB_AND_GROUPBMASKA = 0,
B3_FILTER_GROUPAMASKB_OR_GROUPBMASKA
};
@@ -11,23 +11,22 @@ struct b3PluginCollisionInterface
{
virtual void setBroadphaseCollisionFilter(
int objectUniqueIdA, int objectUniqueIdB,
int linkIndexA, int linkIndexB,
bool enableCollision)=0;
int linkIndexA, int linkIndexB,
bool enableCollision) = 0;
virtual void removeBroadphaseCollisionFilter(
int objectUniqueIdA, int objectUniqueIdB,
int linkIndexA, int linkIndexB)=0;
int linkIndexA, int linkIndexB) = 0;
virtual int getNumRules() const = 0;
virtual void resetAll()=0;
virtual void resetAll() = 0;
virtual int needsBroadphaseCollision(int objectUniqueIdA, int linkIndexA,
int collisionFilterGroupA,int collisionFilterMaskA,
int objectUniqueIdB, int linkIndexB,
int collisionFilterGroupB,int collisionFilterMaskB,
int filterMode
)=0;
virtual int needsBroadphaseCollision(int objectUniqueIdA, int linkIndexA,
int collisionFilterGroupA, int collisionFilterMaskA,
int objectUniqueIdB, int linkIndexB,
int collisionFilterGroupB, int collisionFilterMaskB,
int filterMode) = 0;
};
#endif //B3_PLUGIN_COLLISION_INTERFACE_H
#endif //B3_PLUGIN_COLLISION_INTERFACE_H

13
examples/SharedMemory/plugins/b3PluginContext.h
View File

@@ -5,11 +5,11 @@
struct b3PluginContext
{
b3PhysicsClientHandle m_physClient;
b3PhysicsClientHandle m_physClient;
//plugin can modify the m_userPointer to store persistent object pointer (class or struct instance etc)
void* m_userPointer;
const struct b3VRControllerEvent* m_vrControllerEvents;
int m_numVRControllerEvents;
const struct b3KeyboardEvent* m_keyEvents;
@@ -18,14 +18,9 @@ struct b3PluginContext
int m_numMouseEvents;
const struct b3Notification* m_notifications;
int m_numNotifications;
//only used for grpc/processClientCommands
class PhysicsCommandProcessorInterface* m_rpcCommandProcessorInterface;
};
#endif //B3_PLUGIN_CONTEXT_H
#endif //B3_PLUGIN_CONTEXT_H

98
examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp
View File

@@ -1,6 +1,6 @@
//tinyRendererPlugin implements the TinyRenderer as a plugin
//it is statically linked when using preprocessor #define STATIC_LINK_VR_PLUGIN
//it is statically linked when using preprocessor #define STATIC_LINK_VR_PLUGIN
//otherwise you can dynamically load it using pybullet.loadPlugin
#include "collisionFilterPlugin.h"
@@ -19,25 +19,29 @@ struct b3CustomCollisionFilter
int m_linkIndexB;
bool m_enableCollision;
B3_FORCE_INLINE unsigned int getHash()const
B3_FORCE_INLINE unsigned int getHash() const
{
int obA = (m_objectUniqueIdA&0xff);
int obB = ((m_objectUniqueIdB &0xf)<<8);
int linkA = ((m_linkIndexA&0xff)<<16);
int linkB = ((m_linkIndexB&0xff)<<24);
int key = obA+obB+linkA+linkB;
int obA = (m_objectUniqueIdA & 0xff);
int obB = ((m_objectUniqueIdB & 0xf) << 8);
int linkA = ((m_linkIndexA & 0xff) << 16);
int linkB = ((m_linkIndexB & 0xff) << 24);
int key = obA + obB + linkA + linkB;
// Thomas Wang's hash
key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16);
key += ~(key << 15);
key ^= (key >> 10);
key += (key << 3);
key ^= (key >> 6);
key += ~(key << 11);
key ^= (key >> 16);
return key;
}
bool equals(const b3CustomCollisionFilter& other) const
{
return m_objectUniqueIdA == other.m_objectUniqueIdA &&
m_objectUniqueIdB == other.m_objectUniqueIdB&&
m_linkIndexA == other.m_linkIndexA &&
m_linkIndexB == other.m_linkIndexB;
m_objectUniqueIdB == other.m_objectUniqueIdB &&
m_linkIndexA == other.m_linkIndexA &&
m_linkIndexB == other.m_linkIndexB;
}
};
struct DefaultPluginCollisionInterface : public b3PluginCollisionInterface
@@ -49,7 +53,6 @@ struct DefaultPluginCollisionInterface : public b3PluginCollisionInterface
int linkIndexA, int linkIndexB,
bool enableCollision)
{
b3CustomCollisionFilter keyValue;
keyValue.m_objectUniqueIdA = objectUniqueIdA;
keyValue.m_linkIndexA = linkIndexA;
@@ -57,22 +60,20 @@ struct DefaultPluginCollisionInterface : public b3PluginCollisionInterface
keyValue.m_linkIndexB = linkIndexB;
keyValue.m_enableCollision = enableCollision;
if (objectUniqueIdA>objectUniqueIdB)
if (objectUniqueIdA > objectUniqueIdB)
{
b3Swap(keyValue.m_objectUniqueIdA,keyValue.m_objectUniqueIdB);
b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB);
b3Swap(keyValue.m_objectUniqueIdA, keyValue.m_objectUniqueIdB);
b3Swap(keyValue.m_linkIndexA, keyValue.m_linkIndexB);
}
if (objectUniqueIdA==objectUniqueIdB)
if (objectUniqueIdA == objectUniqueIdB)
{
if (keyValue.m_linkIndexA>keyValue.m_linkIndexB)
if (keyValue.m_linkIndexA > keyValue.m_linkIndexB)
{
b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB);
b3Swap(keyValue.m_linkIndexA, keyValue.m_linkIndexB);
}
}
m_customCollisionFilters.insert(keyValue,keyValue);
m_customCollisionFilters.insert(keyValue, keyValue);
}
virtual void removeBroadphaseCollisionFilter(
@@ -84,17 +85,17 @@ struct DefaultPluginCollisionInterface : public b3PluginCollisionInterface
keyValue.m_linkIndexA = linkIndexA;
keyValue.m_objectUniqueIdB = objectUniqueIdB;
keyValue.m_linkIndexB = linkIndexB;
if (objectUniqueIdA>objectUniqueIdB)
if (objectUniqueIdA > objectUniqueIdB)
{
b3Swap(keyValue.m_objectUniqueIdA,keyValue.m_objectUniqueIdB);
b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB);
b3Swap(keyValue.m_objectUniqueIdA, keyValue.m_objectUniqueIdB);
b3Swap(keyValue.m_linkIndexA, keyValue.m_linkIndexB);
}
if (objectUniqueIdA==objectUniqueIdB)
if (objectUniqueIdA == objectUniqueIdB)
{
if (keyValue.m_linkIndexA>keyValue.m_linkIndexB)
if (keyValue.m_linkIndexA > keyValue.m_linkIndexB)
{
b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB);
b3Swap(keyValue.m_linkIndexA, keyValue.m_linkIndexB);
}
}
@@ -111,12 +112,11 @@ struct DefaultPluginCollisionInterface : public b3PluginCollisionInterface
m_customCollisionFilters.clear();
}
virtual int needsBroadphaseCollision(int objectUniqueIdA, int linkIndexA,
int collisionFilterGroupA,int collisionFilterMaskA,
int objectUniqueIdB, int linkIndexB,
int collisionFilterGroupB,int collisionFilterMaskB,
int filterMode
)
virtual int needsBroadphaseCollision(int objectUniqueIdA, int linkIndexA,
int collisionFilterGroupA, int collisionFilterMaskA,
int objectUniqueIdB, int linkIndexB,
int collisionFilterGroupB, int collisionFilterMaskB,
int filterMode)
{
//check and apply any custom rules for those objects/links
b3CustomCollisionFilter keyValue;
@@ -125,16 +125,16 @@ struct DefaultPluginCollisionInterface : public b3PluginCollisionInterface
keyValue.m_objectUniqueIdB = objectUniqueIdB;
keyValue.m_linkIndexB = linkIndexB;
if (objectUniqueIdA>objectUniqueIdB)
if (objectUniqueIdA > objectUniqueIdB)
{
b3Swap(keyValue.m_objectUniqueIdA,keyValue.m_objectUniqueIdB);
b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB);
b3Swap(keyValue.m_objectUniqueIdA, keyValue.m_objectUniqueIdB);
b3Swap(keyValue.m_linkIndexA, keyValue.m_linkIndexB);
}
if (objectUniqueIdA==objectUniqueIdB)
if (objectUniqueIdA == objectUniqueIdB)
{
if (keyValue.m_linkIndexA>keyValue.m_linkIndexB)
if (keyValue.m_linkIndexA > keyValue.m_linkIndexB)
{
b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB);
b3Swap(keyValue.m_linkIndexA, keyValue.m_linkIndexB);
}
}
@@ -146,21 +146,20 @@ struct DefaultPluginCollisionInterface : public b3PluginCollisionInterface
//otherwise use the default fallback
if (filterMode==B3_FILTER_GROUPAMASKB_AND_GROUPBMASKA)
if (filterMode == B3_FILTER_GROUPAMASKB_AND_GROUPBMASKA)
{
bool collides = (collisionFilterGroupA & collisionFilterMaskB) != 0;
collides = collides && (collisionFilterGroupB & collisionFilterMaskA);
return collides;
}
if (filterMode==B3_FILTER_GROUPAMASKB_OR_GROUPBMASKA)
if (filterMode == B3_FILTER_GROUPAMASKB_OR_GROUPBMASKA)
{
bool collides = (collisionFilterGroupA & collisionFilterMaskB) != 0;
collides = collides || (collisionFilterGroupB & collisionFilterMaskA);
return collides;
}
return false;
}
};
@@ -171,7 +170,7 @@ struct CollisionFilterMyClass
DefaultPluginCollisionInterface m_collisionFilter;
CollisionFilterMyClass()
:m_testData(42)
: m_testData(42)
{
}
virtual ~CollisionFilterMyClass()
@@ -186,23 +185,20 @@ B3_SHARED_API int initPlugin_collisionFilterPlugin(struct b3PluginContext* conte
return SHARED_MEMORY_MAGIC_NUMBER;
}
B3_SHARED_API struct b3PluginCollisionInterface* getCollisionInterface_collisionFilterPlugin(struct b3PluginContext* context)
{
CollisionFilterMyClass* obj = (CollisionFilterMyClass* )context->m_userPointer;
CollisionFilterMyClass* obj = (CollisionFilterMyClass*)context->m_userPointer;
return &obj->m_collisionFilter;
}
B3_SHARED_API int executePluginCommand_collisionFilterPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
{
return 0;
}
B3_SHARED_API void exitPlugin_collisionFilterPlugin(struct b3PluginContext* context)
{
CollisionFilterMyClass* obj = (CollisionFilterMyClass*) context->m_userPointer;
CollisionFilterMyClass* obj = (CollisionFilterMyClass*)context->m_userPointer;
delete obj;
context->m_userPointer = 0;
}

16
examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.h
View File

@@ -4,20 +4,20 @@
#include "../b3PluginAPI.h"
#ifdef __cplusplus
extern "C"
extern "C"
{
#endif
//the following 3 APIs are required
B3_SHARED_API int initPlugin_collisionFilterPlugin(struct b3PluginContext* context);
B3_SHARED_API void exitPlugin_collisionFilterPlugin(struct b3PluginContext* context);
B3_SHARED_API int executePluginCommand_collisionFilterPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
//the following 3 APIs are required
B3_SHARED_API int initPlugin_collisionFilterPlugin(struct b3PluginContext* context);
B3_SHARED_API void exitPlugin_collisionFilterPlugin(struct b3PluginContext* context);
B3_SHARED_API int executePluginCommand_collisionFilterPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
//all the APIs below are optional
B3_SHARED_API struct b3PluginCollisionInterface* getCollisionInterface_collisionFilterPlugin(struct b3PluginContext* context);
//all the APIs below are optional
B3_SHARED_API struct b3PluginCollisionInterface* getCollisionInterface_collisionFilterPlugin(struct b3PluginContext* context);
#ifdef __cplusplus
};
#endif
#endif//#define COLLISION_FILTER_PLUGIN_H
#endif //#define COLLISION_FILTER_PLUGIN_H

51
examples/SharedMemory/plugins/eglPlugin/eglRendererPlugin.cpp
View File

@@ -5,7 +5,7 @@
#else
#include <Python.h>
#endif
#endif //EGL_ADD_PYTHON_INIT
#endif //EGL_ADD_PYTHON_INIT
//eglRenderer plugin
@@ -18,11 +18,8 @@
#include "../b3PluginContext.h"
#include <stdio.h>
struct EGLRendererPluginClass
{
EGLRendererVisualShapeConverter m_renderer;
EGLRendererPluginClass()
{
@@ -39,16 +36,14 @@ B3_SHARED_API int initPlugin_eglRendererPlugin(struct b3PluginContext* context)
return SHARED_MEMORY_MAGIC_NUMBER;
}
B3_SHARED_API int executePluginCommand_eglRendererPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
{
return -1;
return -1;
}
B3_SHARED_API void exitPlugin_eglRendererPlugin(struct b3PluginContext* context)
{
EGLRendererPluginClass* obj = (EGLRendererPluginClass*) context->m_userPointer;
EGLRendererPluginClass* obj = (EGLRendererPluginClass*)context->m_userPointer;
delete obj;
context->m_userPointer = 0;
}
@@ -56,34 +51,29 @@ B3_SHARED_API void exitPlugin_eglRendererPlugin(struct b3PluginContext* context)
//all the APIs below are optional
B3_SHARED_API struct UrdfRenderingInterface* getRenderInterface_eglRendererPlugin(struct b3PluginContext* context)
{
EGLRendererPluginClass* obj = (EGLRendererPluginClass*) context->m_userPointer;
EGLRendererPluginClass* obj = (EGLRendererPluginClass*)context->m_userPointer;
return &obj->m_renderer;
}
#ifdef EGL_ADD_PYTHON_INIT
static PyMethodDef eglMethods[] = {
{NULL, NULL, 0, NULL} /* Sentinel */
{NULL, NULL, 0, NULL} /* Sentinel */
};
#if PY_MAJOR_VERSION >= 3
static struct PyModuleDef moduledef = {
PyModuleDef_HEAD_INIT, "eglRenderer", /* m_name */
"eglRenderer for PyBullet "
, /* m_doc */
-1, /* m_size */
eglMethods, /* m_methods */
NULL, /* m_reload */
NULL, /* m_traverse */
NULL, /* m_clear */
NULL, /* m_free */
PyModuleDef_HEAD_INIT, "eglRenderer", /* m_name */
"eglRenderer for PyBullet ", /* m_doc */
-1, /* m_size */
eglMethods, /* m_methods */
NULL, /* m_reload */
NULL, /* m_traverse */
NULL, /* m_clear */
NULL, /* m_free */
};
#endif
PyMODINIT_FUNC
#if PY_MAJOR_VERSION >= 3
PyInit_eglRenderer(void)
@@ -91,20 +81,17 @@ PyInit_eglRenderer(void)
initeglRenderer(void)
#endif
{
PyObject* m;
PyObject* m;
#if PY_MAJOR_VERSION >= 3
m = PyModule_Create(&moduledef);
m = PyModule_Create(&moduledef);
#else
m = Py_InitModule3("eglRenderer", eglMethods, "eglRenderer for PyBullet");
m = Py_InitModule3("eglRenderer", eglMethods, "eglRenderer for PyBullet");
#endif
#if PY_MAJOR_VERSION >= 3
if (m == NULL) return m;
if (m == NULL) return m;
#else
if (m == NULL) return;
if (m == NULL) return;
#endif
}
#endif //EGL_ADD_PYTHON_INIT
#endif //EGL_ADD_PYTHON_INIT

18
examples/SharedMemory/plugins/eglPlugin/eglRendererPlugin.h
View File

@@ -4,22 +4,20 @@
#include "../b3PluginAPI.h"
#ifdef __cplusplus
extern "C"
extern "C"
{
#endif
//initPlugin, exitPlugin and executePluginCommand are required, otherwise plugin won't load
B3_SHARED_API int initPlugin_eglRendererPlugin(struct b3PluginContext* context);
B3_SHARED_API void exitPlugin_eglRendererPlugin(struct b3PluginContext* context);
B3_SHARED_API int executePluginCommand_eglRendererPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
//all the APIs below are optional
B3_SHARED_API struct UrdfRenderingInterface* getRenderInterface_eglRendererPlugin(struct b3PluginContext* context);
//initPlugin, exitPlugin and executePluginCommand are required, otherwise plugin won't load
B3_SHARED_API int initPlugin_eglRendererPlugin(struct b3PluginContext* context);
B3_SHARED_API void exitPlugin_eglRendererPlugin(struct b3PluginContext* context);
B3_SHARED_API int executePluginCommand_eglRendererPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
//all the APIs below are optional
B3_SHARED_API struct UrdfRenderingInterface* getRenderInterface_eglRendererPlugin(struct b3PluginContext* context);
#ifdef __cplusplus
};
#endif
#endif//#define EGL_RENDERER_PLUGIN_H
#endif //#define EGL_RENDERER_PLUGIN_H

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

File diff suppressed because it is too large Load Diff

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

@@ -5,19 +5,18 @@
struct EGLRendererVisualShapeConverter : public UrdfRenderingInterface
{
struct EGLRendererVisualShapeConverterInternalData* m_data;
EGLRendererVisualShapeConverter();
virtual ~EGLRendererVisualShapeConverter();
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int shapeUid, int objectIndex);
virtual int getNumVisualShapes(int bodyUniqueId);
virtual int getVisualShapesData(int bodyUniqueId, int shapeIndex, struct b3VisualShapeData* shapeData);
virtual void changeRGBAColor(int bodyUniqueId, int linkIndex, int shapeIndex, const double rgbaColor[4]);
virtual void changeShapeTexture(int objectUniqueId, int linkIndex, int shapeIndex, int textureUniqueId);
@@ -25,9 +24,9 @@ struct EGLRendererVisualShapeConverter : public UrdfRenderingInterface
virtual void removeVisualShape(int shapeUid);
virtual void setUpAxis(int axis);
virtual void resetCamera(float camDist, float yaw, float pitch, float camPosX,float camPosY, float camPosZ);
virtual void resetCamera(float camDist, float yaw, float pitch, float camPosX, float camPosY, float camPosZ);
virtual void clearBuffers(struct TGAColor& clearColor);
virtual void resetAll();
@@ -43,22 +42,16 @@ struct EGLRendererVisualShapeConverter : public UrdfRenderingInterface
virtual void setShadow(bool hasShadow);
virtual void setFlags(int flags);
virtual void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels,int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied);
void copyCameraImageDataGL(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels,int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied);
virtual void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied);
void copyCameraImageDataGL(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied);
virtual void render();
virtual void render(const float viewMat[16], const float projMat[16]);
virtual int loadTextureFile(const char* filename);
virtual int registerTexture(unsigned char* texels, int width, int height);
virtual void syncTransform(int shapeUid, const class btTransform& worldTransform, const class btVector3& localScaling);
};
#endif //EGL_RENDERER_VISUAL_SHAPE_CONVERTER_H
#endif //EGL_RENDERER_VISUAL_SHAPE_CONVERTER_H

86
examples/SharedMemory/plugins/grpcPlugin/grpcPlugin.cpp
View File

@@ -19,24 +19,23 @@
using grpc::Server;
using grpc::ServerAsyncResponseWriter;
using grpc::ServerBuilder;
using grpc::ServerContext;
using grpc::ServerCompletionQueue;
using grpc::ServerContext;
using grpc::Status;
using pybullet_grpc::PyBulletAPI;
using pybullet_grpc::PyBulletCommand;
using pybullet_grpc::PyBulletStatus;
using pybullet_grpc::PyBulletAPI;
bool gVerboseNetworkMessagesServer4 = false;
class ServerImpl final {
class ServerImpl final
{
public:
ServerImpl()
{
}
~ServerImpl() {
~ServerImpl()
{
Exit();
}
@@ -51,8 +50,8 @@ public:
}
}
void Init(PhysicsCommandProcessorInterface* comProc, const std::string& hostNamePort) {
void Init(PhysicsCommandProcessorInterface* comProc, const std::string& hostNamePort)
{
// Listen on the given address without any authentication mechanism.
m_builder.AddListeningPort(hostNamePort, grpc::InsecureServerCredentials());
// Register "service_" as the instance through which we'll communicate with
@@ -72,7 +71,6 @@ public:
// This can be run in multiple threads if needed.
bool HandleSingleRpc()
{
CallData::CallStatus status = CallData::CallStatus::CREATE;
{
@@ -98,21 +96,31 @@ public:
private:
// Class encompasing the state and logic needed to serve a request.
class CallData {
class CallData
{
public:
// Take in the "service" instance (in this case representing an asynchronous
// server) and the completion queue "cq" used for asynchronous communication
// with the gRPC runtime.
CallData(PyBulletAPI::AsyncService* service, ServerCompletionQueue* cq, PhysicsCommandProcessorInterface* comProc)
: service_(service), cq_(cq), responder_(&ctx_), status_(CREATE), m_finished(false), m_comProc(comProc) {
: service_(service), cq_(cq), responder_(&ctx_), status_(CREATE), m_finished(false), m_comProc(comProc)
{
// Invoke the serving logic right away.
Proceed();
}
enum CallStatus { CREATE, PROCESS, FINISH, TERMINATE };
enum CallStatus
{
CREATE,
PROCESS,
FINISH,
TERMINATE
};
CallStatus Proceed() {
if (status_ == CREATE) {
CallStatus Proceed()
{
if (status_ == CREATE)
{
// Make this instance progress to the PROCESS state.
status_ = PROCESS;
@@ -122,11 +130,11 @@ private:
// instances can serve different requests concurrently), in this case
// the memory address of this CallData instance.
service_->RequestSubmitCommand(&ctx_, &m_command, &responder_, cq_, cq_,
this);
this);
}
else if (status_ == PROCESS) {
else if (status_ == PROCESS)
{
// Spawn a new CallData instance to serve new clients while we process
// the one for this CallData. The instance will deallocate itself as
// part of its FINISH state.
@@ -144,7 +152,6 @@ private:
m_status.set_statustype(CMD_UNKNOWN_COMMAND_FLUSHED);
if (m_command.has_checkversioncommand())
{
m_status.set_statustype(CMD_CLIENT_COMMAND_COMPLETED);
@@ -154,11 +161,10 @@ private:
{
cmdPtr = convertGRPCToBulletCommand(m_command, cmd);
if (cmdPtr)
{
bool hasStatus = m_comProc->processCommand(*cmdPtr, serverStatus, &buffer[0], buffer.size());
double timeOutInSeconds = 10;
b3Clock clock;
double startTimeSeconds = clock.getTimeInSeconds();
@@ -196,7 +202,8 @@ private:
responder_.Finish(m_status, Status::OK, this);
}
else {
else
{
GPR_ASSERT(status_ == FINISH);
// Once in the FINISH state, deallocate ourselves (CallData).
delete this;
@@ -215,8 +222,6 @@ private:
// client.
ServerContext ctx_;
// What we get from the client.
PyBulletCommand m_command;
// What we send back to the client.
@@ -231,16 +236,15 @@ private:
bool m_finished;
PhysicsCommandProcessorInterface* m_comProc; //physics server command processor
PhysicsCommandProcessorInterface* m_comProc; //physics server command processor
};
// This can be run in multiple threads if needed.
void InitRpcs(PhysicsCommandProcessorInterface* comProc)
void InitRpcs(PhysicsCommandProcessorInterface* comProc)
{
// Spawn a new CallData instance to serve new clients.
new CallData(&service_, cq_.get(), comProc);
}
ServerBuilder m_builder;
std::unique_ptr<ServerCompletionQueue> cq_;
@@ -248,19 +252,18 @@ private:
std::unique_ptr<Server> server_;
};
struct grpcMyClass
{
int m_testData;
ServerImpl m_grpcServer;
bool m_grpcInitialized;
bool m_grpcTerminated;
grpcMyClass()
:m_testData(42),
m_grpcInitialized(false),
m_grpcTerminated(false)
: m_testData(42),
m_grpcInitialized(false),
m_grpcTerminated(false)
{
}
virtual ~grpcMyClass()
@@ -272,12 +275,10 @@ B3_SHARED_API int initPlugin_grpcPlugin(struct b3PluginContext* context)
{
grpcMyClass* obj = new grpcMyClass();
context->m_userPointer = obj;
return SHARED_MEMORY_MAGIC_NUMBER;
}
B3_SHARED_API int preTickPluginCallback_grpcPlugin(struct b3PluginContext* context)
{
//process grpc server messages
@@ -287,32 +288,32 @@ B3_SHARED_API int preTickPluginCallback_grpcPlugin(struct b3PluginContext* conte
B3_SHARED_API int processClientCommands_grpcPlugin(struct b3PluginContext* context)
{
grpcMyClass* obj = (grpcMyClass*)context->m_userPointer;
if (obj->m_grpcInitialized && !obj->m_grpcTerminated)
{
obj->m_grpcTerminated = obj->m_grpcServer.HandleSingleRpc();
}
obj->m_testData++;
return 0;
}
B3_SHARED_API int postTickPluginCallback_grpcPlugin(struct b3PluginContext* context)
{
grpcMyClass* obj = (grpcMyClass* )context->m_userPointer;
grpcMyClass* obj = (grpcMyClass*)context->m_userPointer;
obj->m_testData++;
return 0;
}
B3_SHARED_API int executePluginCommand_grpcPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
{
///3 cases:
///3 cases:
/// 1: send a non-empty string to start the GRPC server
/// 2: send some integer n, to call n times to HandleSingleRpc
/// 3: send nothing to terminate the GRPC server
grpcMyClass* obj = (grpcMyClass*)context->m_userPointer;
if (strlen(arguments->m_text))
{
if (!obj->m_grpcInitialized && context->m_rpcCommandProcessorInterface)
@@ -339,14 +340,13 @@ B3_SHARED_API int executePluginCommand_grpcPlugin(struct b3PluginContext* contex
obj->m_grpcInitialized = false;
}
}
return 0;
}
B3_SHARED_API void exitPlugin_grpcPlugin(struct b3PluginContext* context)
{
grpcMyClass* obj = (grpcMyClass*) context->m_userPointer;
grpcMyClass* obj = (grpcMyClass*)context->m_userPointer;
obj->m_grpcServer.Exit();
delete obj;
context->m_userPointer = 0;

23
examples/SharedMemory/plugins/grpcPlugin/grpcPlugin.h
View File

@@ -4,26 +4,23 @@
#include "../b3PluginAPI.h"
#ifdef __cplusplus
extern "C"
extern "C"
{
#endif
//the following 3 APIs are required
B3_SHARED_API int initPlugin_grpcPlugin(struct b3PluginContext* context);
B3_SHARED_API void exitPlugin_grpcPlugin(struct b3PluginContext* context);
B3_SHARED_API int executePluginCommand_grpcPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
//all the APIs below are optional
B3_SHARED_API int preTickPluginCallback_grpcPlugin(struct b3PluginContext* context);
B3_SHARED_API int postTickPluginCallback_grpcPlugin(struct b3PluginContext* context);
B3_SHARED_API int processClientCommands_grpcPlugin(struct b3PluginContext* context);
//the following 3 APIs are required
B3_SHARED_API int initPlugin_grpcPlugin(struct b3PluginContext* context);
B3_SHARED_API void exitPlugin_grpcPlugin(struct b3PluginContext* context);
B3_SHARED_API int executePluginCommand_grpcPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
//all the APIs below are optional
B3_SHARED_API int preTickPluginCallback_grpcPlugin(struct b3PluginContext* context);
B3_SHARED_API int postTickPluginCallback_grpcPlugin(struct b3PluginContext* context);
B3_SHARED_API int processClientCommands_grpcPlugin(struct b3PluginContext* context);
#ifdef __cplusplus
};
#endif
#endif//#define GRPC_PLUGIN_H
#endif //#define GRPC_PLUGIN_H

103
examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp
View File

@@ -4,7 +4,6 @@
#include "../b3PluginContext.h"
#include <stdio.h>
#include "LinearMath/btScalar.h"
#include "LinearMath/btAlignedObjectArray.h"
@@ -28,7 +27,7 @@ struct MyPDControlContainer
btAlignedObjectArray<MyPDControl> m_controllers;
b3RobotSimulatorClientAPI_NoDirect m_api;
MyPDControlContainer()
:m_testData(42)
: m_testData(42)
{
}
virtual ~MyPDControlContainer()
@@ -48,54 +47,50 @@ B3_SHARED_API int initPlugin_pdControlPlugin(struct b3PluginContext* context)
return SHARED_MEMORY_MAGIC_NUMBER;
}
B3_SHARED_API int preTickPluginCallback_pdControlPlugin(struct b3PluginContext* context)
{
//apply pd control here, apply forces using the PD gains
MyPDControlContainer* obj = (MyPDControlContainer*)context->m_userPointer;
for (int i = 0; i < obj->m_controllers.size(); i++)
{
const MyPDControl& pdControl = obj->m_controllers[i];
int dof1 = 0;
b3JointSensorState actualState;
if (obj->m_api.getJointState(pdControl.m_objectUniqueId, pdControl.m_linkIndex,&actualState))
if (obj->m_api.getJointState(pdControl.m_objectUniqueId, pdControl.m_linkIndex, &actualState))
{
if (pdControl.m_maxForce>0)
if (pdControl.m_maxForce > 0)
{
//compute torque
btScalar qActual = actualState.m_jointPosition;
btScalar qdActual = actualState.m_jointVelocity;
btScalar positionError = (pdControl.m_desiredPosition -qActual);
btScalar positionError = (pdControl.m_desiredPosition - qActual);
double desiredVelocity = 0;
btScalar velocityError = (pdControl.m_desiredVelocity-qdActual);
btScalar velocityError = (pdControl.m_desiredVelocity - qdActual);
btScalar force = pdControl.m_kp * positionError + pdControl.m_kd*velocityError;
btScalar force = pdControl.m_kp * positionError + pdControl.m_kd * velocityError;
btClamp(force, -pdControl.m_maxForce, pdControl.m_maxForce);
btClamp(force,-pdControl.m_maxForce,pdControl.m_maxForce);
//apply torque
b3RobotSimulatorJointMotorArgs args(CONTROL_MODE_TORQUE);
args.m_maxTorqueValue = force;
obj->m_api.setJointMotorControl(pdControl.m_objectUniqueId, pdControl.m_linkIndex, args);
}
}
}
return 0;
}
B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
{
MyPDControlContainer* obj = (MyPDControlContainer*)context->m_userPointer;
obj->m_api.syncBodies();
int numObj = obj->m_api.getNumBodies();
//printf("numObj = %d\n", numObj);
@@ -109,50 +104,50 @@ B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* c
switch (arguments->m_ints[0])
{
case eSetPDControl:
{
if (arguments->m_numFloats < 5)
return -1;
MyPDControl controller;
controller.m_desiredPosition = arguments->m_floats[0];
controller.m_desiredVelocity = arguments->m_floats[1];
controller.m_kd = arguments->m_floats[2];
controller.m_kp = arguments->m_floats[3];
controller.m_maxForce = arguments->m_floats[4];
controller.m_objectUniqueId = arguments->m_ints[1];
controller.m_linkIndex = arguments->m_ints[2];
int foundIndex = -1;
for (int i = 0; i < obj->m_controllers.size(); i++)
case eSetPDControl:
{
if (obj->m_controllers[i].m_objectUniqueId == controller.m_objectUniqueId && obj->m_controllers[i].m_linkIndex == controller.m_linkIndex)
if (arguments->m_numFloats < 5)
return -1;
MyPDControl controller;
controller.m_desiredPosition = arguments->m_floats[0];
controller.m_desiredVelocity = arguments->m_floats[1];
controller.m_kd = arguments->m_floats[2];
controller.m_kp = arguments->m_floats[3];
controller.m_maxForce = arguments->m_floats[4];
controller.m_objectUniqueId = arguments->m_ints[1];
controller.m_linkIndex = arguments->m_ints[2];
int foundIndex = -1;
for (int i = 0; i < obj->m_controllers.size(); i++)
{
obj->m_controllers[i] = controller;
foundIndex=i;
if (obj->m_controllers[i].m_objectUniqueId == controller.m_objectUniqueId && obj->m_controllers[i].m_linkIndex == controller.m_linkIndex)
{
obj->m_controllers[i] = controller;
foundIndex = i;
}
}
if (foundIndex < 0)
{
obj->m_controllers.push_back(controller);
}
break;
}
if (foundIndex<0)
case eRemovePDControl:
{
obj->m_controllers.push_back(controller);
}
break;
}
case eRemovePDControl:
{
MyPDControl controller;
controller.m_objectUniqueId = arguments->m_ints[1];
controller.m_linkIndex = arguments->m_ints[2];
MyPDControl controller;
controller.m_objectUniqueId = arguments->m_ints[1];
controller.m_linkIndex = arguments->m_ints[2];
for (int i = 0; i < obj->m_controllers.size(); i++)
{
if (obj->m_controllers[i].m_objectUniqueId == controller.m_objectUniqueId && obj->m_controllers[i].m_linkIndex == controller.m_linkIndex)
for (int i = 0; i < obj->m_controllers.size(); i++)
{
obj->m_controllers.removeAtIndex(i);
break;
if (obj->m_controllers[i].m_objectUniqueId == controller.m_objectUniqueId && obj->m_controllers[i].m_linkIndex == controller.m_linkIndex)
{
obj->m_controllers.removeAtIndex(i);
break;
}
}
break;
}
break;
}
default:
default:
{
return -1;
}
@@ -162,11 +157,9 @@ B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* c
return result;
}
B3_SHARED_API void exitPlugin_pdControlPlugin(struct b3PluginContext* context)
{
MyPDControlContainer* obj = (MyPDControlContainer*) context->m_userPointer;
MyPDControlContainer* obj = (MyPDControlContainer*)context->m_userPointer;
delete obj;
context->m_userPointer = 0;
}

29
examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h
View File

@@ -4,28 +4,27 @@
#include "../b3PluginAPI.h"
#ifdef __cplusplus
extern "C"
extern "C"
{
#endif
//the following 3 APIs are required
B3_SHARED_API int initPlugin_pdControlPlugin(struct b3PluginContext* context);
B3_SHARED_API void exitPlugin_pdControlPlugin(struct b3PluginContext* context);
B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
//the following 3 APIs are required
B3_SHARED_API int initPlugin_pdControlPlugin(struct b3PluginContext* context);
B3_SHARED_API void exitPlugin_pdControlPlugin(struct b3PluginContext* context);
B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
///
enum PDControlCommandEnum
{
eSetPDControl = 1,
eRemovePDControl = 2,
};
//all the APIs below are optional
B3_SHARED_API int preTickPluginCallback_pdControlPlugin(struct b3PluginContext* context);
///
enum PDControlCommandEnum
{
eSetPDControl = 1,
eRemovePDControl = 2,
};
//all the APIs below are optional
B3_SHARED_API int preTickPluginCallback_pdControlPlugin(struct b3PluginContext* context);
#ifdef __cplusplus
};
#endif
#endif//#define PID_CONTROL_PLUGIN_H
#endif //#define PID_CONTROL_PLUGIN_H

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