From 0d83667817f50093852c053c4cad4665b3e362b9 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Sun, 2 Apr 2017 15:45:48 -0700 Subject: [PATCH] Add C API to log contact points. --- examples/SharedMemory/PhysicsClientC_API.cpp | 52 +++++++++++++++++++ examples/SharedMemory/PhysicsClientC_API.h | 4 ++ .../PhysicsServerCommandProcessor.cpp | 28 +++++++++- examples/SharedMemory/SharedMemoryCommands.h | 14 +++-- 4 files changed, 94 insertions(+), 4 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index b0695cd20..ca7da8509 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2659,6 +2659,58 @@ int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHa return 0; } +int b3StateLoggingSetLinkIndexA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA) +{ + 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_LINK_INDEX_A; + command->m_stateLoggingArguments.m_linkIndexA = linkIndexA; + } + return 0; +} + +int b3StateLoggingSetLinkIndexB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB) +{ + 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_LINK_INDEX_B; + command->m_stateLoggingArguments.m_linkIndexB = linkIndexB; + } + return 0; +} + +int b3StateLoggingSetBodyIndexA(b3SharedMemoryCommandHandle commandHandle, int bodyIndexA) +{ + 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_BODY_INDEX_A; + command->m_stateLoggingArguments.m_bodyIndexA = bodyIndexA; + } + return 0; +} + +int b3StateLoggingSetBodyIndexB(b3SharedMemoryCommandHandle commandHandle, int bodyIndexB) +{ + 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_BODY_INDEX_B; + command->m_stateLoggingArguments.m_bodyIndexB = bodyIndexB; + } + return 0; +} + int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int maxLogDof) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 35071ae34..29efca7d4 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -363,6 +363,10 @@ b3SharedMemoryCommandHandle b3StateLoggingCommandInit(b3PhysicsClientHandle phys int b3StateLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName); int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId); int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int maxLogDof); +int b3StateLoggingSetLinkIndexA(b3SharedMemoryStatusHandle commandHandle, int linkIndexA); +int b3StateLoggingSetLinkIndexB(b3SharedMemoryStatusHandle commandHandle, int linkIndexB); +int b3StateLoggingSetBodyIndexA(b3SharedMemoryStatusHandle commandHandle, int bodyIndexA); +int b3StateLoggingSetBodyIndexB(b3SharedMemoryStatusHandle commandHandle, int bodyIndexB); int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle); int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUniqueId); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 0b093eef7..691728a23 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2275,7 +2275,33 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; } - + if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_CONTACT_POINTS) + { + std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName; + int loggerUid = m_data->m_stateLoggersUniqueId++; + ContactPointsStateLogger* logger = new ContactPointsStateLogger(loggerUid,fileName,m_data->m_dynamicsWorld); + if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_LINK_INDEX_A) && clientCmd.m_stateLoggingArguments.m_linkIndexA >= -1) + { + logger->m_filterLinkA = true; + logger->m_linkIndexA = clientCmd.m_stateLoggingArguments.m_linkIndexA; + } + if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_LINK_INDEX_B) && clientCmd.m_stateLoggingArguments.m_linkIndexB >= -1) + { + logger->m_filterLinkB = true; + logger->m_linkIndexB = clientCmd.m_stateLoggingArguments.m_linkIndexB; + } + if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_BODY_INDEX_A) && clientCmd.m_stateLoggingArguments.m_bodyIndexA > -1) + { + logger->m_bodyIndexA = clientCmd.m_stateLoggingArguments.m_bodyIndexA; + } + if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_BODY_INDEX_B) && clientCmd.m_stateLoggingArguments.m_bodyIndexB > -1) + { + logger->m_bodyIndexB = clientCmd.m_stateLoggingArguments.m_bodyIndexB; + } + m_data->m_stateLoggers.push_back(logger); + serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; + serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; + } if (clientCmd.m_stateLoggingArguments.m_logType ==STATE_LOGGING_VR_CONTROLLERS) { std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index f5038c6a4..f3a54c2e8 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -628,7 +628,11 @@ enum eStateLoggingEnums STATE_LOGGING_START_LOG=1, STATE_LOGGING_STOP_LOG=2, STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID=4, - STATE_LOGGING_MAX_LOG_DOF=8 + STATE_LOGGING_MAX_LOG_DOF=8, + STATE_LOGGING_FILTER_LINK_INDEX_A=16, + STATE_LOGGING_FILTER_LINK_INDEX_B=32, + STATE_LOGGING_FILTER_BODY_INDEX_A=64, + STATE_LOGGING_FILTER_BODY_INDEX_B=128, }; struct VRCameraState @@ -643,11 +647,15 @@ struct VRCameraState struct StateLoggingRequest { char m_fileName[MAX_FILENAME_LENGTH]; - int m_logType;//Minitaur, generic robot, VR states - int m_numBodyUniqueIds;////only if ROBOT_LOGGING_FILTER_OBJECT_UNIQUE_ID flag is set + int m_logType;//Minitaur, generic robot, VR states, contact points + int m_numBodyUniqueIds;////only if STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID flag is set int m_bodyUniqueIds[MAX_SDF_BODIES]; int m_loggingUniqueId; int m_maxLogDof; + int m_linkIndexA; // only if STATE_LOGGING_FILTER_LINK_INDEX_A flag is set + int m_linkIndexB; // only if STATE_LOGGING_FILTER_LINK_INDEX_B flag is set + int m_bodyIndexA; + int m_bodyIndexB; }; struct StateLoggingResultArgs