diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 595f9de4e..154547836 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2648,6 +2648,7 @@ b3SharedMemoryCommandHandle b3StateLoggingCommandInit(b3PhysicsClientHandle phys command->m_type = CMD_STATE_LOGGING; command->m_updateFlags = 0; command->m_stateLoggingArguments.m_numBodyUniqueIds = 0; + command->m_stateLoggingArguments.m_deviceFilterType = VR_DEVICE_CONTROLLER; return (b3SharedMemoryCommandHandle)command; @@ -2768,6 +2769,20 @@ int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int ma return 0; } +int b3StateLoggingSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_STATE_LOGGING); + if (command->m_type == CMD_STATE_LOGGING) + { + command->m_updateFlags |= STATE_LOGGING_FILTER_DEVICE_TYPE; + command->m_stateLoggingArguments.m_deviceFilterType = deviceTypeFilter; + } + return 0; +} + + int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUid) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 223a562b8..e313d18e6 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -372,6 +372,7 @@ int b3StateLoggingSetLinkIndexA(b3SharedMemoryCommandHandle commandHandle, int l int b3StateLoggingSetLinkIndexB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB); int b3StateLoggingSetBodyAUniqueId(b3SharedMemoryCommandHandle commandHandle, int bodyAUniqueId); int b3StateLoggingSetBodyBUniqueId(b3SharedMemoryCommandHandle commandHandle, int bodyBUniqueId); +int b3StateLoggingSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter); int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle); int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUniqueId); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 906104ad7..436a2f5b2 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -661,13 +661,14 @@ struct VRControllerStateLogger : public InternalStateLogger { b3VRControllerEvents m_vrEvents; int m_loggingTimeStamp; - + int m_deviceTypeFilter; std::string m_fileName; FILE* m_logFileHandle; std::string m_structTypes; - VRControllerStateLogger(int loggingUniqueId, const std::string& fileName) + VRControllerStateLogger(int loggingUniqueId, int deviceTypeFilter, const std::string& fileName) :m_loggingTimeStamp(0), + m_deviceTypeFilter(deviceTypeFilter), m_fileName(fileName), m_logFileHandle(0) { @@ -716,66 +717,68 @@ struct VRControllerStateLogger : public InternalStateLogger int stepCount = m_loggingTimeStamp; float timeStamp = m_loggingTimeStamp*timeStep; - + for (int i=0;i=10) + //serverStatusOut.m_sendVREvents.m_controllerEvents[serverStatusOut.m_sendVREvents.m_numVRControllerEvents++] = event; + //log the event + logData.m_values.push_back(stepCount); + logData.m_values.push_back(timeStamp); + logData.m_values.push_back(event.m_controllerId); + logData.m_values.push_back(event.m_numMoveEvents); + logData.m_values.push_back(event.m_numButtonEvents); + logData.m_values.push_back(event.m_pos[0]); + logData.m_values.push_back(event.m_pos[1]); + logData.m_values.push_back(event.m_pos[2]); + logData.m_values.push_back(event.m_orn[0]); + logData.m_values.push_back(event.m_orn[1]); + logData.m_values.push_back(event.m_orn[2]); + logData.m_values.push_back(event.m_orn[3]); + logData.m_values.push_back(event.m_analogAxis); + int packedButtons[7]={0,0,0,0,0,0,0}; + + int packedButtonIndex = 0; + int packedButtonShift = 0; + //encode the 64 buttons into 7 int (3 bits each), each int stores 10 buttons + for (int b=0;b=7) + int buttonMask = event.m_buttons[b]; + buttonMask = buttonMask << (packedButtonShift*3); + packedButtons[packedButtonIndex] |= buttonMask; + packedButtonShift++; + + if (packedButtonShift>=10) { - btAssert(0); - break; + packedButtonShift=0; + packedButtonIndex++; + if (packedButtonIndex>=7) + { + btAssert(0); + break; + } } } - } - for (int b=0;b<7;b++) - { - logData.m_values.push_back(packedButtons[b]); - } + for (int b=0;b<7;b++) + { + logData.m_values.push_back(packedButtons[b]); + } - appendMinitaurLogData(m_logFileHandle, m_structTypes, logData); + appendMinitaurLogData(m_logFileHandle, m_structTypes, logData); - event.m_numButtonEvents = 0; - event.m_numMoveEvents = 0; - for (int b=0;bm_stateLoggersUniqueId++; - VRControllerStateLogger* logger = new VRControllerStateLogger(loggerUid,fileName); + int deviceFilterType = VR_DEVICE_CONTROLLER; + if (clientCmd.m_updateFlags & STATE_LOGGING_FILTER_DEVICE_TYPE) + { + deviceFilterType = clientCmd.m_stateLoggingArguments.m_deviceFilterType; + } + VRControllerStateLogger* logger = new VRControllerStateLogger(loggerUid,deviceFilterType, fileName); m_data->m_stateLoggers.push_back(logger); serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 5b847de65..2fbda032c 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -618,6 +618,7 @@ struct SendKeyboardEvents b3KeyboardEvent m_keyboardEvents[MAX_KEYBOARD_EVENTS]; }; + enum eVRCameraEnums { VR_CAMERA_ROOT_POSITION=1, @@ -635,6 +636,7 @@ enum eStateLoggingEnums STATE_LOGGING_FILTER_LINK_INDEX_B=32, STATE_LOGGING_FILTER_BODY_UNIQUE_ID_A=64, STATE_LOGGING_FILTER_BODY_UNIQUE_ID_B=128, + STATE_LOGGING_FILTER_DEVICE_TYPE=256 }; struct VRCameraState @@ -658,6 +660,7 @@ struct StateLoggingRequest int m_linkIndexB; // only if STATE_LOGGING_FILTER_LINK_INDEX_B flag is set int m_bodyUniqueIdA; // only if STATE_LOGGING_FILTER_BODY_UNIQUE_ID_A flag is set int m_bodyUniqueIdB; // only if STATE_LOGGING_FILTER_BODY_UNIQUE_ID_B flag is set + int m_deviceFilterType; //user to select (filter) which VR devices to log }; struct StateLoggingResultArgs diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index f03276239..cba2c39b3 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -2683,8 +2683,9 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb int bodyUniqueIdB = -1; int linkIndexA = -2; int linkIndexB = -2; + int deviceTypeFilter = -1; - static char* kwlist[] = {"loggingType", "fileName", "objectUniqueIds", "maxLogDof", "bodyUniqueIdA", "bodyUniqueIdB", "linkIndexA", "linkIndexB", "physicsClientId", NULL}; + static char* kwlist[] = {"loggingType", "fileName", "objectUniqueIds", "maxLogDof", "bodyUniqueIdA", "bodyUniqueIdB", "linkIndexA", "linkIndexB", "deviceTypeFilter", "physicsClientId", NULL}; int physicsClientId = 0; if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|Oiiiiii", kwlist, @@ -2741,6 +2742,11 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb b3StateLoggingSetLinkIndexB(commandHandle, linkIndexB); } + if (deviceTypeFilter>=0) + { + b3StateLoggingSetDeviceTypeFilter(commandHandle,deviceTypeFilter); + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); if (statusType == CMD_STATE_LOGGING_START_COMPLETED)