2015-09-25 05:50:34 +00:00
|
|
|
#include "PhysicsClientSharedMemory.h"
|
|
|
|
#include "PosixSharedMemory.h"
|
|
|
|
#include "Win32SharedMemory.h"
|
|
|
|
#include "LinearMath/btAlignedObjectArray.h"
|
|
|
|
#include "LinearMath/btVector3.h"
|
2015-11-11 20:44:26 +00:00
|
|
|
#include <string.h>
|
2015-09-25 05:50:34 +00:00
|
|
|
|
|
|
|
#include "Bullet3Common/b3Logging.h"
|
|
|
|
#include "../Utils/b3ResourcePath.h"
|
|
|
|
#include "../../Extras/Serialize/BulletFileLoader/btBulletFile.h"
|
|
|
|
#include "../../Extras/Serialize/BulletFileLoader/autogenerated/bullet.h"
|
|
|
|
#include "SharedMemoryBlock.h"
|
2015-11-23 04:50:32 +00:00
|
|
|
#include "BodyJointInfoUtility.h"
|
|
|
|
|
2015-09-25 05:50:34 +00:00
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
2015-10-13 18:32:25 +00:00
|
|
|
struct BodyJointInfoCache
|
|
|
|
{
|
2016-09-27 19:13:45 +00:00
|
|
|
std::string m_baseName;
|
2015-10-13 18:32:25 +00:00
|
|
|
btAlignedObjectArray<b3JointInfo> m_jointInfo;
|
2017-03-29 22:37:33 +00:00
|
|
|
std::string m_bodyName;
|
2015-10-13 18:32:25 +00:00
|
|
|
};
|
|
|
|
|
2015-09-25 05:50:34 +00:00
|
|
|
struct PhysicsClientSharedMemoryInternalData {
|
|
|
|
SharedMemoryInterface* m_sharedMemory;
|
2016-03-10 22:36:46 +00:00
|
|
|
bool m_ownsSharedMemory;
|
2015-09-25 05:50:34 +00:00
|
|
|
SharedMemoryBlock* m_testBlock1;
|
2015-10-13 18:32:25 +00:00
|
|
|
|
|
|
|
btHashMap<btHashInt,BodyJointInfoCache*> m_bodyJointMap;
|
2017-01-23 03:08:31 +00:00
|
|
|
btHashMap<btHashInt,b3UserConstraint> m_userConstraintInfoMap;
|
2015-09-25 05:50:34 +00:00
|
|
|
|
|
|
|
btAlignedObjectArray<TmpFloat3> m_debugLinesFrom;
|
|
|
|
btAlignedObjectArray<TmpFloat3> m_debugLinesTo;
|
|
|
|
btAlignedObjectArray<TmpFloat3> m_debugLinesColor;
|
|
|
|
|
2016-05-31 17:23:04 +00:00
|
|
|
int m_cachedCameraPixelsWidth;
|
|
|
|
int m_cachedCameraPixelsHeight;
|
|
|
|
btAlignedObjectArray<unsigned char> m_cachedCameraPixelsRGBA;
|
|
|
|
btAlignedObjectArray<float> m_cachedCameraDepthBuffer;
|
2016-08-11 21:55:30 +00:00
|
|
|
btAlignedObjectArray<int> m_cachedSegmentationMaskBuffer;
|
2016-05-31 17:23:04 +00:00
|
|
|
|
2016-09-01 20:30:07 +00:00
|
|
|
btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
|
2016-11-10 19:22:22 +00:00
|
|
|
btAlignedObjectArray<b3OverlappingObject> m_cachedOverlappingObjects;
|
2016-10-19 05:05:28 +00:00
|
|
|
btAlignedObjectArray<b3VisualShapeData> m_cachedVisualShapes;
|
2016-12-27 03:40:09 +00:00
|
|
|
btAlignedObjectArray<b3VRControllerEvent> m_cachedVREvents;
|
2017-03-02 20:33:22 +00:00
|
|
|
btAlignedObjectArray<b3KeyboardEvent> m_cachedKeyboardEvents;
|
2016-12-27 03:40:09 +00:00
|
|
|
btAlignedObjectArray<b3RayHitInfo> m_raycastHits;
|
2016-09-01 20:30:07 +00:00
|
|
|
|
2016-06-16 01:01:41 +00:00
|
|
|
btAlignedObjectArray<int> m_bodyIdsRequestInfo;
|
2017-01-23 03:08:31 +00:00
|
|
|
btAlignedObjectArray<int> m_constraintIdsRequestInfo;
|
|
|
|
|
2016-06-16 01:01:41 +00:00
|
|
|
SharedMemoryStatus m_tempBackupServerStatus;
|
|
|
|
|
2015-09-25 05:50:34 +00:00
|
|
|
SharedMemoryStatus m_lastServerStatus;
|
|
|
|
|
|
|
|
int m_counter;
|
2016-06-13 17:11:28 +00:00
|
|
|
|
2015-09-25 05:50:34 +00:00
|
|
|
bool m_isConnected;
|
|
|
|
bool m_waitingForServer;
|
|
|
|
bool m_hasLastServerStatus;
|
|
|
|
int m_sharedMemoryKey;
|
|
|
|
bool m_verboseOutput;
|
2017-02-24 23:34:11 +00:00
|
|
|
double m_timeOutInSeconds;
|
2015-09-25 05:50:34 +00:00
|
|
|
|
|
|
|
PhysicsClientSharedMemoryInternalData()
|
|
|
|
: m_sharedMemory(0),
|
2016-03-10 22:36:46 +00:00
|
|
|
m_ownsSharedMemory(false),
|
2015-09-25 05:50:34 +00:00
|
|
|
m_testBlock1(0),
|
2016-05-31 17:23:04 +00:00
|
|
|
m_cachedCameraPixelsWidth(0),
|
|
|
|
m_cachedCameraPixelsHeight(0),
|
2017-01-16 06:26:11 +00:00
|
|
|
m_counter(0),
|
2015-09-25 05:50:34 +00:00
|
|
|
m_isConnected(false),
|
|
|
|
m_waitingForServer(false),
|
|
|
|
m_hasLastServerStatus(false),
|
|
|
|
m_sharedMemoryKey(SHARED_MEMORY_KEY),
|
2017-02-24 23:34:11 +00:00
|
|
|
m_verboseOutput(false),
|
|
|
|
m_timeOutInSeconds(1e30)
|
|
|
|
{}
|
2015-09-25 05:50:34 +00:00
|
|
|
|
|
|
|
void processServerStatus();
|
|
|
|
|
|
|
|
bool canSubmitCommand() const;
|
|
|
|
};
|
|
|
|
|
2015-11-23 04:50:32 +00:00
|
|
|
|
|
|
|
|
2016-09-27 19:13:45 +00:00
|
|
|
int PhysicsClientSharedMemory::getNumBodies() const
|
|
|
|
{
|
|
|
|
return m_data->m_bodyJointMap.size();
|
|
|
|
}
|
2015-11-11 20:44:26 +00:00
|
|
|
|
2016-09-27 19:13:45 +00:00
|
|
|
int PhysicsClientSharedMemory::getBodyUniqueId(int serialIndex) const
|
2015-10-13 18:32:25 +00:00
|
|
|
{
|
2016-09-27 19:13:45 +00:00
|
|
|
if ((serialIndex >= 0) && (serialIndex < getNumBodies()))
|
|
|
|
{
|
|
|
|
return m_data->m_bodyJointMap.getKeyAtIndex(serialIndex).getUid1();
|
|
|
|
}
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
|
|
|
|
bool PhysicsClientSharedMemory::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const
|
|
|
|
{
|
|
|
|
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
|
|
|
|
if (bodyJointsPtr && *bodyJointsPtr)
|
|
|
|
{
|
|
|
|
BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
|
|
|
|
info.m_baseName = bodyJoints->m_baseName.c_str();
|
2017-03-29 22:37:33 +00:00
|
|
|
info.m_bodyName = bodyJoints->m_bodyName.c_str();
|
2016-09-27 19:13:45 +00:00
|
|
|
return true;
|
|
|
|
}
|
2016-11-17 00:12:59 +00:00
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
2016-09-27 19:13:45 +00:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
int PhysicsClientSharedMemory::getNumJoints(int bodyUniqueId) const
|
|
|
|
{
|
|
|
|
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
|
2015-10-13 18:32:25 +00:00
|
|
|
if (bodyJointsPtr && *bodyJointsPtr)
|
|
|
|
{
|
|
|
|
BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
|
|
|
|
|
|
|
|
return bodyJoints->m_jointInfo.size();
|
|
|
|
}
|
|
|
|
return 0;
|
|
|
|
|
|
|
|
}
|
2015-09-25 05:50:34 +00:00
|
|
|
|
2016-09-27 19:13:45 +00:00
|
|
|
bool PhysicsClientSharedMemory::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo& info) const
|
2015-10-13 18:32:25 +00:00
|
|
|
{
|
2016-09-27 19:13:45 +00:00
|
|
|
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
|
2015-10-13 18:32:25 +00:00
|
|
|
if (bodyJointsPtr && *bodyJointsPtr)
|
|
|
|
{
|
|
|
|
BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
|
2016-11-04 20:15:10 +00:00
|
|
|
if ((jointIndex >= 0) && (jointIndex < bodyJoints->m_jointInfo.size()))
|
|
|
|
{
|
|
|
|
info = bodyJoints->m_jointInfo[jointIndex];
|
|
|
|
return true;
|
|
|
|
}
|
2015-10-13 18:32:25 +00:00
|
|
|
}
|
2016-06-16 01:01:41 +00:00
|
|
|
return false;
|
2015-09-25 05:50:34 +00:00
|
|
|
}
|
|
|
|
|
2017-01-23 03:08:31 +00:00
|
|
|
int PhysicsClientSharedMemory::getNumUserConstraints() const
|
|
|
|
{
|
|
|
|
return m_data->m_userConstraintInfoMap.size();
|
|
|
|
}
|
|
|
|
|
|
|
|
int PhysicsClientSharedMemory::getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const
|
|
|
|
{
|
|
|
|
b3UserConstraint* constraintPtr =m_data->m_userConstraintInfoMap[constraintUniqueId];
|
|
|
|
if (constraintPtr)
|
|
|
|
{
|
|
|
|
info = *constraintPtr;
|
|
|
|
return 1;
|
|
|
|
}
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2017-05-04 01:25:25 +00:00
|
|
|
int PhysicsClientSharedMemory::getUserConstraintId(int serialIndex) const
|
|
|
|
{
|
|
|
|
if ((serialIndex >= 0) && (serialIndex < getNumUserConstraints()))
|
|
|
|
{
|
|
|
|
return m_data->m_userConstraintInfoMap.getKeyAtIndex(serialIndex).getUid1();
|
|
|
|
}
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
|
2015-09-25 05:50:34 +00:00
|
|
|
PhysicsClientSharedMemory::PhysicsClientSharedMemory()
|
|
|
|
|
|
|
|
{
|
|
|
|
m_data = new PhysicsClientSharedMemoryInternalData;
|
|
|
|
|
|
|
|
#ifdef _WIN32
|
|
|
|
m_data->m_sharedMemory = new Win32SharedMemoryClient();
|
|
|
|
#else
|
|
|
|
m_data->m_sharedMemory = new PosixSharedMemory();
|
|
|
|
#endif
|
2016-03-10 22:36:46 +00:00
|
|
|
m_data->m_ownsSharedMemory = true;
|
|
|
|
|
2015-09-25 05:50:34 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
PhysicsClientSharedMemory::~PhysicsClientSharedMemory() {
|
|
|
|
if (m_data->m_isConnected) {
|
|
|
|
disconnectSharedMemory();
|
|
|
|
}
|
2017-01-23 03:08:31 +00:00
|
|
|
resetData();
|
|
|
|
|
2016-03-10 22:36:46 +00:00
|
|
|
if (m_data->m_ownsSharedMemory)
|
|
|
|
{
|
|
|
|
delete m_data->m_sharedMemory;
|
|
|
|
}
|
2015-09-25 05:50:34 +00:00
|
|
|
delete m_data;
|
|
|
|
}
|
|
|
|
|
2017-05-04 04:53:29 +00:00
|
|
|
void PhysicsClientSharedMemory::removeCachedBody(int bodyUniqueId)
|
|
|
|
{
|
|
|
|
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
|
|
|
|
if (bodyJointsPtr && *bodyJointsPtr)
|
|
|
|
{
|
|
|
|
|
|
|
|
BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
|
|
|
|
for (int j=0;j<bodyJoints->m_jointInfo.size();j++)
|
|
|
|
{
|
|
|
|
if (bodyJoints->m_jointInfo[j].m_jointName)
|
|
|
|
{
|
|
|
|
free(bodyJoints->m_jointInfo[j].m_jointName);
|
|
|
|
}
|
|
|
|
if (bodyJoints->m_jointInfo[j].m_linkName)
|
|
|
|
{
|
|
|
|
free(bodyJoints->m_jointInfo[j].m_linkName);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
delete (*bodyJointsPtr);
|
|
|
|
m_data->m_bodyJointMap.remove(bodyUniqueId);
|
|
|
|
}
|
|
|
|
}
|
2017-01-23 03:08:31 +00:00
|
|
|
void PhysicsClientSharedMemory::resetData()
|
|
|
|
{
|
|
|
|
m_data->m_debugLinesFrom.clear();
|
|
|
|
m_data->m_debugLinesTo.clear();
|
|
|
|
m_data->m_debugLinesColor.clear();
|
|
|
|
for (int i=0;i<m_data->m_bodyJointMap.size();i++)
|
|
|
|
{
|
|
|
|
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i);
|
|
|
|
if (bodyJointsPtr && *bodyJointsPtr)
|
|
|
|
{
|
|
|
|
BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
|
|
|
|
for (int j=0;j<bodyJoints->m_jointInfo.size();j++) {
|
|
|
|
if (bodyJoints->m_jointInfo[j].m_jointName)
|
|
|
|
{
|
|
|
|
free(bodyJoints->m_jointInfo[j].m_jointName);
|
|
|
|
}
|
|
|
|
if (bodyJoints->m_jointInfo[j].m_linkName)
|
|
|
|
{
|
|
|
|
free(bodyJoints->m_jointInfo[j].m_linkName);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
delete (*bodyJointsPtr);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
m_data->m_bodyJointMap.clear();
|
|
|
|
m_data->m_userConstraintInfoMap.clear();
|
|
|
|
|
|
|
|
}
|
2015-09-25 05:50:34 +00:00
|
|
|
void PhysicsClientSharedMemory::setSharedMemoryKey(int key) { m_data->m_sharedMemoryKey = key; }
|
|
|
|
|
2016-03-10 22:36:46 +00:00
|
|
|
|
|
|
|
void PhysicsClientSharedMemory::setSharedMemoryInterface(class SharedMemoryInterface* sharedMem)
|
|
|
|
{
|
|
|
|
if (m_data->m_sharedMemory && m_data->m_ownsSharedMemory)
|
|
|
|
{
|
|
|
|
delete m_data->m_sharedMemory;
|
|
|
|
}
|
|
|
|
m_data->m_ownsSharedMemory = false;
|
|
|
|
m_data->m_sharedMemory = sharedMem;
|
|
|
|
}
|
|
|
|
|
2015-09-25 05:50:34 +00:00
|
|
|
void PhysicsClientSharedMemory::disconnectSharedMemory() {
|
2016-03-10 22:36:46 +00:00
|
|
|
if (m_data->m_isConnected && m_data->m_sharedMemory) {
|
2015-09-25 05:50:34 +00:00
|
|
|
m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE);
|
|
|
|
}
|
2016-03-10 22:36:46 +00:00
|
|
|
m_data->m_isConnected = false;
|
|
|
|
|
2015-09-25 05:50:34 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
bool PhysicsClientSharedMemory::isConnected() const { return m_data->m_isConnected; }
|
|
|
|
|
|
|
|
bool PhysicsClientSharedMemory::connect() {
|
|
|
|
/// server always has to create and initialize shared memory
|
|
|
|
bool allowCreation = false;
|
|
|
|
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) {
|
Improve debug text/line rendering, can be use to draw frames and text in local coordinate of an object / link.
example:
kuka = p.loadURDF("kuka_iiwa/model.urdf")
p.getNumJoints(kuka)
pybullet.addUserDebugLine([0,0,0],[0,0,0.1],[0,0,1],trackObjectUniqueId=2,trackLinkIndex=6)
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],trackObjectUniqueId=2,trackLinkIndex=6)
Also allow to render text using a given orientation (instead of pointing to the camera), example:
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],textOrientation=[0,0,0,1], trackObjectUniqueId=2,trackLinkIndex=6)
Add drawTexturedTriangleMesh, for drawing 3d text.
Expose readSingleInstanceTransformToCPU, to extract position/orientation from graphics index.
updateTexture: allow to not flip texels around up axis
2017-05-24 05:05:26 +00:00
|
|
|
//there is no chance people are still using this software 100 years from now ;-)
|
|
|
|
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 connecting to shared memory: please start server before client\n");
|
|
|
|
}
|
2015-09-25 05:50:34 +00:00
|
|
|
m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey,
|
|
|
|
SHARED_MEMORY_SIZE);
|
|
|
|
m_data->m_testBlock1 = 0;
|
|
|
|
return false;
|
|
|
|
} else {
|
|
|
|
if (m_data->m_verboseOutput) {
|
|
|
|
b3Printf("Connected to existing shared memory, status OK.\n");
|
|
|
|
}
|
|
|
|
m_data->m_isConnected = true;
|
|
|
|
}
|
|
|
|
} else {
|
2016-12-29 05:51:54 +00:00
|
|
|
b3Warning("Cannot connect to shared memory");
|
2015-09-25 05:50:34 +00:00
|
|
|
return false;
|
|
|
|
}
|
2016-11-17 00:12:59 +00:00
|
|
|
#if 0
|
|
|
|
if (m_data->m_isConnected)
|
|
|
|
{
|
|
|
|
//get all existing bodies and body info...
|
|
|
|
|
|
|
|
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
|
|
|
|
//now transfer the information of the individual objects etc.
|
|
|
|
command.m_type = CMD_REQUEST_BODY_INFO;
|
|
|
|
command.m_sdfRequestInfoArgs.m_bodyUniqueId = 37;
|
|
|
|
submitClientCommand(command);
|
2017-02-22 17:33:30 +00:00
|
|
|
|
|
|
|
double startTime = clock.getTimeInSeconds();
|
|
|
|
double timeOutInSeconds = 10;
|
|
|
|
|
2016-11-17 00:12:59 +00:00
|
|
|
const SharedMemoryStatus* status = 0;
|
|
|
|
|
2017-02-22 17:33:30 +00:00
|
|
|
while ((status == 0) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds))
|
2016-11-17 00:12:59 +00:00
|
|
|
{
|
|
|
|
status = processServerStatus();
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
//submitClientCommand(command);
|
|
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
#endif
|
2015-09-25 05:50:34 +00:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2016-06-16 01:01:41 +00:00
|
|
|
|
|
|
|
///todo(erwincoumans) refactor this: merge with PhysicsDirect::processBodyJointInfo
|
|
|
|
void PhysicsClientSharedMemory::processBodyJointInfo(int bodyUniqueId, const SharedMemoryStatus& serverCmd)
|
|
|
|
{
|
|
|
|
bParse::btBulletFile bf(
|
|
|
|
&this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0],
|
2016-11-04 20:15:10 +00:00
|
|
|
serverCmd.m_numDataStreamBytes);
|
2016-06-16 01:01:41 +00:00
|
|
|
bf.setFileDNAisMemoryDNA();
|
|
|
|
bf.parse(false);
|
|
|
|
|
|
|
|
|
|
|
|
BodyJointInfoCache* bodyJoints = new BodyJointInfoCache;
|
|
|
|
m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints);
|
2017-03-29 22:37:33 +00:00
|
|
|
bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName;
|
2016-06-16 01:01:41 +00:00
|
|
|
|
|
|
|
for (int i = 0; i < bf.m_multiBodies.size(); i++)
|
|
|
|
{
|
|
|
|
int flag = bf.getFlags();
|
|
|
|
if ((flag & bParse::FD_DOUBLE_PRECISION) != 0)
|
|
|
|
{
|
|
|
|
Bullet::btMultiBodyDoubleData* mb =
|
|
|
|
(Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i];
|
|
|
|
|
2016-09-27 19:13:45 +00:00
|
|
|
bodyJoints->m_baseName = mb->m_baseName;
|
2016-06-16 01:01:41 +00:00
|
|
|
addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
|
|
|
|
} else
|
|
|
|
{
|
|
|
|
Bullet::btMultiBodyFloatData* mb =
|
|
|
|
(Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i];
|
2016-09-27 19:13:45 +00:00
|
|
|
bodyJoints->m_baseName = mb->m_baseName;
|
2016-06-16 01:01:41 +00:00
|
|
|
addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
if (bf.ok()) {
|
|
|
|
if (m_data->m_verboseOutput)
|
|
|
|
{
|
|
|
|
b3Printf("Received robot description ok!\n");
|
|
|
|
}
|
|
|
|
} else
|
|
|
|
{
|
|
|
|
b3Warning("Robot description not received");
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2015-09-25 05:50:34 +00:00
|
|
|
const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
|
2017-01-16 06:26:11 +00:00
|
|
|
// SharedMemoryStatus* stat = 0;
|
2017-03-26 20:06:46 +00:00
|
|
|
|
2015-09-25 05:50:34 +00:00
|
|
|
if (!m_data->m_testBlock1) {
|
2016-03-10 22:36:46 +00:00
|
|
|
m_data->m_lastServerStatus.m_type = CMD_SHARED_MEMORY_NOT_INITIALIZED;
|
|
|
|
return &m_data->m_lastServerStatus;
|
2015-09-25 05:50:34 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
if (!m_data->m_waitingForServer) {
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2016-03-10 22:36:46 +00:00
|
|
|
if (m_data->m_testBlock1->m_magicId != SHARED_MEMORY_MAGIC_NUMBER)
|
|
|
|
{
|
|
|
|
m_data->m_lastServerStatus.m_type = CMD_SHARED_MEMORY_NOT_INITIALIZED;
|
|
|
|
return &m_data->m_lastServerStatus;
|
|
|
|
}
|
|
|
|
|
2015-09-25 05:50:34 +00:00
|
|
|
if (m_data->m_testBlock1->m_numServerCommands >
|
2017-03-26 20:06:46 +00:00
|
|
|
m_data->m_testBlock1->m_numProcessedServerCommands)
|
|
|
|
{
|
|
|
|
B3_PROFILE("processServerCMD");
|
2015-09-25 05:50:34 +00:00
|
|
|
btAssert(m_data->m_testBlock1->m_numServerCommands ==
|
|
|
|
m_data->m_testBlock1->m_numProcessedServerCommands + 1);
|
|
|
|
|
|
|
|
const SharedMemoryStatus& serverCmd = m_data->m_testBlock1->m_serverCommands[0];
|
|
|
|
m_data->m_lastServerStatus = serverCmd;
|
|
|
|
|
2017-01-16 06:26:11 +00:00
|
|
|
// EnumSharedMemoryServerStatus s = (EnumSharedMemoryServerStatus)serverCmd.m_type;
|
2015-09-25 05:50:34 +00:00
|
|
|
// consume the command
|
2015-11-23 04:50:32 +00:00
|
|
|
|
2017-03-26 20:06:46 +00:00
|
|
|
switch (serverCmd.m_type)
|
|
|
|
{
|
|
|
|
|
|
|
|
case CMD_CLIENT_COMMAND_COMPLETED:
|
|
|
|
{
|
|
|
|
B3_PROFILE("CMD_CLIENT_COMMAND_COMPLETED");
|
|
|
|
|
2015-09-25 05:50:34 +00:00
|
|
|
if (m_data->m_verboseOutput) {
|
|
|
|
b3Printf("Server completed command");
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
}
|
2016-12-31 22:43:15 +00:00
|
|
|
|
|
|
|
case CMD_MJCF_LOADING_COMPLETED:
|
|
|
|
{
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_MJCF_LOADING_COMPLETED");
|
|
|
|
|
2016-12-31 22:43:15 +00:00
|
|
|
if (m_data->m_verboseOutput) {
|
|
|
|
b3Printf("Server loading the MJCF OK\n");
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
}
|
2017-03-26 20:06:46 +00:00
|
|
|
case CMD_SDF_LOADING_COMPLETED:
|
|
|
|
{
|
|
|
|
B3_PROFILE("CMD_SDF_LOADING_COMPLETED");
|
|
|
|
|
2016-06-13 17:11:28 +00:00
|
|
|
if (m_data->m_verboseOutput) {
|
|
|
|
b3Printf("Server loading the SDF OK\n");
|
|
|
|
}
|
2016-06-16 01:01:41 +00:00
|
|
|
|
2016-06-13 17:11:28 +00:00
|
|
|
break;
|
|
|
|
}
|
2016-11-04 20:15:10 +00:00
|
|
|
|
2017-03-26 20:06:46 +00:00
|
|
|
case CMD_URDF_LOADING_COMPLETED:
|
|
|
|
{
|
|
|
|
B3_PROFILE("CMD_URDF_LOADING_COMPLETED");
|
2016-06-13 17:11:28 +00:00
|
|
|
|
2015-09-25 05:50:34 +00:00
|
|
|
if (m_data->m_verboseOutput) {
|
|
|
|
b3Printf("Server loading the URDF OK\n");
|
|
|
|
}
|
|
|
|
|
2016-11-04 20:15:10 +00:00
|
|
|
if (serverCmd.m_numDataStreamBytes > 0) {
|
2015-11-11 20:44:26 +00:00
|
|
|
bParse::btBulletFile bf(
|
2015-11-23 04:50:32 +00:00
|
|
|
this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor,
|
2016-11-04 20:15:10 +00:00
|
|
|
serverCmd.m_numDataStreamBytes);
|
2015-11-11 20:44:26 +00:00
|
|
|
bf.setFileDNAisMemoryDNA();
|
|
|
|
bf.parse(false);
|
2016-09-27 19:13:45 +00:00
|
|
|
int bodyUniqueId = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
|
2015-10-13 18:32:25 +00:00
|
|
|
|
|
|
|
BodyJointInfoCache* bodyJoints = new BodyJointInfoCache;
|
2016-09-27 19:13:45 +00:00
|
|
|
m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints);
|
2017-03-29 22:37:33 +00:00
|
|
|
bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName;
|
2015-09-25 05:50:34 +00:00
|
|
|
|
2015-11-11 20:44:26 +00:00
|
|
|
for (int i = 0; i < bf.m_multiBodies.size(); i++) {
|
2015-09-25 05:50:34 +00:00
|
|
|
|
2015-11-23 04:50:32 +00:00
|
|
|
|
|
|
|
int flag = bf.getFlags();
|
|
|
|
|
2015-09-25 05:50:34 +00:00
|
|
|
if ((flag & bParse::FD_DOUBLE_PRECISION) != 0) {
|
|
|
|
Bullet::btMultiBodyDoubleData* mb =
|
2015-11-11 20:44:26 +00:00
|
|
|
(Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i];
|
2017-03-03 19:33:20 +00:00
|
|
|
if (mb->m_baseName)
|
|
|
|
{
|
|
|
|
bodyJoints->m_baseName = mb->m_baseName;
|
|
|
|
}
|
2015-11-23 04:50:32 +00:00
|
|
|
addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
|
|
|
|
} else
|
|
|
|
{
|
2015-09-25 05:50:34 +00:00
|
|
|
Bullet::btMultiBodyFloatData* mb =
|
2015-11-11 20:44:26 +00:00
|
|
|
(Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i];
|
2017-03-03 19:33:20 +00:00
|
|
|
if (mb->m_baseName)
|
|
|
|
{
|
|
|
|
bodyJoints->m_baseName = mb->m_baseName;
|
|
|
|
}
|
2015-11-23 04:50:32 +00:00
|
|
|
addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
|
2015-09-25 05:50:34 +00:00
|
|
|
}
|
|
|
|
}
|
2015-11-11 20:44:26 +00:00
|
|
|
if (bf.ok()) {
|
2015-09-25 05:50:34 +00:00
|
|
|
if (m_data->m_verboseOutput) {
|
|
|
|
b3Printf("Received robot description ok!\n");
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
b3Warning("Robot description not received");
|
|
|
|
}
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
}
|
2017-03-26 20:06:46 +00:00
|
|
|
case CMD_DESIRED_STATE_RECEIVED_COMPLETED:
|
|
|
|
{
|
|
|
|
B3_PROFILE("CMD_DESIRED_STATE_RECEIVED_COMPLETED");
|
|
|
|
|
2015-09-25 05:50:34 +00:00
|
|
|
if (m_data->m_verboseOutput) {
|
|
|
|
b3Printf("Server received desired state");
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
}
|
2017-03-26 20:06:46 +00:00
|
|
|
case CMD_STEP_FORWARD_SIMULATION_COMPLETED:
|
|
|
|
{
|
|
|
|
B3_PROFILE("CMD_STEP_FORWARD_SIMULATION_COMPLETED");
|
|
|
|
|
2015-09-25 05:50:34 +00:00
|
|
|
if (m_data->m_verboseOutput) {
|
|
|
|
b3Printf("Server completed step simulation");
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
}
|
2017-03-26 20:06:46 +00:00
|
|
|
case CMD_URDF_LOADING_FAILED:
|
|
|
|
{
|
|
|
|
B3_PROFILE("CMD_URDF_LOADING_FAILED");
|
|
|
|
|
2015-09-25 05:50:34 +00:00
|
|
|
if (m_data->m_verboseOutput) {
|
|
|
|
b3Printf("Server failed loading the URDF...\n");
|
|
|
|
}
|
2016-06-13 17:11:28 +00:00
|
|
|
|
|
|
|
break;
|
|
|
|
}
|
2017-01-23 03:08:31 +00:00
|
|
|
case CMD_USER_CONSTRAINT_INFO_COMPLETED:
|
|
|
|
{
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_USER_CONSTRAINT_INFO_COMPLETED");
|
|
|
|
|
|
|
|
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
|
2017-01-23 03:08:31 +00:00
|
|
|
m_data->m_userConstraintInfoMap.insert(cid,serverCmd.m_userConstraintResultArgs);
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
case CMD_USER_CONSTRAINT_COMPLETED:
|
|
|
|
{
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_USER_CONSTRAINT_COMPLETED");
|
2017-01-23 03:08:31 +00:00
|
|
|
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
|
|
|
|
m_data->m_userConstraintInfoMap.insert(cid,serverCmd.m_userConstraintResultArgs);
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
case CMD_REMOVE_USER_CONSTRAINT_COMPLETED:
|
|
|
|
{
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_REMOVE_USER_CONSTRAINT_COMPLETED");
|
2017-01-23 03:08:31 +00:00
|
|
|
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
|
|
|
|
m_data->m_userConstraintInfoMap.remove(cid);
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
case CMD_CHANGE_USER_CONSTRAINT_COMPLETED:
|
|
|
|
{
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_CHANGE_USER_CONSTRAINT_COMPLETED");
|
|
|
|
|
2017-01-23 03:08:31 +00:00
|
|
|
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
|
|
|
|
b3UserConstraint* userConstraintPtr = m_data->m_userConstraintInfoMap[cid];
|
|
|
|
if (userConstraintPtr)
|
|
|
|
{
|
|
|
|
const b3UserConstraint* serverConstraint = &serverCmd.m_userConstraintResultArgs;
|
|
|
|
if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_PIVOT_IN_B)
|
|
|
|
{
|
|
|
|
userConstraintPtr->m_childFrame[0] = serverConstraint->m_childFrame[0];
|
|
|
|
userConstraintPtr->m_childFrame[1] = serverConstraint->m_childFrame[1];
|
|
|
|
userConstraintPtr->m_childFrame[2] = serverConstraint->m_childFrame[2];
|
|
|
|
}
|
|
|
|
if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B)
|
|
|
|
{
|
|
|
|
userConstraintPtr->m_childFrame[3] = serverConstraint->m_childFrame[3];
|
|
|
|
userConstraintPtr->m_childFrame[4] = serverConstraint->m_childFrame[4];
|
|
|
|
userConstraintPtr->m_childFrame[5] = serverConstraint->m_childFrame[5];
|
|
|
|
userConstraintPtr->m_childFrame[6] = serverConstraint->m_childFrame[6];
|
|
|
|
}
|
|
|
|
if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE)
|
|
|
|
{
|
|
|
|
userConstraintPtr->m_maxAppliedForce = serverConstraint->m_maxAppliedForce;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
case CMD_USER_CONSTRAINT_FAILED:
|
|
|
|
{
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_USER_CONSTRAINT_FAILED");
|
2017-01-23 03:08:31 +00:00
|
|
|
b3Warning("createConstraint failed");
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
case CMD_REMOVE_USER_CONSTRAINT_FAILED:
|
|
|
|
{
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_REMOVE_USER_CONSTRAINT_FAILED");
|
2017-01-23 03:08:31 +00:00
|
|
|
b3Warning("removeConstraint failed");
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
case CMD_CHANGE_USER_CONSTRAINT_FAILED:
|
|
|
|
{
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_CHANGE_USER_CONSTRAINT_FAILED");
|
2017-01-23 03:08:31 +00:00
|
|
|
b3Warning("changeConstraint failed");
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
case CMD_ACTUAL_STATE_UPDATE_FAILED:
|
|
|
|
{
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_ACTUAL_STATE_UPDATE_FAILED");
|
2017-01-23 03:08:31 +00:00
|
|
|
b3Warning("request actual state failed");
|
|
|
|
break;
|
|
|
|
}
|
2016-06-16 01:01:41 +00:00
|
|
|
case CMD_BODY_INFO_COMPLETED:
|
|
|
|
{
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_BODY_INFO_COMPLETED");
|
2016-06-16 01:01:41 +00:00
|
|
|
if (m_data->m_verboseOutput) {
|
|
|
|
b3Printf("Received body info\n");
|
|
|
|
}
|
|
|
|
int bodyUniqueId = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
|
|
|
|
processBodyJointInfo(bodyUniqueId, serverCmd);
|
|
|
|
|
|
|
|
break;
|
|
|
|
}
|
2016-12-31 22:43:15 +00:00
|
|
|
case CMD_MJCF_LOADING_FAILED:
|
|
|
|
{
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_MJCF_LOADING_FAILED");
|
2016-12-31 22:43:15 +00:00
|
|
|
if (m_data->m_verboseOutput) {
|
|
|
|
b3Printf("Server failed loading the MJCF...\n");
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
}
|
2016-06-13 17:11:28 +00:00
|
|
|
case CMD_SDF_LOADING_FAILED: {
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_SDF_LOADING_FAILED");
|
2016-06-13 17:11:28 +00:00
|
|
|
if (m_data->m_verboseOutput) {
|
|
|
|
b3Printf("Server failed loading the SDF...\n");
|
|
|
|
}
|
|
|
|
|
2015-09-25 05:50:34 +00:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
case CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED: {
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED");
|
2015-09-25 05:50:34 +00:00
|
|
|
if (m_data->m_verboseOutput) {
|
|
|
|
b3Printf("Server received bullet data stream OK\n");
|
|
|
|
}
|
|
|
|
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
case CMD_BULLET_DATA_STREAM_RECEIVED_FAILED: {
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_BULLET_DATA_STREAM_RECEIVED_FAILED");
|
2015-09-25 05:50:34 +00:00
|
|
|
if (m_data->m_verboseOutput) {
|
|
|
|
b3Printf("Server failed receiving bullet data stream\n");
|
|
|
|
}
|
|
|
|
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
case CMD_ACTUAL_STATE_UPDATE_COMPLETED: {
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_ACTUAL_STATE_UPDATE_COMPLETED");
|
|
|
|
if (m_data->m_verboseOutput)
|
|
|
|
{
|
2015-09-25 05:50:34 +00:00
|
|
|
b3Printf("Received actual state\n");
|
2017-03-26 20:06:46 +00:00
|
|
|
|
|
|
|
SharedMemoryStatus& command = m_data->m_testBlock1->m_serverCommands[0];
|
2015-09-25 05:50:34 +00:00
|
|
|
|
2017-03-26 20:06:46 +00:00
|
|
|
int numQ = command.m_sendActualStateArgs.m_numDegreeOfFreedomQ;
|
|
|
|
int numU = command.m_sendActualStateArgs.m_numDegreeOfFreedomU;
|
2015-09-25 05:50:34 +00:00
|
|
|
b3Printf("size Q = %d, size U = %d\n", numQ, numU);
|
2017-03-26 20:06:46 +00:00
|
|
|
char msg[1024];
|
|
|
|
{
|
|
|
|
sprintf(msg, "Q=[");
|
|
|
|
|
|
|
|
for (int i = 0; i < numQ; i++) {
|
|
|
|
if (i < numQ - 1) {
|
|
|
|
sprintf(msg, "%s%f,", msg,
|
|
|
|
command.m_sendActualStateArgs.m_actualStateQ[i]);
|
|
|
|
} else {
|
|
|
|
sprintf(msg, "%s%f", msg,
|
|
|
|
command.m_sendActualStateArgs.m_actualStateQ[i]);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
sprintf(msg, "%s]", msg);
|
|
|
|
}
|
2015-09-25 05:50:34 +00:00
|
|
|
b3Printf(msg);
|
2017-03-26 20:06:46 +00:00
|
|
|
|
2015-09-25 05:50:34 +00:00
|
|
|
sprintf(msg, "U=[");
|
|
|
|
|
|
|
|
for (int i = 0; i < numU; i++) {
|
|
|
|
if (i < numU - 1) {
|
|
|
|
sprintf(msg, "%s%f,", msg,
|
|
|
|
command.m_sendActualStateArgs.m_actualStateQdot[i]);
|
|
|
|
} else {
|
|
|
|
sprintf(msg, "%s%f", msg,
|
|
|
|
command.m_sendActualStateArgs.m_actualStateQdot[i]);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
sprintf(msg, "%s]", msg);
|
2017-03-26 20:06:46 +00:00
|
|
|
|
2015-09-25 05:50:34 +00:00
|
|
|
b3Printf(msg);
|
|
|
|
b3Printf("\n");
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
case CMD_RESET_SIMULATION_COMPLETED: {
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_RESET_SIMULATION_COMPLETED");
|
|
|
|
if (m_data->m_verboseOutput) {
|
2015-09-25 05:50:34 +00:00
|
|
|
b3Printf("CMD_RESET_SIMULATION_COMPLETED clean data\n");
|
|
|
|
}
|
2017-01-23 03:08:31 +00:00
|
|
|
resetData();
|
|
|
|
|
2015-09-25 05:50:34 +00:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
case CMD_DEBUG_LINES_COMPLETED: {
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_DEBUG_LINES_COMPLETED");
|
2015-09-25 05:50:34 +00:00
|
|
|
if (m_data->m_verboseOutput) {
|
|
|
|
b3Printf("Success receiving %d debug lines",
|
|
|
|
serverCmd.m_sendDebugLinesArgs.m_numDebugLines);
|
|
|
|
}
|
|
|
|
|
|
|
|
int numLines = serverCmd.m_sendDebugLinesArgs.m_numDebugLines;
|
|
|
|
float* linesFrom =
|
2015-11-23 04:50:32 +00:00
|
|
|
(float*)&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0];
|
2015-09-25 05:50:34 +00:00
|
|
|
float* linesTo =
|
2015-11-23 04:50:32 +00:00
|
|
|
(float*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0] +
|
2015-09-25 05:50:34 +00:00
|
|
|
numLines * 3 * sizeof(float));
|
|
|
|
float* linesColor =
|
2015-11-23 04:50:32 +00:00
|
|
|
(float*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0] +
|
2015-09-25 05:50:34 +00:00
|
|
|
2 * numLines * 3 * sizeof(float));
|
|
|
|
|
|
|
|
m_data->m_debugLinesFrom.resize(serverCmd.m_sendDebugLinesArgs.m_startingLineIndex +
|
|
|
|
numLines);
|
|
|
|
m_data->m_debugLinesTo.resize(serverCmd.m_sendDebugLinesArgs.m_startingLineIndex +
|
|
|
|
numLines);
|
|
|
|
m_data->m_debugLinesColor.resize(
|
|
|
|
serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + numLines);
|
|
|
|
|
|
|
|
for (int i = 0; i < numLines; i++) {
|
|
|
|
TmpFloat3 from = CreateTmpFloat3(linesFrom[i * 3], linesFrom[i * 3 + 1],
|
|
|
|
linesFrom[i * 3 + 2]);
|
|
|
|
TmpFloat3 to =
|
|
|
|
CreateTmpFloat3(linesTo[i * 3], linesTo[i * 3 + 1], linesTo[i * 3 + 2]);
|
|
|
|
TmpFloat3 color = CreateTmpFloat3(linesColor[i * 3], linesColor[i * 3 + 1],
|
|
|
|
linesColor[i * 3 + 2]);
|
|
|
|
|
|
|
|
m_data
|
|
|
|
->m_debugLinesFrom[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + i] =
|
|
|
|
from;
|
|
|
|
m_data->m_debugLinesTo[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + i] =
|
|
|
|
to;
|
|
|
|
m_data->m_debugLinesColor[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex +
|
|
|
|
i] = color;
|
|
|
|
}
|
|
|
|
|
|
|
|
break;
|
|
|
|
}
|
2015-10-27 22:46:13 +00:00
|
|
|
case CMD_RIGID_BODY_CREATION_COMPLETED:
|
|
|
|
{
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_RIGID_BODY_CREATION_COMPLETED");
|
2015-10-27 22:46:13 +00:00
|
|
|
|
|
|
|
break;
|
|
|
|
}
|
2015-09-25 05:50:34 +00:00
|
|
|
case CMD_DEBUG_LINES_OVERFLOW_FAILED: {
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_DEBUG_LINES_OVERFLOW_FAILED");
|
|
|
|
b3Warning("Error receiving debug lines");
|
2015-09-25 05:50:34 +00:00
|
|
|
m_data->m_debugLinesFrom.resize(0);
|
|
|
|
m_data->m_debugLinesTo.resize(0);
|
|
|
|
m_data->m_debugLinesColor.resize(0);
|
|
|
|
|
|
|
|
break;
|
|
|
|
}
|
2016-05-20 01:37:15 +00:00
|
|
|
|
|
|
|
case CMD_CAMERA_IMAGE_COMPLETED:
|
|
|
|
{
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_CAMERA_IMAGE_COMPLETED");
|
2016-05-31 17:23:04 +00:00
|
|
|
if (m_data->m_verboseOutput)
|
|
|
|
{
|
|
|
|
b3Printf("Camera image OK\n");
|
|
|
|
}
|
|
|
|
|
|
|
|
int numBytesPerPixel = 4;//RGBA
|
|
|
|
int numTotalPixels = serverCmd.m_sendPixelDataArguments.m_startingPixelIndex+
|
|
|
|
serverCmd.m_sendPixelDataArguments.m_numPixelsCopied+
|
|
|
|
serverCmd.m_sendPixelDataArguments.m_numRemainingPixels;
|
|
|
|
|
|
|
|
m_data->m_cachedCameraPixelsWidth = 0;
|
|
|
|
m_data->m_cachedCameraPixelsHeight = 0;
|
2016-06-02 00:47:41 +00:00
|
|
|
|
|
|
|
int numPixels = serverCmd.m_sendPixelDataArguments.m_imageWidth*serverCmd.m_sendPixelDataArguments.m_imageHeight;
|
|
|
|
|
|
|
|
m_data->m_cachedCameraPixelsRGBA.reserve(numPixels*numBytesPerPixel);
|
2016-05-31 17:23:04 +00:00
|
|
|
m_data->m_cachedCameraDepthBuffer.resize(numTotalPixels);
|
2016-08-11 21:55:30 +00:00
|
|
|
m_data->m_cachedSegmentationMaskBuffer.resize(numTotalPixels);
|
2016-05-31 17:23:04 +00:00
|
|
|
m_data->m_cachedCameraPixelsRGBA.resize(numTotalPixels*numBytesPerPixel);
|
2016-06-02 00:47:41 +00:00
|
|
|
|
|
|
|
|
2016-05-31 17:23:04 +00:00
|
|
|
unsigned char* rgbaPixelsReceived =
|
|
|
|
(unsigned char*)&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0];
|
2016-06-07 01:54:05 +00:00
|
|
|
// printf("pixel = %d\n", rgbaPixelsReceived[0]);
|
2016-06-02 00:47:41 +00:00
|
|
|
|
2016-06-07 23:11:58 +00:00
|
|
|
float* depthBuffer = (float*)&(m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*4]);
|
2016-08-11 21:55:30 +00:00
|
|
|
int* segmentationMaskBuffer = (int*)&(m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*8]);
|
2016-06-07 23:11:58 +00:00
|
|
|
|
|
|
|
for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;i++)
|
|
|
|
{
|
|
|
|
m_data->m_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i];
|
|
|
|
}
|
|
|
|
|
2016-08-11 21:55:30 +00:00
|
|
|
for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;i++)
|
|
|
|
{
|
|
|
|
m_data->m_cachedSegmentationMaskBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = segmentationMaskBuffer[i];
|
|
|
|
}
|
|
|
|
|
2016-05-31 17:23:04 +00:00
|
|
|
for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*numBytesPerPixel;i++)
|
|
|
|
{
|
|
|
|
m_data->m_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel]
|
|
|
|
= rgbaPixelsReceived[i];
|
|
|
|
}
|
|
|
|
|
2016-05-20 01:37:15 +00:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
case CMD_CAMERA_IMAGE_FAILED:
|
|
|
|
{
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_CAMERA_IMAGE_FAILED");
|
|
|
|
b3Warning("Camera image FAILED\n");
|
2016-05-20 01:37:15 +00:00
|
|
|
break;
|
|
|
|
}
|
2016-08-10 01:40:12 +00:00
|
|
|
case CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED:
|
|
|
|
{
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
case CMD_CALCULATED_INVERSE_DYNAMICS_FAILED:
|
|
|
|
{
|
|
|
|
b3Warning("Inverse Dynamics computations failed");
|
|
|
|
break;
|
|
|
|
}
|
2016-11-10 19:22:22 +00:00
|
|
|
case CMD_REQUEST_AABB_OVERLAP_FAILED:
|
|
|
|
{
|
|
|
|
b3Warning("Overlapping object query failed");
|
|
|
|
break;
|
|
|
|
}
|
2016-12-27 03:40:09 +00:00
|
|
|
|
|
|
|
case CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED:
|
|
|
|
{
|
|
|
|
if (m_data->m_verboseOutput)
|
|
|
|
{
|
|
|
|
b3Printf("Raycast completed");
|
|
|
|
}
|
|
|
|
m_data->m_raycastHits.clear();
|
|
|
|
for (int i=0;i<serverCmd.m_raycastHits.m_numRaycastHits;i++)
|
|
|
|
{
|
|
|
|
m_data->m_raycastHits.push_back(serverCmd.m_raycastHits.m_rayHits[i]);
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
case CMD_REQUEST_VR_EVENTS_DATA_COMPLETED:
|
|
|
|
{
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_REQUEST_VR_EVENTS_DATA_COMPLETED");
|
2016-12-27 03:40:09 +00:00
|
|
|
if (m_data->m_verboseOutput)
|
|
|
|
{
|
|
|
|
b3Printf("Request VR Events completed");
|
|
|
|
}
|
|
|
|
m_data->m_cachedVREvents.clear();
|
|
|
|
for (int i=0;i< serverCmd.m_sendVREvents.m_numVRControllerEvents;i++)
|
|
|
|
{
|
|
|
|
m_data->m_cachedVREvents.push_back(serverCmd.m_sendVREvents.m_controllerEvents[i]);
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
2017-03-02 20:33:22 +00:00
|
|
|
case CMD_REQUEST_KEYBOARD_EVENTS_DATA_COMPLETED:
|
|
|
|
{
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_REQUEST_KEYBOARD_EVENTS_DATA_COMPLETED");
|
2017-03-02 20:33:22 +00:00
|
|
|
if (m_data->m_verboseOutput)
|
|
|
|
{
|
|
|
|
b3Printf("Request keyboard events completed");
|
|
|
|
}
|
|
|
|
m_data->m_cachedKeyboardEvents.resize(serverCmd.m_sendKeyboardEvents.m_numKeyboardEvents);
|
|
|
|
for (int i=0;i<serverCmd.m_sendKeyboardEvents.m_numKeyboardEvents;i++)
|
|
|
|
{
|
|
|
|
m_data->m_cachedKeyboardEvents[i] = serverCmd.m_sendKeyboardEvents.m_keyboardEvents[i];
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
2016-11-10 19:22:22 +00:00
|
|
|
case CMD_REQUEST_AABB_OVERLAP_COMPLETED:
|
|
|
|
{
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_REQUEST_AABB_OVERLAP_COMPLETED");
|
2016-11-10 19:22:22 +00:00
|
|
|
if (m_data->m_verboseOutput)
|
|
|
|
{
|
|
|
|
b3Printf("Overlapping object request completed");
|
|
|
|
}
|
|
|
|
|
|
|
|
int startOverlapIndex = serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex;
|
|
|
|
int numOverlapCopied = serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied;
|
|
|
|
m_data->m_cachedOverlappingObjects.resize(startOverlapIndex + numOverlapCopied);
|
|
|
|
b3OverlappingObject* objects = (b3OverlappingObject*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
|
|
|
|
|
|
|
|
for (int i = 0; i < numOverlapCopied; i++)
|
|
|
|
{
|
|
|
|
m_data->m_cachedOverlappingObjects[startOverlapIndex + i] = objects[i];
|
|
|
|
}
|
|
|
|
|
|
|
|
break;
|
|
|
|
}
|
2016-09-01 20:30:07 +00:00
|
|
|
case CMD_CONTACT_POINT_INFORMATION_COMPLETED:
|
|
|
|
{
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_CONTACT_POINT_INFORMATION_COMPLETED");
|
|
|
|
if (m_data->m_verboseOutput)
|
2016-09-01 20:30:07 +00:00
|
|
|
{
|
|
|
|
b3Printf("Contact Point Information Request OK\n");
|
|
|
|
}
|
|
|
|
int startContactIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex;
|
|
|
|
int numContactsCopied = serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
|
|
|
|
|
|
|
|
m_data->m_cachedContactPoints.resize(startContactIndex+numContactsCopied);
|
|
|
|
|
|
|
|
b3ContactPointData* contactData = (b3ContactPointData*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
|
|
|
|
|
|
|
|
for (int i=0;i<numContactsCopied;i++)
|
|
|
|
{
|
|
|
|
m_data->m_cachedContactPoints[startContactIndex+i] = contactData[i];
|
|
|
|
}
|
|
|
|
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
case CMD_CONTACT_POINT_INFORMATION_FAILED:
|
|
|
|
{
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_CONTACT_POINT_INFORMATION_FAILED");
|
|
|
|
b3Warning("Contact Point Information Request failed");
|
2016-09-01 20:30:07 +00:00
|
|
|
break;
|
|
|
|
}
|
2016-10-13 06:03:36 +00:00
|
|
|
|
2016-10-13 06:07:41 +00:00
|
|
|
case CMD_SAVE_WORLD_COMPLETED:
|
2017-03-26 20:06:46 +00:00
|
|
|
{
|
|
|
|
B3_PROFILE("CMD_SAVE_WORLD_COMPLETED");
|
|
|
|
break;
|
|
|
|
}
|
2016-10-13 06:07:41 +00:00
|
|
|
|
2016-10-13 06:03:36 +00:00
|
|
|
case CMD_SAVE_WORLD_FAILED:
|
2016-10-13 06:07:41 +00:00
|
|
|
{
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_SAVE_WORLD_FAILED");
|
|
|
|
b3Warning("Saving world failed");
|
2016-10-13 06:07:41 +00:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
case CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED:
|
|
|
|
{
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED");
|
|
|
|
break;
|
2016-10-12 17:46:07 +00:00
|
|
|
}
|
|
|
|
case CMD_CALCULATE_INVERSE_KINEMATICS_FAILED:
|
|
|
|
{
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_CALCULATE_INVERSE_KINEMATICS_FAILED");
|
|
|
|
b3Warning("Calculate Inverse Kinematics Request failed");
|
2016-10-12 17:46:07 +00:00
|
|
|
break;
|
|
|
|
}
|
2016-10-19 05:05:28 +00:00
|
|
|
case CMD_VISUAL_SHAPE_INFO_COMPLETED:
|
|
|
|
{
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_VISUAL_SHAPE_INFO_COMPLETED");
|
|
|
|
if (m_data->m_verboseOutput)
|
2016-10-19 05:05:28 +00:00
|
|
|
{
|
|
|
|
b3Printf("Visual Shape Information Request OK\n");
|
|
|
|
}
|
|
|
|
int startVisualShapeIndex = serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex;
|
|
|
|
int numVisualShapesCopied = serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied;
|
|
|
|
m_data->m_cachedVisualShapes.resize(startVisualShapeIndex + numVisualShapesCopied);
|
|
|
|
b3VisualShapeData* shapeData = (b3VisualShapeData*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
|
|
|
|
for (int i = 0; i < numVisualShapesCopied; i++)
|
|
|
|
{
|
|
|
|
m_data->m_cachedVisualShapes[startVisualShapeIndex + i] = shapeData[i];
|
|
|
|
}
|
2016-10-13 06:07:41 +00:00
|
|
|
|
2016-10-19 05:05:28 +00:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
case CMD_VISUAL_SHAPE_INFO_FAILED:
|
|
|
|
{
|
|
|
|
b3Warning("Visual Shape Info Request failed");
|
|
|
|
break;
|
|
|
|
}
|
2016-10-20 17:56:44 +00:00
|
|
|
case CMD_VISUAL_SHAPE_UPDATE_COMPLETED:
|
|
|
|
{
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
case CMD_VISUAL_SHAPE_UPDATE_FAILED:
|
|
|
|
{
|
|
|
|
b3Warning("Visual Shape Update failed");
|
|
|
|
break;
|
|
|
|
}
|
2016-10-21 06:40:30 +00:00
|
|
|
case CMD_LOAD_TEXTURE_COMPLETED:
|
|
|
|
{
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
case CMD_LOAD_TEXTURE_FAILED:
|
|
|
|
{
|
|
|
|
b3Warning("Load texture failed");
|
|
|
|
break;
|
|
|
|
}
|
2016-11-12 02:07:42 +00:00
|
|
|
case CMD_BULLET_LOADING_COMPLETED:
|
|
|
|
{
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
case CMD_BULLET_LOADING_FAILED:
|
|
|
|
{
|
|
|
|
b3Warning("Load .bullet failed");
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
case CMD_BULLET_SAVING_FAILED:
|
|
|
|
{
|
|
|
|
b3Warning("Save .bullet failed");
|
|
|
|
break;
|
|
|
|
}
|
2017-01-17 23:42:32 +00:00
|
|
|
case CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED:
|
2016-11-14 15:39:34 +00:00
|
|
|
case CMD_USER_DEBUG_DRAW_COMPLETED:
|
|
|
|
{
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
case CMD_USER_DEBUG_DRAW_FAILED:
|
|
|
|
{
|
|
|
|
b3Warning("User debug draw failed");
|
|
|
|
break;
|
|
|
|
}
|
2017-01-23 03:08:31 +00:00
|
|
|
|
2017-01-21 02:13:24 +00:00
|
|
|
case CMD_SYNC_BODY_INFO_COMPLETED:
|
|
|
|
{
|
|
|
|
break;
|
|
|
|
}
|
2017-02-17 22:25:53 +00:00
|
|
|
case CMD_STATE_LOGGING_START_COMPLETED:
|
|
|
|
{
|
|
|
|
break;
|
|
|
|
};
|
|
|
|
case CMD_STATE_LOGGING_COMPLETED:
|
|
|
|
{
|
|
|
|
break;
|
|
|
|
};
|
|
|
|
|
|
|
|
case CMD_STATE_LOGGING_FAILED:
|
|
|
|
{
|
|
|
|
b3Warning("State Logging failed");
|
|
|
|
break;
|
|
|
|
}
|
2017-04-10 18:03:41 +00:00
|
|
|
case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_FAILED:
|
|
|
|
{
|
|
|
|
b3Warning("Request visualizer camera failed");
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_COMPLETED:
|
|
|
|
{
|
|
|
|
break;
|
|
|
|
}
|
2017-05-04 04:53:29 +00:00
|
|
|
case CMD_REMOVE_BODY_COMPLETED:
|
|
|
|
{
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
case CMD_REMOVE_BODY_FAILED:
|
|
|
|
{
|
|
|
|
b3Warning("Removing body failed");
|
|
|
|
break;
|
|
|
|
}
|
2017-05-09 17:31:28 +00:00
|
|
|
case CMD_GET_DYNAMICS_INFO_COMPLETED:
|
2017-05-08 05:21:38 +00:00
|
|
|
{
|
|
|
|
break;
|
|
|
|
}
|
2017-05-09 17:31:28 +00:00
|
|
|
case CMD_GET_DYNAMICS_INFO_FAILED:
|
2017-05-08 05:21:38 +00:00
|
|
|
{
|
2017-05-09 17:44:33 +00:00
|
|
|
b3Warning("Request dynamics info failed");
|
2017-05-08 05:21:38 +00:00
|
|
|
break;
|
|
|
|
}
|
2015-09-25 05:50:34 +00:00
|
|
|
default: {
|
2016-10-12 17:46:07 +00:00
|
|
|
b3Error("Unknown server status %d\n", serverCmd.m_type);
|
2015-09-25 05:50:34 +00:00
|
|
|
btAssert(0);
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
2017-03-26 20:06:46 +00:00
|
|
|
|
2015-09-25 05:50:34 +00:00
|
|
|
m_data->m_testBlock1->m_numProcessedServerCommands++;
|
|
|
|
// we don't have more than 1 command outstanding (in total, either server or client)
|
|
|
|
btAssert(m_data->m_testBlock1->m_numProcessedServerCommands ==
|
|
|
|
m_data->m_testBlock1->m_numServerCommands);
|
|
|
|
|
|
|
|
if (m_data->m_testBlock1->m_numServerCommands ==
|
|
|
|
m_data->m_testBlock1->m_numProcessedServerCommands) {
|
|
|
|
m_data->m_waitingForServer = false;
|
|
|
|
} else {
|
|
|
|
m_data->m_waitingForServer = true;
|
2016-05-31 17:23:04 +00:00
|
|
|
}
|
|
|
|
|
2016-12-31 22:43:15 +00:00
|
|
|
|
2017-01-21 02:13:24 +00:00
|
|
|
if ((serverCmd.m_type == CMD_SDF_LOADING_COMPLETED) || (serverCmd.m_type == CMD_MJCF_LOADING_COMPLETED) || (serverCmd.m_type == CMD_SYNC_BODY_INFO_COMPLETED))
|
2016-06-13 17:11:28 +00:00
|
|
|
{
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_LOADING_COMPLETED");
|
2017-01-23 03:08:31 +00:00
|
|
|
int numConstraints = serverCmd.m_sdfLoadedArgs.m_numUserConstraints;
|
|
|
|
for (int i=0;i<numConstraints;i++)
|
|
|
|
{
|
|
|
|
int constraintUid = serverCmd.m_sdfLoadedArgs.m_userConstraintUniqueIds[i];
|
|
|
|
m_data->m_constraintIdsRequestInfo.push_back(constraintUid);
|
|
|
|
}
|
2016-06-13 17:11:28 +00:00
|
|
|
int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
|
|
|
|
if (numBodies>0)
|
|
|
|
{
|
2016-06-16 01:01:41 +00:00
|
|
|
m_data->m_tempBackupServerStatus = m_data->m_lastServerStatus;
|
|
|
|
|
|
|
|
for (int i=0;i<numBodies;i++)
|
|
|
|
{
|
|
|
|
m_data->m_bodyIdsRequestInfo.push_back(serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i]);
|
|
|
|
}
|
|
|
|
|
|
|
|
int bodyId = m_data->m_bodyIdsRequestInfo[m_data->m_bodyIdsRequestInfo.size()-1];
|
|
|
|
m_data->m_bodyIdsRequestInfo.pop_back();
|
|
|
|
|
2016-06-13 17:11:28 +00:00
|
|
|
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
|
|
|
|
//now transfer the information of the individual objects etc.
|
2016-06-16 01:01:41 +00:00
|
|
|
command.m_type = CMD_REQUEST_BODY_INFO;
|
|
|
|
command.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyId;
|
2016-06-13 17:11:28 +00:00
|
|
|
submitClientCommand(command);
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
}
|
2017-05-04 04:53:29 +00:00
|
|
|
|
|
|
|
if (serverCmd.m_type == CMD_REMOVE_BODY_COMPLETED)
|
|
|
|
{
|
|
|
|
for (int i=0;i<serverCmd.m_removeObjectArgs.m_numBodies;i++)
|
|
|
|
{
|
|
|
|
int bodyUniqueId = serverCmd.m_removeObjectArgs.m_bodyUniqueIds[i];
|
|
|
|
removeCachedBody(bodyUniqueId);
|
|
|
|
}
|
2017-05-04 20:14:24 +00:00
|
|
|
for (int i=0;i<serverCmd.m_removeObjectArgs.m_numUserConstraints;i++)
|
|
|
|
{
|
|
|
|
int key = serverCmd.m_removeObjectArgs.m_userConstraintUniqueIds[i];
|
|
|
|
m_data->m_userConstraintInfoMap.remove(key);
|
|
|
|
}
|
2017-05-04 04:53:29 +00:00
|
|
|
}
|
|
|
|
|
2017-01-23 03:08:31 +00:00
|
|
|
if (serverCmd.m_type == CMD_USER_CONSTRAINT_INFO_COMPLETED)
|
|
|
|
{
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_USER_CONSTRAINT_INFO_COMPLETED");
|
|
|
|
|
2017-01-23 03:08:31 +00:00
|
|
|
if (m_data->m_constraintIdsRequestInfo.size())
|
|
|
|
{
|
|
|
|
int cid = m_data->m_constraintIdsRequestInfo[m_data->m_constraintIdsRequestInfo.size()-1];
|
|
|
|
m_data->m_constraintIdsRequestInfo.pop_back();
|
|
|
|
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
|
|
|
|
command.m_type = CMD_USER_CONSTRAINT;
|
|
|
|
command.m_updateFlags = USER_CONSTRAINT_REQUEST_INFO;
|
|
|
|
command.m_userConstraintArguments.m_userConstraintUniqueId = cid;
|
|
|
|
submitClientCommand(command);
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2016-06-16 01:01:41 +00:00
|
|
|
if (serverCmd.m_type == CMD_BODY_INFO_COMPLETED)
|
|
|
|
{
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_BODY_INFO_COMPLETED");
|
2016-06-16 01:01:41 +00:00
|
|
|
//are there any bodies left to be processed?
|
|
|
|
if (m_data->m_bodyIdsRequestInfo.size())
|
|
|
|
{
|
|
|
|
int bodyId = m_data->m_bodyIdsRequestInfo[m_data->m_bodyIdsRequestInfo.size()-1];
|
|
|
|
m_data->m_bodyIdsRequestInfo.pop_back();
|
|
|
|
|
|
|
|
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
|
|
|
|
//now transfer the information of the individual objects etc.
|
|
|
|
command.m_type = CMD_REQUEST_BODY_INFO;
|
|
|
|
command.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyId;
|
|
|
|
submitClientCommand(command);
|
|
|
|
return 0;
|
|
|
|
} else
|
|
|
|
{
|
2017-01-23 03:08:31 +00:00
|
|
|
if (m_data->m_constraintIdsRequestInfo.size())
|
|
|
|
{
|
|
|
|
int cid = m_data->m_constraintIdsRequestInfo[m_data->m_constraintIdsRequestInfo.size()-1];
|
|
|
|
m_data->m_constraintIdsRequestInfo.pop_back();
|
|
|
|
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
|
|
|
|
command.m_type = CMD_USER_CONSTRAINT;
|
|
|
|
command.m_updateFlags = USER_CONSTRAINT_REQUEST_INFO;
|
|
|
|
command.m_userConstraintArguments.m_userConstraintUniqueId = cid;
|
|
|
|
submitClientCommand(command);
|
|
|
|
return 0;
|
|
|
|
} else
|
|
|
|
{
|
|
|
|
m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus;
|
|
|
|
}
|
2016-06-16 01:01:41 +00:00
|
|
|
}
|
|
|
|
}
|
2016-11-10 19:22:22 +00:00
|
|
|
|
|
|
|
if (serverCmd.m_type == CMD_REQUEST_AABB_OVERLAP_COMPLETED)
|
|
|
|
{
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_REQUEST_AABB_OVERLAP_COMPLETED2");
|
2016-11-10 19:22:22 +00:00
|
|
|
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
|
|
|
|
if (serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects > 0 && serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied)
|
|
|
|
{
|
|
|
|
command.m_type = CMD_REQUEST_AABB_OVERLAP;
|
|
|
|
command.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex = serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex + serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied;
|
|
|
|
submitClientCommand(command);
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
}
|
2016-06-13 17:11:28 +00:00
|
|
|
|
2016-09-01 20:30:07 +00:00
|
|
|
if (serverCmd.m_type == CMD_CONTACT_POINT_INFORMATION_COMPLETED)
|
|
|
|
{
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_CONTACT_POINT_INFORMATION_COMPLETED2");
|
2016-09-02 01:28:39 +00:00
|
|
|
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
|
2016-09-01 20:30:07 +00:00
|
|
|
if (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints>0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied)
|
|
|
|
{
|
|
|
|
command.m_type = CMD_REQUEST_CONTACT_POINT_INFORMATION;
|
|
|
|
command.m_requestContactPointArguments.m_startingContactPointIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex+serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
|
|
|
|
command.m_requestContactPointArguments.m_objectAIndexFilter = -1;
|
|
|
|
command.m_requestContactPointArguments.m_objectBIndexFilter = -1;
|
|
|
|
submitClientCommand(command);
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2016-10-19 05:05:28 +00:00
|
|
|
if (serverCmd.m_type == CMD_VISUAL_SHAPE_INFO_COMPLETED)
|
|
|
|
{
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_VISUAL_SHAPE_INFO_COMPLETED2");
|
2016-10-19 05:05:28 +00:00
|
|
|
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
|
|
|
|
if (serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes >0 && serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied)
|
|
|
|
{
|
|
|
|
command.m_type = CMD_REQUEST_VISUAL_SHAPE_INFO;
|
|
|
|
command.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex = serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex + serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied;
|
|
|
|
command.m_requestVisualShapeDataArguments.m_bodyUniqueId = serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId;
|
|
|
|
submitClientCommand(command);
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
2016-05-31 17:23:04 +00:00
|
|
|
if (serverCmd.m_type == CMD_CAMERA_IMAGE_COMPLETED)
|
|
|
|
{
|
2017-03-26 20:06:46 +00:00
|
|
|
B3_PROFILE("CMD_CAMERA_IMAGE_COMPLETED2");
|
2016-05-31 17:23:04 +00:00
|
|
|
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
|
|
|
|
|
2016-08-08 21:23:44 +00:00
|
|
|
if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0 && serverCmd.m_sendPixelDataArguments.m_numPixelsCopied)
|
2016-05-31 17:23:04 +00:00
|
|
|
{
|
|
|
|
|
|
|
|
|
|
|
|
// continue requesting remaining pixels
|
|
|
|
command.m_type = CMD_REQUEST_CAMERA_IMAGE_DATA;
|
|
|
|
command.m_requestPixelDataArguments.m_startPixelIndex =
|
|
|
|
serverCmd.m_sendPixelDataArguments.m_startingPixelIndex +
|
|
|
|
serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;
|
|
|
|
submitClientCommand(command);
|
|
|
|
return 0;
|
|
|
|
} else
|
|
|
|
{
|
|
|
|
m_data->m_cachedCameraPixelsWidth = serverCmd.m_sendPixelDataArguments.m_imageWidth;
|
|
|
|
m_data->m_cachedCameraPixelsHeight = serverCmd.m_sendPixelDataArguments.m_imageHeight;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2015-09-25 05:50:34 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
if ((serverCmd.m_type == CMD_DEBUG_LINES_COMPLETED) &&
|
2017-03-26 20:06:46 +00:00
|
|
|
(serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines > 0))
|
|
|
|
{
|
|
|
|
B3_PROFILE("CMD_DEBUG_LINES_COMPLETED2");
|
2015-09-25 05:50:34 +00:00
|
|
|
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
|
|
|
|
|
|
|
|
// continue requesting debug lines for drawing
|
|
|
|
command.m_type = CMD_REQUEST_DEBUG_LINES;
|
|
|
|
command.m_requestDebugLinesArguments.m_startingLineIndex =
|
|
|
|
serverCmd.m_sendDebugLinesArgs.m_numDebugLines +
|
|
|
|
serverCmd.m_sendDebugLinesArgs.m_startingLineIndex;
|
|
|
|
submitClientCommand(command);
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
return &m_data->m_lastServerStatus;
|
|
|
|
|
|
|
|
} else {
|
2017-03-26 20:06:46 +00:00
|
|
|
if (m_data->m_verboseOutput)
|
|
|
|
{
|
|
|
|
B3_PROFILE("m_verboseOutput");
|
|
|
|
|
2015-09-25 05:50:34 +00:00
|
|
|
b3Printf("m_numServerStatus = %d, processed = %d\n",
|
|
|
|
m_data->m_testBlock1->m_numServerCommands,
|
|
|
|
m_data->m_testBlock1->m_numProcessedServerCommands);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
bool PhysicsClientSharedMemory::canSubmitCommand() const {
|
2017-03-02 20:33:22 +00:00
|
|
|
if (m_data->m_isConnected && !m_data->m_waitingForServer)
|
|
|
|
{
|
|
|
|
if (m_data->m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER)
|
|
|
|
{
|
|
|
|
return true;
|
|
|
|
} else
|
|
|
|
{
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
return false;
|
2015-09-25 05:50:34 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
struct SharedMemoryCommand* PhysicsClientSharedMemory::getAvailableSharedMemoryCommand() {
|
2016-07-08 02:24:44 +00:00
|
|
|
static int sequence = 0;
|
|
|
|
m_data->m_testBlock1->m_clientCommands[0].m_sequenceNumber = sequence++;
|
2015-09-25 05:50:34 +00:00
|
|
|
return &m_data->m_testBlock1->m_clientCommands[0];
|
|
|
|
}
|
|
|
|
|
|
|
|
bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& command) {
|
|
|
|
/// at the moment we allow a maximum of 1 outstanding command, so we check for this
|
|
|
|
// once the server processed the command and returns a status, we clear the flag
|
|
|
|
// "m_data->m_waitingForServer" and allow submitting the next command
|
|
|
|
|
|
|
|
if (!m_data->m_waitingForServer) {
|
|
|
|
if (&m_data->m_testBlock1->m_clientCommands[0] != &command) {
|
|
|
|
m_data->m_testBlock1->m_clientCommands[0] = command;
|
|
|
|
}
|
|
|
|
m_data->m_testBlock1->m_numClientCommands++;
|
|
|
|
m_data->m_waitingForServer = true;
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
void PhysicsClientSharedMemory::uploadBulletFileToSharedMemory(const char* data, int len) {
|
|
|
|
btAssert(len < SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
|
|
|
|
if (len >= SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE) {
|
|
|
|
b3Warning("uploadBulletFileToSharedMemory %d exceeds max size %d\n", len,
|
|
|
|
SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
|
|
|
|
} else {
|
|
|
|
for (int i = 0; i < len; i++) {
|
|
|
|
m_data->m_testBlock1->m_bulletStreamDataClientToServer[i] = data[i];
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2016-05-31 17:23:04 +00:00
|
|
|
void PhysicsClientSharedMemory::getCachedCameraImage(struct b3CameraImageData* cameraData)
|
|
|
|
{
|
|
|
|
cameraData->m_pixelWidth = m_data->m_cachedCameraPixelsWidth;
|
|
|
|
cameraData->m_pixelHeight = m_data->m_cachedCameraPixelsHeight;
|
2016-08-12 20:55:37 +00:00
|
|
|
cameraData->m_depthValues = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0;
|
2016-05-31 17:23:04 +00:00
|
|
|
cameraData->m_rgbColorData = m_data->m_cachedCameraPixelsRGBA.size() ? &m_data->m_cachedCameraPixelsRGBA[0] : 0;
|
2016-08-11 21:55:30 +00:00
|
|
|
cameraData->m_segmentationMaskValues = m_data->m_cachedSegmentationMaskBuffer.size()?&m_data->m_cachedSegmentationMaskBuffer[0] : 0;
|
2016-05-31 17:23:04 +00:00
|
|
|
}
|
|
|
|
|
2016-09-01 20:30:07 +00:00
|
|
|
void PhysicsClientSharedMemory::getCachedContactPointInformation(struct b3ContactInformation* contactPointData)
|
|
|
|
{
|
|
|
|
contactPointData->m_numContactPoints = m_data->m_cachedContactPoints.size();
|
|
|
|
contactPointData->m_contactPointData = contactPointData->m_numContactPoints? &m_data->m_cachedContactPoints[0] : 0;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
2016-11-10 19:22:22 +00:00
|
|
|
void PhysicsClientSharedMemory::getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects)
|
|
|
|
{
|
|
|
|
overlappingObjects->m_numOverlappingObjects = m_data->m_cachedOverlappingObjects.size();
|
|
|
|
overlappingObjects->m_overlappingObjects = m_data->m_cachedOverlappingObjects.size() ?
|
|
|
|
&m_data->m_cachedOverlappingObjects[0] : 0;
|
|
|
|
}
|
|
|
|
|
2016-12-27 03:40:09 +00:00
|
|
|
void PhysicsClientSharedMemory::getCachedVREvents(struct b3VREventsData* vrEventsData)
|
|
|
|
{
|
|
|
|
vrEventsData->m_numControllerEvents = m_data->m_cachedVREvents.size();
|
|
|
|
vrEventsData->m_controllerEvents = vrEventsData->m_numControllerEvents?
|
|
|
|
&m_data->m_cachedVREvents[0] : 0;
|
|
|
|
}
|
|
|
|
|
2017-03-02 20:33:22 +00:00
|
|
|
void PhysicsClientSharedMemory::getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData)
|
|
|
|
{
|
|
|
|
keyboardEventsData->m_numKeyboardEvents = m_data->m_cachedKeyboardEvents.size();
|
|
|
|
keyboardEventsData->m_keyboardEvents = keyboardEventsData->m_numKeyboardEvents?
|
|
|
|
&m_data->m_cachedKeyboardEvents[0] : 0;
|
|
|
|
}
|
|
|
|
|
2016-12-27 03:40:09 +00:00
|
|
|
void PhysicsClientSharedMemory::getCachedRaycastHits(struct b3RaycastInformation* raycastHits)
|
|
|
|
{
|
|
|
|
raycastHits->m_numRayHits = m_data->m_raycastHits.size();
|
|
|
|
raycastHits->m_rayHits = raycastHits->m_numRayHits? &m_data->m_raycastHits[0] : 0;
|
|
|
|
}
|
|
|
|
|
2016-11-10 19:22:22 +00:00
|
|
|
|
2016-10-19 05:05:28 +00:00
|
|
|
void PhysicsClientSharedMemory::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo)
|
|
|
|
{
|
|
|
|
visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size();
|
|
|
|
visualShapesInfo->m_visualShapeData = visualShapesInfo->m_numVisualShapes ? &m_data->m_cachedVisualShapes[0] : 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2015-09-25 05:50:34 +00:00
|
|
|
const float* PhysicsClientSharedMemory::getDebugLinesFrom() const {
|
|
|
|
if (m_data->m_debugLinesFrom.size()) {
|
|
|
|
return &m_data->m_debugLinesFrom[0].m_x;
|
|
|
|
}
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
const float* PhysicsClientSharedMemory::getDebugLinesTo() const {
|
|
|
|
if (m_data->m_debugLinesTo.size()) {
|
|
|
|
return &m_data->m_debugLinesTo[0].m_x;
|
|
|
|
}
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
const float* PhysicsClientSharedMemory::getDebugLinesColor() const {
|
|
|
|
if (m_data->m_debugLinesColor.size()) {
|
|
|
|
return &m_data->m_debugLinesColor[0].m_x;
|
|
|
|
}
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
int PhysicsClientSharedMemory::getNumDebugLines() const { return m_data->m_debugLinesFrom.size(); }
|
2016-12-27 03:40:09 +00:00
|
|
|
|
2017-02-24 23:34:11 +00:00
|
|
|
void PhysicsClientSharedMemory::setTimeOut(double timeOutInSeconds)
|
|
|
|
{
|
|
|
|
m_data->m_timeOutInSeconds = timeOutInSeconds;
|
|
|
|
}
|
|
|
|
double PhysicsClientSharedMemory::getTimeOut() const
|
|
|
|
{
|
|
|
|
return m_data->m_timeOutInSeconds;
|
|
|
|
}
|