Merge remote-tracking branch 'origin/master'
This commit is contained in:
@@ -30,7 +30,27 @@ b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClien
|
||||
return (b3SharedMemoryCommandHandle) command;
|
||||
}
|
||||
|
||||
|
||||
b3SharedMemoryCommandHandle b3SaveWorldCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient* ) physClient;
|
||||
b3Assert(cl);
|
||||
b3Assert(cl->canSubmitCommand());
|
||||
|
||||
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
|
||||
b3Assert(command);
|
||||
command->m_type = CMD_SAVE_WORLD;
|
||||
int len = strlen(sdfFileName);
|
||||
if (len<MAX_SDF_FILENAME_LENGTH)
|
||||
{
|
||||
strcpy(command->m_sdfArguments.m_sdfFileName,sdfFileName);
|
||||
} else
|
||||
{
|
||||
command->m_sdfArguments.m_sdfFileName[0] = 0;
|
||||
}
|
||||
command->m_updateFlags = SDF_ARGS_FILE_NAME;
|
||||
|
||||
return (b3SharedMemoryCommandHandle) command;
|
||||
}
|
||||
|
||||
b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName)
|
||||
{
|
||||
|
||||
@@ -146,6 +146,9 @@ int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle status
|
||||
b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
|
||||
int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
|
||||
|
||||
b3SharedMemoryCommandHandle b3SaveWorldCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
|
||||
|
||||
|
||||
///The b3JointControlCommandInit method is obsolete, use b3JointControlCommandInit2 instead
|
||||
b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode);
|
||||
|
||||
|
||||
@@ -5,6 +5,7 @@
|
||||
#include "../CommonInterfaces/Common2dCanvasInterface.h"
|
||||
#include "SharedMemoryCommon.h"
|
||||
#include "../CommonInterfaces/CommonParameterInterface.h"
|
||||
|
||||
#include "PhysicsClientC_API.h"
|
||||
#include "PhysicsClient.h"
|
||||
//#include "SharedMemoryCommands.h"
|
||||
@@ -451,6 +452,12 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
|
||||
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
|
||||
break;
|
||||
}
|
||||
case CMD_SAVE_WORLD:
|
||||
{
|
||||
b3SharedMemoryCommandHandle commandHandle = b3SaveWorldCommandInit(m_physicsClientHandle, "saveWorld.py");
|
||||
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
b3Error("Unknown buttonId");
|
||||
@@ -525,6 +532,7 @@ void PhysicsClientExample::createButtons()
|
||||
|
||||
createButton("Load URDF",CMD_LOAD_URDF, isTrigger);
|
||||
createButton("Load SDF",CMD_LOAD_SDF, isTrigger);
|
||||
createButton("Save World",CMD_SAVE_WORLD, isTrigger);
|
||||
createButton("Get Camera Image",CMD_REQUEST_CAMERA_IMAGE_DATA,isTrigger);
|
||||
createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger);
|
||||
createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM, isTrigger);
|
||||
|
||||
@@ -615,9 +615,27 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
|
||||
b3Warning("Contact Point Information Request failed");
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
case CMD_SAVE_WORLD_COMPLETED:
|
||||
break;
|
||||
|
||||
case CMD_SAVE_WORLD_FAILED:
|
||||
{
|
||||
b3Warning("Saving world failed");
|
||||
break;
|
||||
}
|
||||
case CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED:
|
||||
{
|
||||
break;
|
||||
}
|
||||
case CMD_CALCULATE_INVERSE_KINEMATICS_FAILED:
|
||||
{
|
||||
b3Warning("Calculate Inverse Kinematics Request failed");
|
||||
break;
|
||||
}
|
||||
|
||||
default: {
|
||||
b3Error("Unknown server status\n");
|
||||
b3Error("Unknown server status %d\n", serverCmd.m_type);
|
||||
btAssert(0);
|
||||
}
|
||||
};
|
||||
|
||||
@@ -31,6 +31,7 @@
|
||||
#include "BulletSoftBody/btSoftBodyHelpers.h"
|
||||
#include "../SoftDemo/BunnyMesh.h"
|
||||
|
||||
|
||||
//@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet!
|
||||
btVector3 gLastPickPos(0, 0, 0);
|
||||
bool gCloseToKuka=false;
|
||||
@@ -305,6 +306,12 @@ struct CommandLogPlayback
|
||||
}
|
||||
};
|
||||
|
||||
struct SaveWorldObjectData
|
||||
{
|
||||
b3AlignedObjectArray<int> m_bodyUniqueIds;
|
||||
std::string m_fileName;
|
||||
};
|
||||
|
||||
struct PhysicsServerCommandProcessorInternalData
|
||||
{
|
||||
///handle management
|
||||
@@ -401,6 +408,8 @@ struct PhysicsServerCommandProcessorInternalData
|
||||
btMultiBody* m_kukaGripperMultiBody;
|
||||
btMultiBodyPoint2Point* m_kukaGripperRevolute1;
|
||||
btMultiBodyPoint2Point* m_kukaGripperRevolute2;
|
||||
|
||||
|
||||
int m_huskyId;
|
||||
int m_KukaId;
|
||||
int m_sphereId;
|
||||
@@ -414,7 +423,7 @@ struct PhysicsServerCommandProcessorInternalData
|
||||
btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
|
||||
btHashMap<btHashPtr, btInverseDynamics::MultiBodyTree*> m_inverseDynamicsBodies;
|
||||
btHashMap<btHashPtr, IKTrajectoryHelper*> m_inverseKinematicsHelpers;
|
||||
|
||||
b3AlignedObjectArray<SaveWorldObjectData> m_saveWorldBodyData;
|
||||
|
||||
|
||||
btAlignedObjectArray<btBulletWorldImporter*> m_worldImporters;
|
||||
@@ -457,6 +466,10 @@ struct PhysicsServerCommandProcessorInternalData
|
||||
:m_hasGround(false),
|
||||
m_gripperRigidbodyFixed(0),
|
||||
m_gripperMultiBody(0),
|
||||
m_kukaGripperFixed(0),
|
||||
m_kukaGripperMultiBody(0),
|
||||
m_kukaGripperRevolute1(0),
|
||||
m_kukaGripperRevolute2(0),
|
||||
m_allowRealTimeSimulation(false),
|
||||
m_huskyId(-1),
|
||||
m_KukaId(-1),
|
||||
@@ -817,6 +830,9 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
|
||||
{
|
||||
b3Printf("loaded %s OK!", fileName);
|
||||
}
|
||||
SaveWorldObjectData sd;
|
||||
sd.m_fileName = fileName;
|
||||
|
||||
|
||||
for (int m =0; m<u2b.getNumModels();m++)
|
||||
{
|
||||
@@ -830,6 +846,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
|
||||
|
||||
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
|
||||
|
||||
sd.m_bodyUniqueIds.push_back(bodyUniqueId);
|
||||
|
||||
u2b.setBodyUniqueId(bodyUniqueId);
|
||||
{
|
||||
@@ -860,6 +877,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
|
||||
if (mb)
|
||||
mb->setUserIndex2(bodyUniqueId);
|
||||
|
||||
|
||||
if (mb)
|
||||
{
|
||||
bodyHandle->m_multiBody = mb;
|
||||
@@ -904,6 +922,9 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
m_data->m_saveWorldBodyData.push_back(sd);
|
||||
|
||||
}
|
||||
return loadOk;
|
||||
}
|
||||
@@ -936,6 +957,14 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
|
||||
if (bodyUniqueIdPtr)
|
||||
*bodyUniqueIdPtr= bodyUniqueId;
|
||||
|
||||
//quick prototype of 'save world' for crude world editing
|
||||
{
|
||||
SaveWorldObjectData sd;
|
||||
sd.m_fileName = fileName;
|
||||
sd.m_bodyUniqueIds.push_back(bodyUniqueId);
|
||||
m_data->m_saveWorldBodyData.push_back(sd);
|
||||
}
|
||||
|
||||
u2b.setBodyUniqueId(bodyUniqueId);
|
||||
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
|
||||
|
||||
@@ -1342,6 +1371,154 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
hasStatus = true;
|
||||
break;
|
||||
}
|
||||
case CMD_SAVE_WORLD:
|
||||
{
|
||||
///this is a very rudimentary way to save the state of the world, for scene authoring
|
||||
///many todo's, for example save the state of motor controllers etc.
|
||||
|
||||
|
||||
{
|
||||
//saveWorld(clientCmd.m_sdfArguments.m_sdfFileName);
|
||||
|
||||
FILE* f = fopen(clientCmd.m_sdfArguments.m_sdfFileName,"w");
|
||||
if (f)
|
||||
{
|
||||
char line[1024];
|
||||
{
|
||||
sprintf(line,"import pybullet as p\n");
|
||||
int len = strlen(line);
|
||||
fwrite(line,len,1,f);
|
||||
}
|
||||
{
|
||||
sprintf(line,"p.connect(p.SHARED_MEMORY)\n");
|
||||
int len = strlen(line);
|
||||
fwrite(line,len,1,f);
|
||||
}
|
||||
//for each objects ...
|
||||
for (int i=0;i<m_data->m_saveWorldBodyData.size();i++)
|
||||
{
|
||||
SaveWorldObjectData& sd = m_data->m_saveWorldBodyData[i];
|
||||
|
||||
for (int i=0;i<sd.m_bodyUniqueIds.size();i++)
|
||||
{
|
||||
{
|
||||
int bodyUniqueId = sd.m_bodyUniqueIds[i];
|
||||
InteralBodyData* body = m_data->getHandle(bodyUniqueId);
|
||||
if (body)
|
||||
{
|
||||
if (body->m_multiBody)
|
||||
{
|
||||
btMultiBody* mb = body->m_multiBody;
|
||||
|
||||
btTransform comTr = mb->getBaseWorldTransform();
|
||||
btTransform tr = comTr * body->m_rootLocalInertialFrame.inverse();
|
||||
|
||||
if (strstr(sd.m_fileName.c_str(),".urdf"))
|
||||
{
|
||||
sprintf(line,"objects = [p.loadURDF(\"%s\", %f,%f,%f,%f,%f,%f,%f)]\n",sd.m_fileName.c_str(),
|
||||
tr.getOrigin()[0],tr.getOrigin()[1],tr.getOrigin()[2],
|
||||
tr.getRotation()[0],tr.getRotation()[1],tr.getRotation()[2],tr.getRotation()[3]);
|
||||
int len = strlen(line);
|
||||
fwrite(line,len,1,f);
|
||||
}
|
||||
|
||||
if (strstr(sd.m_fileName.c_str(),".sdf") && i==0)
|
||||
{
|
||||
sprintf(line,"objects = p.loadSDF(\"%s\")\n",sd.m_fileName.c_str());
|
||||
int len = strlen(line);
|
||||
fwrite(line,len,1,f);
|
||||
}
|
||||
|
||||
if (strstr(sd.m_fileName.c_str(),".sdf") || ((strstr(sd.m_fileName.c_str(),".urdf")) && mb->getNumLinks()) )
|
||||
{
|
||||
sprintf(line,"ob = objects[%d]\n",i);
|
||||
int len = strlen(line);
|
||||
fwrite(line,len,1,f);
|
||||
}
|
||||
|
||||
if (strstr(sd.m_fileName.c_str(),".sdf"))
|
||||
{
|
||||
sprintf(line,"p.resetBasePositionAndOrientation(ob,[%f,%f,%f],[%f,%f,%f,%f])\n",
|
||||
comTr.getOrigin()[0],comTr.getOrigin()[1],comTr.getOrigin()[2],
|
||||
comTr.getRotation()[0],comTr.getRotation()[1],comTr.getRotation()[2],comTr.getRotation()[3]);
|
||||
int len = strlen(line);
|
||||
fwrite(line,len,1,f);
|
||||
}
|
||||
|
||||
if (mb->getNumLinks())
|
||||
{
|
||||
{
|
||||
sprintf(line,"jointPositions=[");
|
||||
int len = strlen(line);
|
||||
fwrite(line,len,1,f);
|
||||
}
|
||||
|
||||
for (int i=0;i<mb->getNumLinks();i++)
|
||||
{
|
||||
btScalar jointPos = mb->getJointPosMultiDof(i)[0];
|
||||
if (i<mb->getNumLinks()-1)
|
||||
{
|
||||
sprintf(line," %f,",jointPos);
|
||||
int len = strlen(line);
|
||||
fwrite(line,len,1,f);
|
||||
} else
|
||||
{
|
||||
sprintf(line," %f ",jointPos);
|
||||
int len = strlen(line);
|
||||
fwrite(line,len,1,f);
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
sprintf(line,"]\nfor jointIndex in range (p.getNumJoints(ob)):\n\tp.resetJointState(ob,jointIndex,jointPositions[jointIndex])\n\n");
|
||||
int len = strlen(line);
|
||||
fwrite(line,len,1,f);
|
||||
}
|
||||
}
|
||||
} else
|
||||
{
|
||||
//todo: btRigidBody/btSoftBody etc case
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
//for URDF, load at origin, then reposition...
|
||||
|
||||
|
||||
struct SaveWorldObjectData
|
||||
{
|
||||
b3AlignedObjectArray<int> m_bodyUniqueIds;
|
||||
std::string m_fileName;
|
||||
};
|
||||
}
|
||||
|
||||
{
|
||||
btVector3 grav=this->m_data->m_dynamicsWorld->getGravity();
|
||||
sprintf(line,"p.setGravity(%f,%f,%f)\n",grav[0],grav[1],grav[2]);
|
||||
int len = strlen(line);
|
||||
fwrite(line,len,1,f);
|
||||
}
|
||||
|
||||
|
||||
{
|
||||
sprintf(line,"p.stepSimulation()\np.disconnect()\n");
|
||||
int len = strlen(line);
|
||||
fwrite(line,len,1,f);
|
||||
}
|
||||
fclose(f);
|
||||
}
|
||||
|
||||
|
||||
serverStatusOut.m_type = CMD_SAVE_WORLD_COMPLETED;
|
||||
hasStatus = true;
|
||||
break;
|
||||
}
|
||||
serverStatusOut.m_type = CMD_SAVE_WORLD_FAILED;
|
||||
hasStatus = true;
|
||||
break;
|
||||
}
|
||||
case CMD_LOAD_SDF:
|
||||
{
|
||||
const SdfArgs& sdfArgs = clientCmd.m_sdfArguments;
|
||||
@@ -1846,16 +2023,16 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
btVector3 linkLocalInertialOrigin = body->m_linkLocalInertialFrames[l].getOrigin();
|
||||
btQuaternion linkLocalInertialRotation = body->m_linkLocalInertialFrames[l].getRotation();
|
||||
|
||||
btVector3 linkOrigin = mb->getLink(l).m_cachedWorldTransform.getOrigin();
|
||||
btQuaternion linkRotation = mb->getLink(l).m_cachedWorldTransform.getRotation();
|
||||
btVector3 linkCOMOrigin = mb->getLink(l).m_cachedWorldTransform.getOrigin();
|
||||
btQuaternion linkCOMRotation = mb->getLink(l).m_cachedWorldTransform.getRotation();
|
||||
|
||||
serverCmd.m_sendActualStateArgs.m_linkState[l*7+0] = linkOrigin.getX();
|
||||
serverCmd.m_sendActualStateArgs.m_linkState[l*7+1] = linkOrigin.getY();
|
||||
serverCmd.m_sendActualStateArgs.m_linkState[l*7+2] = linkOrigin.getZ();
|
||||
serverCmd.m_sendActualStateArgs.m_linkState[l*7+3] = linkRotation.x();
|
||||
serverCmd.m_sendActualStateArgs.m_linkState[l*7+4] = linkRotation.y();
|
||||
serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = linkRotation.z();
|
||||
serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = linkRotation.w();
|
||||
serverCmd.m_sendActualStateArgs.m_linkState[l*7+0] = linkCOMOrigin.getX();
|
||||
serverCmd.m_sendActualStateArgs.m_linkState[l*7+1] = linkCOMOrigin.getY();
|
||||
serverCmd.m_sendActualStateArgs.m_linkState[l*7+2] = linkCOMOrigin.getZ();
|
||||
serverCmd.m_sendActualStateArgs.m_linkState[l*7+3] = linkCOMRotation.x();
|
||||
serverCmd.m_sendActualStateArgs.m_linkState[l*7+4] = linkCOMRotation.y();
|
||||
serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = linkCOMRotation.z();
|
||||
serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = linkCOMRotation.w();
|
||||
|
||||
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+0] = linkLocalInertialOrigin.getX();
|
||||
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+1] = linkLocalInertialOrigin.getY();
|
||||
@@ -2744,12 +2921,14 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
}
|
||||
ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]);
|
||||
}
|
||||
|
||||
btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse();
|
||||
|
||||
btVector3DoubleData endEffectorWorldPosition;
|
||||
btVector3DoubleData endEffectorWorldOrientation;
|
||||
|
||||
btVector3 endEffectorPosWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getOrigin();
|
||||
btQuaternion endEffectorOriWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getRotation();
|
||||
btVector3 endEffectorPosWorld = endEffectorTransformWorld.getOrigin();
|
||||
btQuaternion endEffectorOriWorld = endEffectorTransformWorld.getRotation();
|
||||
btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w());
|
||||
|
||||
endEffectorPosWorld.serializeDouble(endEffectorWorldPosition);
|
||||
@@ -3000,6 +3179,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
|
||||
gBufferServerToClient.resize(SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
|
||||
int bodyId = 0;
|
||||
|
||||
|
||||
if (gCreateObjectSimVR >= 0)
|
||||
{
|
||||
gCreateObjectSimVR = -1;
|
||||
@@ -3189,7 +3369,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
|
||||
//loadUrdf("rook.urdf", btVector3(-1.2, 0, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
|
||||
//loadUrdf("knight.urdf", btVector3(-1.2, 0.2, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
|
||||
|
||||
loadUrdf("husky/husky.urdf", btVector3(2, -5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
|
||||
//loadUrdf("husky/husky.urdf", btVector3(2, -5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
|
||||
m_data->m_huskyId = bodyId;
|
||||
|
||||
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
|
||||
|
||||
@@ -12,6 +12,12 @@
|
||||
#include "Bullet3Common/b3Matrix3x3.h"
|
||||
#include "../Utils/b3Clock.h"
|
||||
#include "../MultiThreading/b3ThreadSupportInterface.h"
|
||||
#ifdef BT_ENABLE_VR
|
||||
#include "../RenderingExamples/TinyVRGui.h"
|
||||
#endif//BT_ENABLE_VR
|
||||
|
||||
|
||||
#include "../CommonInterfaces/CommonParameterInterface.h"
|
||||
|
||||
#define MAX_VR_CONTROLLERS 8
|
||||
|
||||
@@ -546,6 +552,10 @@ class PhysicsServerExample : public SharedMemoryCommon
|
||||
bool m_replay;
|
||||
int m_options;
|
||||
|
||||
#ifdef BT_ENABLE_VR
|
||||
TinyVRGui* m_tinyVrGui;
|
||||
#endif
|
||||
|
||||
public:
|
||||
|
||||
PhysicsServerExample(MultiThreadedOpenGLGuiHelper* helper, SharedMemoryInterface* sharedMem=0, int options=0);
|
||||
@@ -677,6 +687,9 @@ m_wantsShutdown(false),
|
||||
m_isConnected(false),
|
||||
m_replay(false),
|
||||
m_options(options)
|
||||
#ifdef BT_ENABLE_VR
|
||||
,m_tinyVrGui(0)
|
||||
#endif
|
||||
{
|
||||
m_multiThreadedHelper = helper;
|
||||
b3Printf("Started PhysicsServer\n");
|
||||
@@ -686,6 +699,9 @@ m_options(options)
|
||||
|
||||
PhysicsServerExample::~PhysicsServerExample()
|
||||
{
|
||||
#ifdef BT_ENABLE_VR
|
||||
delete m_tinyVrGui;
|
||||
#endif
|
||||
bool deInitializeSharedMemory = true;
|
||||
m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory);
|
||||
m_isConnected = false;
|
||||
@@ -942,7 +958,73 @@ extern double gSubStep;
|
||||
void PhysicsServerExample::renderScene()
|
||||
{
|
||||
B3_PROFILE("PhysicsServerExample::RenderScene");
|
||||
static char line0[1024];
|
||||
static char line1[1024];
|
||||
|
||||
if (gEnableRealTimeSimVR)
|
||||
{
|
||||
|
||||
static int frameCount=0;
|
||||
static btScalar prevTime = m_clock.getTimeSeconds();
|
||||
frameCount++;
|
||||
|
||||
static btScalar worseFps = 1000000;
|
||||
int numFrames = 200;
|
||||
static int count = 0;
|
||||
count++;
|
||||
|
||||
if (0 == (count & 1))
|
||||
{
|
||||
btScalar curTime = m_clock.getTimeSeconds();
|
||||
btScalar fps = 1. / (curTime - prevTime);
|
||||
prevTime = curTime;
|
||||
if (fps < worseFps)
|
||||
{
|
||||
worseFps = fps;
|
||||
}
|
||||
|
||||
if (count > numFrames)
|
||||
{
|
||||
count = 0;
|
||||
sprintf(line0, "fps:%f frame:%d", worseFps, frameCount / 2);
|
||||
sprintf(line1, "drop:%d tscale:%f dt:%f, substep %f)", gDroppedSimulationSteps, simTimeScalingFactor,gDtInSec, gSubStep);
|
||||
gDroppedSimulationSteps = 0;
|
||||
|
||||
worseFps = 1000000;
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef BT_ENABLE_VR
|
||||
if (m_tinyVrGui==0)
|
||||
{
|
||||
ComboBoxParams comboParams;
|
||||
comboParams.m_comboboxId = 0;
|
||||
comboParams.m_numItems = 0;
|
||||
comboParams.m_startItem = 0;
|
||||
comboParams.m_callback = 0;//MyComboBoxCallback;
|
||||
comboParams.m_userPointer = 0;//this;
|
||||
|
||||
m_tinyVrGui = new TinyVRGui(comboParams,this->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface());
|
||||
m_tinyVrGui->init();
|
||||
}
|
||||
|
||||
if (m_tinyVrGui)
|
||||
{
|
||||
|
||||
b3Transform tr;tr.setIdentity();
|
||||
tr.setOrigin(b3MakeVector3(gVRController2Pos[0],gVRController2Pos[1],gVRController2Pos[2]));
|
||||
tr.setRotation(b3Quaternion(gVRController2Orn[0],gVRController2Orn[1],gVRController2Orn[2],gVRController2Orn[3]));
|
||||
tr = tr*b3Transform(b3Quaternion(0,0,-SIMD_HALF_PI),b3MakeVector3(0,0,0));
|
||||
b3Scalar dt = 0.01;
|
||||
m_tinyVrGui->clearTextArea();
|
||||
|
||||
m_tinyVrGui->grapicalPrintf(line0,0,0,0,0,0,255);
|
||||
m_tinyVrGui->grapicalPrintf(line1,0,16,255,255,255,255);
|
||||
|
||||
m_tinyVrGui->tick(dt,tr);
|
||||
}
|
||||
#endif//BT_ENABLE_VR
|
||||
}
|
||||
///debug rendering
|
||||
//m_args[0].m_cs->lock();
|
||||
|
||||
@@ -993,38 +1075,6 @@ void PhysicsServerExample::renderScene()
|
||||
B3_PROFILE("Draw Debug HUD");
|
||||
//some little experiment to add text/HUD to a VR camera (HTC Vive/Oculus Rift)
|
||||
|
||||
static int frameCount=0;
|
||||
static btScalar prevTime = m_clock.getTimeSeconds();
|
||||
frameCount++;
|
||||
static char line0[1024];
|
||||
static char line1[1024];
|
||||
|
||||
static btScalar worseFps = 1000000;
|
||||
int numFrames = 200;
|
||||
static int count = 0;
|
||||
count++;
|
||||
|
||||
if (0 == (count & 1))
|
||||
{
|
||||
btScalar curTime = m_clock.getTimeSeconds();
|
||||
btScalar fps = 1. / (curTime - prevTime);
|
||||
prevTime = curTime;
|
||||
if (fps < worseFps)
|
||||
{
|
||||
worseFps = fps;
|
||||
}
|
||||
|
||||
if (count > numFrames)
|
||||
{
|
||||
count = 0;
|
||||
sprintf(line0, "Graphics FPS (worse) = %f, frame %d", worseFps, frameCount / 2);
|
||||
|
||||
sprintf(line1, "Physics Steps = %d, Drop = %d, time scale=%f, dt %f, Substep %f)", gNumSteps, gDroppedSimulationSteps, simTimeScalingFactor,gDtInSec, gSubStep);
|
||||
gDroppedSimulationSteps = 0;
|
||||
|
||||
worseFps = 1000000;
|
||||
}
|
||||
}
|
||||
|
||||
float pos[4];
|
||||
m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->getCameraTargetPosition(pos);
|
||||
|
||||
@@ -33,6 +33,7 @@ enum EnumSharedMemoryClientCommand
|
||||
CMD_CALCULATE_JACOBIAN,
|
||||
CMD_CREATE_JOINT,
|
||||
CMD_REQUEST_CONTACT_POINT_INFORMATION,
|
||||
CMD_SAVE_WORLD,
|
||||
//don't go beyond this command!
|
||||
CMD_MAX_CLIENT_COMMANDS,
|
||||
|
||||
@@ -75,6 +76,10 @@ enum EnumSharedMemoryServerStatus
|
||||
CMD_CONTACT_POINT_INFORMATION_FAILED,
|
||||
CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED,
|
||||
CMD_CALCULATE_INVERSE_KINEMATICS_FAILED,
|
||||
CMD_SAVE_WORLD_COMPLETED,
|
||||
CMD_SAVE_WORLD_FAILED,
|
||||
|
||||
//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
|
||||
CMD_MAX_SERVER_COMMANDS
|
||||
};
|
||||
|
||||
|
||||
@@ -231,6 +231,9 @@ if os.is("Windows") then
|
||||
"../ExampleBrowser/OpenGLGuiHelper.cpp",
|
||||
"../ExampleBrowser/GL_ShapeDrawer.cpp",
|
||||
"../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
|
||||
"../RenderingExamples/TinyVRGui.cpp",
|
||||
"../RenderingExamples/TimeSeriesCanvas.cpp",
|
||||
"../RenderingExamples/TimeSeriesFontData.cpp",
|
||||
"../ThirdPartyLibs/openvr/samples/shared/lodepng.cpp",
|
||||
"../ThirdPartyLibs/openvr/samples/shared/lodepng.h",
|
||||
"../ThirdPartyLibs/openvr/samples/shared/Matrices.cpp",
|
||||
|
||||
Reference in New Issue
Block a user