AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 22:00:05 +00:00
Code Issues Releases Wiki Activity

Merge branch 'master' of https://github.com/erwincoumans/bullet3

Browse Source
This commit is contained in:
Erwin Coumans 2018-07-24 15:21:12 +02:00
commit f0c8bbf1a1
26 changed files with 703 additions and 691 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
.ci/docker/env.list Normal file
View File

@ -0,0 +1,7 @@
TRAVIS_OS_NAME
TRAVIS_PULL_REQUEST
BUILD_NAME
CC
CXX
SUDO

18
.ci/docker/ubuntu-bionic Normal file
View File

@ -0,0 +1,18 @@
FROM ubuntu:bionic
RUN apt-get update -qq
RUN apt-get install -y \
build-essential \
clang \
cmake \
curl \
git \
libgl-dev \
libglu-dev \
libpython3-dev \
lsb-release \
pkg-config \
python3 \
python3-distutils \
software-properties-common \
sudo

17
.ci/docker/ubuntu-xenial Normal file
View File

@ -0,0 +1,17 @@
FROM ubuntu:xenial
RUN apt-get update -qq
RUN apt-get install -y \
build-essential \
clang \
cmake \
curl \
git \
libgl-dev \
libglu-dev \
libpython3-dev \
lsb-release \
pkg-config \
python3 \
software-properties-common \
sudo

30
.ci/script.sh Executable file
View File

@ -0,0 +1,30 @@
#!/usr/bin/env bash
set -ex
echo "CXX="$CXX
echo "CC="$CC
if [[ "$TRAVIS_OS_NAME" == "linux" && "$CXX" = "g++" ]]; then
$SUDO apt-get update
$SUDO apt-get install -y python3
$SUDO apt-get install -y python3-pip
$SUDO pip3 install -U wheel
$SUDO pip3 install -U setuptools
$SUDO python3 setup.py install
python3 examples/pybullet/unittests/unittests.py --verbose
python3 examples/pybullet/unittests/userDataTest.py --verbose
python3 examples/pybullet/unittests/saveRestoreStateTest.py --verbose
fi
cmake . -DBUILD_PYBULLET=ON -G"Unix Makefiles" #-DCMAKE_CXX_FLAGS=-Werror
make -j8
ctest -j8 --output-on-failure
# Build again with double precision
cmake . -G "Unix Makefiles" -DUSE_DOUBLE_PRECISION=ON #-DCMAKE_CXX_FLAGS=-Werror
make -j8
ctest -j8 --output-on-failure
# Build again with shared libraries
cmake . -G "Unix Makefiles" -DBUILD_SHARED_LIBS=ON
make -j8
ctest -j8 --output-on-failure
$SUDO make install

87
.travis.yml
View File

@ -1,35 +1,56 @@
language: cpp
os:
- linux
- osx
compiler:
- gcc
- clang
addons:
apt:
packages:
- python3
matrix:
include:
- os: linux
compiler: gcc
env:
- BUILD_NAME=TRUSTY_GCC
- SUDO=sudo
- os: linux
compiler: clang
env:
- BUILD_NAME=TRUSTY_CLANG
- SUDO=sudo
- os: linux
compiler: gcc
env:
- BUILD_NAME=XENIAL_GCC
- DOCKER_FILE="ubuntu-xenial"
services: docker
- os: linux
compiler: clang
env:
- BUILD_NAME=XENIAL_CLANG
- DOCKER_FILE="ubuntu-xenial"
services: docker
- os: linux
compiler: gcc
env:
- BUILD_NAME=BIONIC_GCC
- DOCKER_FILE="ubuntu-bionic"
services: docker
- os: linux
compiler: clang
env:
- BUILD_NAME=BIONIC_CLANG
- DOCKER_FILE="ubuntu-bionic"
services: docker
- os: osx
compiler: gcc
env:
- BUILD_NAME=OSX_GCC
- os: osx
compiler: clang
env:
- BUILD_NAME=OSX_CLANG
before_install:
- if [ -n "$DOCKER_FILE" ]; then
docker build -t "$DOCKER_FILE" -f ".ci/docker/$DOCKER_FILE" .;
docker run -itd -v $TRAVIS_BUILD_DIR:$TRAVIS_BUILD_DIR --env-file .ci/docker/env.list --name bullet-docker "$DOCKER_FILE";
fi
script:
- echo "CXX="$CXX
- echo "CC="$CC
- if [[ "$TRAVIS_OS_NAME" == "linux" && "$CXX" = "g++" ]]; then sudo apt-get install python3-pip; fi
- if [[ "$TRAVIS_OS_NAME" == "linux" && "$CXX" = "g++" ]]; then sudo pip3 install -U pip wheel; fi
- if [[ "$TRAVIS_OS_NAME" == "linux" && "$CXX" = "g++" ]]; then sudo pip3 install setuptools; fi
- if [[ "$TRAVIS_OS_NAME" == "linux" && "$CXX" = "g++" ]]; then sudo python3 setup.py install; fi
- if [[ "$TRAVIS_OS_NAME" == "linux" && "$CXX" = "g++" ]]; then python3 examples/pybullet/unittests/unittests.py --verbose; fi
- if [[ "$TRAVIS_OS_NAME" == "linux" && "$CXX" = "g++" ]]; then python3 examples/pybullet/unittests/userDataTest.py --verbose; fi
- if [[ "$TRAVIS_OS_NAME" == "linux" && "$CXX" = "g++" ]]; then python3 examples/pybullet/unittests/saveRestoreStateTest.py --verbose; fi
- cmake . -DBUILD_PYBULLET=ON -G"Unix Makefiles" #-DCMAKE_CXX_FLAGS=-Werror
- make -j8
- ctest -j8 --output-on-failure
# Build again with double precision
- cmake . -G "Unix Makefiles" -DUSE_DOUBLE_PRECISION=ON #-DCMAKE_CXX_FLAGS=-Werror
- make -j8
- ctest -j8 --output-on-failure
# Build again with shared libraries
- cmake . -G "Unix Makefiles" -DBUILD_SHARED_LIBS=ON
- make -j8
- ctest -j8 --output-on-failure
- sudo make install
- if [ -n "$DOCKER_FILE" ]; then
docker exec bullet-docker /bin/sh -c "cd $TRAVIS_BUILD_DIR && ./.ci/script.sh";
else
'.ci/script.sh';
fi

2
CMakeLists.txt
View File

@ -316,7 +316,7 @@ IF(BUILD_PYBULLET)
SET(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/build3/cmake ${CMAKE_MODULE_PATH})
OPTION(EXACT_PYTHON_VERSION "Require Python and match PYTHON_VERSION_PYBULLET exactly, e.g. 2.7.12" OFF)
IF(EXACT_PYTHON_VERSION)
set(EXACT_PYTHON_VERSION_FLAG EXACT REQUIRED)
set(EXACT_PYTHON_VERSION_FLAG EXACT REQUIRED)
ENDIF(EXACT_PYTHON_VERSION)
# first find the python interpreter
FIND_PACKAGE(PythonInterp ${PYTHON_VERSION_PYBULLET} ${EXACT_PYTHON_VERSION_FLAG})

2
examples/MultiBody/InvertedPendulumPDControl.cpp
View File

@ -268,7 +268,7 @@ btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GU
btVector3 posr = local_origin[i+1];
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()};
const btScalar quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()};
btCollisionShape* shape =0;
if (i==0)

2
examples/MultiBody/MultiBodyConstraintFeedback.cpp
View File

@ -320,7 +320,7 @@ void MultiBodyConstraintFeedbackSetup::initPhysics()
btVector3 posr = local_origin[i+1];
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()};
const btScalar quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()};
btCollisionShape* shape =0;
if (i==0)

8
examples/SharedMemory/PhysicsClient.h
View File

@ -75,10 +75,10 @@ public:
virtual void setTimeOut(double timeOutInSeconds) = 0;
virtual double getTimeOut() const = 0;
virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const = 0;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const = 0;
virtual int getNumUserData(int bodyUniqueId, int linkIndex) const = 0;
virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const = 0;
virtual bool getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const = 0;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const = 0;
virtual int getNumUserData(int bodyUniqueId) const = 0;
virtual void getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const = 0;
virtual void pushProfileTiming(const char* timingName)=0;
virtual void popProfileTiming()=0;

25
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -2848,7 +2848,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitSyncUserDataCommand(b3PhysicsCl
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const char* key, UserDataValueType valueType, int valueLength, const void *valueData) {
B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int visualShapeIndex, const char* key, UserDataValueType valueType, int valueLength, const void *valueData) {
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(strlen(key) < MAX_USER_DATA_KEY_LENGTH);
b3Assert(cl);
@ -2860,6 +2860,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsCli
command->m_type = CMD_ADD_USER_DATA;
command->m_addUserDataRequestArgs.m_bodyUniqueId = bodyUniqueId;
command->m_addUserDataRequestArgs.m_linkIndex = linkIndex;
command->m_addUserDataRequestArgs.m_visualShapeIndex = visualShapeIndex;
command->m_addUserDataRequestArgs.m_valueType = valueType;
command->m_addUserDataRequestArgs.m_valueLength = valueLength;
strcpy(command->m_addUserDataRequestArgs.m_key, key);
@ -2868,7 +2869,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsCli
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataId) {
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserDataCommand(b3PhysicsClientHandle physClient, int userDataId) {
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
@ -2876,29 +2877,27 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserDataCommand(b3Physics
b3Assert(command);
command->m_type = CMD_REMOVE_USER_DATA;
command->m_removeUserDataRequestArgs.m_bodyUniqueId = bodyUniqueId;
command->m_removeUserDataRequestArgs.m_linkIndex = linkIndex;
command->m_removeUserDataRequestArgs.m_userDataId = userDataId;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API int b3GetUserData(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue *valueOut)
B3_SHARED_API int b3GetUserData(b3PhysicsClientHandle physClient, int userDataId, struct b3UserDataValue *valueOut)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
if (cl)
{
return cl->getCachedUserData(bodyUniqueId, linkIndex, userDataId, *valueOut);
return cl->getCachedUserData(userDataId, *valueOut);
}
return false;
}
B3_SHARED_API int b3GetUserDataId(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const char *key)
B3_SHARED_API int b3GetUserDataId(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
if (cl)
{
return cl->getCachedUserDataId(bodyUniqueId, linkIndex, key);
return cl->getCachedUserDataId(bodyUniqueId, linkIndex, visualShapeIndex, key);
}
return -1;
}
@ -2909,27 +2908,27 @@ B3_SHARED_API int b3GetUserDataIdFromStatus(b3SharedMemoryStatusHandle statusHan
if (status)
{
btAssert(status->m_type == CMD_ADD_USER_DATA_COMPLETED);
return status->m_userDataResponseArgs.m_userDataGlobalId.m_userDataId;
return status->m_userDataResponseArgs.m_userDataId;
}
return -1;
}
B3_SHARED_API int b3GetNumUserData(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex)
B3_SHARED_API int b3GetNumUserData(b3PhysicsClientHandle physClient, int bodyUniqueId)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
if (cl)
{
return cl->getNumUserData(bodyUniqueId, linkIndex);
return cl->getNumUserData(bodyUniqueId);
}
return 0;
}
B3_SHARED_API void b3GetUserDataInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut)
B3_SHARED_API void b3GetUserDataInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
if (cl)
{
cl->getUserDataInfo(bodyUniqueId, linkIndex, userDataIndex, keyOut, userDataIdOut);
cl->getUserDataInfo(bodyUniqueId, userDataIndex, keyOut, userDataIdOut, linkIndexOut, visualShapeIndexOut);
}
}

12
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -115,14 +115,14 @@ B3_SHARED_API int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyUniqu
///user data handling
B3_SHARED_API b3SharedMemoryCommandHandle b3InitSyncUserDataCommand(b3PhysicsClientHandle physClient);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const char* key, enum UserDataValueType valueType, int valueLength, const void *valueData);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataId);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int visualShapeIndex, const char* key, enum UserDataValueType valueType, int valueLength, const void *valueData);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserDataCommand(b3PhysicsClientHandle physClient, int userDataId);
B3_SHARED_API int b3GetUserData(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue *valueOut);
B3_SHARED_API int b3GetUserDataId(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const char *key);
B3_SHARED_API int b3GetUserData(b3PhysicsClientHandle physClient, int userDataId, struct b3UserDataValue *valueOut);
B3_SHARED_API int b3GetUserDataId(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key);
B3_SHARED_API int b3GetUserDataIdFromStatus(b3SharedMemoryStatusHandle statusHandle);
B3_SHARED_API int b3GetNumUserData(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex);
B3_SHARED_API void b3GetUserDataInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut);
B3_SHARED_API int b3GetNumUserData(b3PhysicsClientHandle physClient, int bodyUniqueId);
B3_SHARED_API void b3GetUserDataInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut);
B3_SHARED_API b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex);
///given a body unique id and link index, return the dynamics information. See b3DynamicsInfo in SharedMemoryPublic.h

155
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@ -15,26 +15,12 @@
#include "LinearMath/btQuickprof.h"
struct UserDataCache
{
btHashMap<btHashInt, SharedMemoryUserData> m_userDataMap;
btHashMap<btHashString, int> m_keyToUserDataIdMap;
UserDataCache()
{
}
~UserDataCache()
{
}
};
struct BodyJointInfoCache
{
std::string m_baseName;
b3AlignedObjectArray<b3JointInfo> m_jointInfo;
std::string m_bodyName;
// Joint index -> user data.
btHashMap<btHashInt, UserDataCache> m_jointToUserDataMap;
btAlignedObjectArray<int> m_userDataIds;
~BodyJointInfoCache()
{
@ -78,7 +64,10 @@ struct PhysicsClientSharedMemoryInternalData {
btAlignedObjectArray<int> m_bodyIdsRequestInfo;
btAlignedObjectArray<int> m_constraintIdsRequestInfo;
btAlignedObjectArray<b3UserDataGlobalIdentifier> m_userDataIdsRequestInfo;
btAlignedObjectArray<int> m_userDataIdsRequestInfo;
btHashMap<btHashInt, SharedMemoryUserData> m_userDataMap;
btHashMap<SharedMemoryUserDataHashKey, int> m_userDataHandleLookup;
SharedMemoryStatus m_tempBackupServerStatus;
@ -243,6 +232,12 @@ void PhysicsClientSharedMemory::removeCachedBody(int bodyUniqueId)
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr)
{
for(int i=0; i<(*bodyJointsPtr)->m_userDataIds.size(); i++) {
const int userDataId = (*bodyJointsPtr)->m_userDataIds[i];
SharedMemoryUserData *userData = m_data->m_userDataMap[userDataId];
m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData));
m_data->m_userDataMap.remove(userDataId);
}
delete (*bodyJointsPtr);
m_data->m_bodyJointMap.remove(bodyUniqueId);
}
@ -264,6 +259,8 @@ void PhysicsClientSharedMemory::resetData()
}
m_data->m_bodyJointMap.clear();
m_data->m_userConstraintInfoMap.clear();
m_data->m_userDataHandleLookup.clear();
m_data->m_userDataMap.clear();
}
void PhysicsClientSharedMemory::setSharedMemoryKey(int key)
@ -1409,25 +1406,26 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i);
if (bodyJointsPtr && *bodyJointsPtr)
{
(*bodyJointsPtr)->m_jointToUserDataMap.clear();
(*bodyJointsPtr)->m_userDataIds.clear();
}
m_data->m_userDataMap.clear();
m_data->m_userDataHandleLookup.clear();
}
const int numIdentifiers = serverCmd.m_syncUserDataArgs.m_numUserDataIdentifiers;
if (numIdentifiers > 0) {
m_data->m_tempBackupServerStatus = m_data->m_lastServerStatus;
const b3UserDataGlobalIdentifier *identifiers = (b3UserDataGlobalIdentifier *)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
for (int i=0; i<numIdentifiers; i++) {
const int *identifiers = (int *)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
m_data->m_userDataIdsRequestInfo.reserve(numIdentifiers - 1);
// Store the identifiers that still need to be requested.
for (int i=0; i<numIdentifiers - 1; i++) {
m_data->m_userDataIdsRequestInfo.push_back(identifiers[i]);
}
// Request individual user data entries.
const b3UserDataGlobalIdentifier userDataGlobalId = m_data->m_userDataIdsRequestInfo[m_data->m_userDataIdsRequestInfo.size()-1];
m_data->m_userDataIdsRequestInfo.pop_back();
// Request individual user data entries, start with last identifier.
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
command.m_type = CMD_REQUEST_USER_DATA;
command.m_userDataRequestArgs = userDataGlobalId;
command.m_userDataRequestArgs.m_userDataId = identifiers[numIdentifiers - 1];
submitClientCommand(command);
return 0;
}
@ -1435,31 +1433,23 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
if (serverCmd.m_type == CMD_ADD_USER_DATA_COMPLETED || serverCmd.m_type == CMD_REQUEST_USER_DATA_COMPLETED) {
B3_PROFILE("CMD_ADD_USER_DATA_COMPLETED");
const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_userDataResponseArgs.m_userDataGlobalId;
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId];
const UserDataResponseArgs response = serverCmd.m_userDataResponseArgs;
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[response.m_bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr) {
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex];
if (!userDataCachePtr)
{
UserDataCache cache;
(*bodyJointsPtr)->m_jointToUserDataMap.insert(userDataGlobalId.m_linkIndex, cache);
}
userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex];
const char *dataStream = m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId];
if (userDataPtr) {
SharedMemoryUserData* userData = m_data->m_userDataMap[response.m_userDataId];
if (userData) {
// Only replace the value.
userDataPtr->replaceValue(dataStream, serverCmd.m_userDataResponseArgs.m_valueLength, serverCmd.m_userDataResponseArgs.m_valueType);
userData->replaceValue(dataStream, response.m_valueLength, response.m_valueType);
}
else {
// Add a new user data entry.
const char *key = serverCmd.m_userDataResponseArgs.m_key;
(userDataCachePtr)->m_userDataMap.insert(userDataGlobalId.m_userDataId, SharedMemoryUserData(key));
(userDataCachePtr)->m_keyToUserDataIdMap.insert(key, userDataGlobalId.m_userDataId);
userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId];
userDataPtr->replaceValue(dataStream, serverCmd.m_userDataResponseArgs.m_valueLength, serverCmd.m_userDataResponseArgs.m_valueType);
const char *key = response.m_key;
m_data->m_userDataMap.insert(response.m_userDataId, SharedMemoryUserData(key, response.m_bodyUniqueId, response.m_linkIndex, response.m_visualShapeIndex));
userData = m_data->m_userDataMap[response.m_userDataId];
userData->replaceValue(dataStream, response.m_valueLength, response.m_valueType);
m_data->m_userDataHandleLookup.insert(SharedMemoryUserDataHashKey(userData), response.m_userDataId);
(*bodyJointsPtr)->m_userDataIds.push_back(response.m_userDataId);
}
}
}
@ -1468,12 +1458,12 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
if (m_data->m_userDataIdsRequestInfo.size() > 0) {
// Request individual user data entries.
const b3UserDataGlobalIdentifier userDataGlobalId = m_data->m_userDataIdsRequestInfo[m_data->m_userDataIdsRequestInfo.size()-1];
const int userDataId = m_data->m_userDataIdsRequestInfo[m_data->m_userDataIdsRequestInfo.size()-1];
m_data->m_userDataIdsRequestInfo.pop_back();
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
command.m_type = CMD_REQUEST_USER_DATA;
command.m_userDataRequestArgs = userDataGlobalId;
command.m_userDataRequestArgs.m_userDataId = userDataId;
submitClientCommand(command);
return 0;
}
@ -1482,19 +1472,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
if (serverCmd.m_type == CMD_REMOVE_USER_DATA_COMPLETED) {
B3_PROFILE("CMD_REMOVE_USER_DATA_COMPLETED");
const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_removeUserDataResponseArgs;
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr) {
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex];
if (userDataCachePtr)
{
SharedMemoryUserData *userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId];
if (userDataPtr)
{
(userDataCachePtr)->m_keyToUserDataIdMap.remove((userDataPtr)->m_key.c_str());
(userDataCachePtr)->m_userDataMap.remove(userDataGlobalId.m_userDataId);
}
const int userDataId = serverCmd.m_removeUserDataResponseArgs.m_userDataId;
SharedMemoryUserData *userData = m_data->m_userDataMap[userDataId];
if (userData) {
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[userData->m_bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr) {
(*bodyJointsPtr)->m_userDataIds.remove(userDataId);
}
m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData));
m_data->m_userDataMap.remove(userDataId);
}
}
@ -1852,16 +1838,8 @@ double PhysicsClientSharedMemory::getTimeOut() const
return m_data->m_timeOutInSeconds;
}
bool PhysicsClientSharedMemory::getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const {
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (!bodyJointsPtr || !(*bodyJointsPtr)) {
return false;
}
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex];
if (!userDataCachePtr) {
return false;
}
SharedMemoryUserData *userDataPtr = (userDataCachePtr)->m_userDataMap[userDataId];
bool PhysicsClientSharedMemory::getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const {
SharedMemoryUserData *userDataPtr = m_data->m_userDataMap[userDataId];
if (!userDataPtr)
{
return false;
@ -1872,54 +1850,37 @@ bool PhysicsClientSharedMemory::getCachedUserData(int bodyUniqueId, int linkInde
return true;
}
int PhysicsClientSharedMemory::getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const {
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (!bodyJointsPtr || !(*bodyJointsPtr)) {
return -1;
}
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex];
if (!userDataCachePtr)
{
return -1;
}
int *userDataId = (userDataCachePtr)->m_keyToUserDataIdMap[key];
int PhysicsClientSharedMemory::getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const {
int* userDataId = m_data->m_userDataHandleLookup.find(SharedMemoryUserDataHashKey(key, bodyUniqueId, linkIndex, visualShapeIndex));
if (!userDataId) {
return -1;
}
return *userDataId;
}
int PhysicsClientSharedMemory::getNumUserData(int bodyUniqueId, int linkIndex) const {
int PhysicsClientSharedMemory::getNumUserData(int bodyUniqueId) const {
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (!bodyJointsPtr || !(*bodyJointsPtr)) {
return 0;
}
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex];
if (!userDataCachePtr)
{
return 0;
}
return (userDataCachePtr)->m_userDataMap.size();
return (*bodyJointsPtr)->m_userDataIds.size();
}
void PhysicsClientSharedMemory::getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const {
void PhysicsClientSharedMemory::getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const {
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (!bodyJointsPtr || !(*bodyJointsPtr))
if (!bodyJointsPtr || !(*bodyJointsPtr) || userDataIndex < 0 || userDataIndex > (*bodyJointsPtr)->m_userDataIds.size())
{
*keyOut = 0;
*userDataIdOut = -1;
return;
}
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex];
if (!userDataCachePtr || userDataIndex >= (userDataCachePtr)->m_userDataMap.size())
{
*keyOut = 0;
*userDataIdOut = -1;
return;
}
*userDataIdOut = (userDataCachePtr)->m_userDataMap.getKeyAtIndex(userDataIndex).getUid1();
SharedMemoryUserData *userDataPtr = (userDataCachePtr)->m_userDataMap.getAtIndex(userDataIndex);
*keyOut = (userDataPtr)->m_key.c_str();
int userDataId = (*bodyJointsPtr)->m_userDataIds[userDataIndex];
SharedMemoryUserData *userData = m_data->m_userDataMap[userDataId];
*userDataIdOut = userDataId;
*keyOut = userData->m_key.c_str();
*linkIndexOut = userData->m_linkIndex;
*visualShapeIndexOut = userData->m_visualShapeIndex;
}

8
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

@ -85,10 +85,10 @@ public:
virtual void setTimeOut(double timeOutInSeconds);
virtual double getTimeOut() const;
virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const;
virtual int getNumUserData(int bodyUniqueId, int linkIndex) const;
virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const;
virtual bool getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const;
virtual int getNumUserData(int bodyUniqueId) const;
virtual void getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const;
virtual void pushProfileTiming(const char* timingName);
virtual void popProfileTiming();

141
examples/SharedMemory/PhysicsDirect.cpp
View File

@ -16,24 +16,13 @@
#include "SharedMemoryUserData.h"
#include "LinearMath/btQuickprof.h"
struct UserDataCache {
btHashMap<btHashInt, SharedMemoryUserData> m_userDataMap;
btHashMap<btHashString, int> m_keyToUserDataIdMap;
~UserDataCache()
{
}
};
struct BodyJointInfoCache2
{
std::string m_baseName;
btAlignedObjectArray<b3JointInfo> m_jointInfo;
std::string m_bodyName;
// Joint index -> user data.
btHashMap<btHashInt, UserDataCache> m_jointToUserDataMap;
btAlignedObjectArray<int> m_userDataIds;
~BodyJointInfoCache2() {
}
@ -85,6 +74,9 @@ struct PhysicsDirectInternalData
btAlignedObjectArray<b3RayHitInfo> m_raycastHits;
btHashMap<btHashInt, SharedMemoryUserData> m_userDataMap;
btHashMap<SharedMemoryUserDataHashKey, int> m_userDataHandleLookup;
PhysicsCommandProcessorInterface* m_commandProcessor;
bool m_ownsCommandProcessor;
double m_timeOutInSeconds;
@ -685,34 +677,23 @@ void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemorySta
}
void PhysicsDirect::processAddUserData(const struct SharedMemoryStatus& serverCmd) {
const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_userDataResponseArgs.m_userDataGlobalId;
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr)
{
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex];
if (!userDataCachePtr)
{
UserDataCache cache;
(*bodyJointsPtr)->m_jointToUserDataMap.insert(userDataGlobalId.m_linkIndex, cache);
}
userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex];
const UserDataResponseArgs response = serverCmd.m_userDataResponseArgs;
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[response.m_bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr) {
const char *dataStream = m_data->m_bulletStreamDataServerToClient;
b3UserDataValue userDataValue;
userDataValue.m_type = serverCmd.m_userDataResponseArgs.m_valueType;
userDataValue.m_length = serverCmd.m_userDataResponseArgs.m_valueLength;
SharedMemoryUserData *userDataPtr = userDataCachePtr->m_userDataMap[userDataGlobalId.m_userDataId];
if (userDataPtr) {
SharedMemoryUserData* userData = m_data->m_userDataMap[response.m_userDataId];
if (userData) {
// Only replace the value.
(userDataPtr)->replaceValue(dataStream,serverCmd.m_userDataResponseArgs.m_valueLength,userDataValue.m_type);
userData->replaceValue(dataStream, response.m_valueLength, response.m_valueType);
}
else {
// Add a new user data entry.
(userDataCachePtr)->m_userDataMap.insert(userDataGlobalId.m_userDataId, SharedMemoryUserData(serverCmd.m_userDataResponseArgs.m_key));
userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId];
userDataPtr->replaceValue(dataStream,serverCmd.m_userDataResponseArgs.m_valueLength,userDataValue.m_type);
(userDataCachePtr)->m_keyToUserDataIdMap.insert(serverCmd.m_userDataResponseArgs.m_key, userDataGlobalId.m_userDataId);
const char *key = response.m_key;
m_data->m_userDataMap.insert(response.m_userDataId, SharedMemoryUserData(key, response.m_bodyUniqueId, response.m_linkIndex, response.m_visualShapeIndex));
userData = m_data->m_userDataMap[response.m_userDataId];
userData->replaceValue(dataStream, response.m_valueLength, response.m_valueType);
m_data->m_userDataHandleLookup.insert(SharedMemoryUserDataHashKey(userData), response.m_userDataId);
(*bodyJointsPtr)->m_userDataIds.push_back(response.m_userDataId);
}
}
}
@ -1152,16 +1133,18 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i);
if (bodyJointsPtr && *bodyJointsPtr)
{
(*bodyJointsPtr)->m_jointToUserDataMap.clear();
(*bodyJointsPtr)->m_userDataIds.clear();
}
m_data->m_userDataMap.clear();
m_data->m_userDataHandleLookup.clear();
}
const int numIdentifiers = serverCmd.m_syncUserDataArgs.m_numUserDataIdentifiers;
b3UserDataGlobalIdentifier *identifiers = new b3UserDataGlobalIdentifier[numIdentifiers];
memcpy(identifiers, &m_data->m_bulletStreamDataServerToClient[0], numIdentifiers * sizeof(b3UserDataGlobalIdentifier));
int *identifiers = new int[numIdentifiers];
memcpy(identifiers, &m_data->m_bulletStreamDataServerToClient[0], numIdentifiers * sizeof(int));
for (int i=0; i<numIdentifiers; i++) {
m_data->m_tmpInfoRequestCommand.m_type = CMD_REQUEST_USER_DATA;
m_data->m_tmpInfoRequestCommand.m_userDataRequestArgs = identifiers[i];
m_data->m_tmpInfoRequestCommand.m_userDataRequestArgs.m_userDataId = identifiers[i];
bool hasStatus = m_data->m_commandProcessor->processCommand(m_data->m_tmpInfoRequestCommand, m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
@ -1184,18 +1167,15 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
}
case CMD_REMOVE_USER_DATA_COMPLETED:
{
const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_removeUserDataResponseArgs;
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr) {
UserDataCache *userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex];
if (userDataCachePtr)
{
SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId];
if (userDataPtr) {
(userDataCachePtr)->m_keyToUserDataIdMap.remove((userDataPtr)->m_key.c_str());
(userDataCachePtr)->m_userDataMap.remove(userDataGlobalId.m_userDataId);
}
const int userDataId = serverCmd.m_removeUserDataResponseArgs.m_userDataId;
SharedMemoryUserData *userData = m_data->m_userDataMap[userDataId];
if (userData) {
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[userData->m_bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr) {
(*bodyJointsPtr)->m_userDataIds.remove(userDataId);
}
m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData));
m_data->m_userDataMap.remove(userDataId);
}
break;
}
@ -1253,6 +1233,12 @@ void PhysicsDirect::removeCachedBody(int bodyUniqueId)
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr)
{
for(int i=0; i<(*bodyJointsPtr)->m_userDataIds.size(); i++) {
const int userDataId = (*bodyJointsPtr)->m_userDataIds[i];
SharedMemoryUserData *userData = m_data->m_userDataMap[userDataId];
m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData));
m_data->m_userDataMap.remove(userDataId);
}
delete (*bodyJointsPtr);
m_data->m_bodyJointMap.remove(bodyUniqueId);
}
@ -1499,72 +1485,49 @@ double PhysicsDirect::getTimeOut() const
return m_data->m_timeOutInSeconds;
}
bool PhysicsDirect::getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const {
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (!bodyJointsPtr || !(*bodyJointsPtr)) {
return false;
}
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex];
if (!userDataCachePtr)
{
return false;
}
SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap[userDataId];
bool PhysicsDirect::getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const {
SharedMemoryUserData *userDataPtr = m_data->m_userDataMap[userDataId];
if (!userDataPtr)
{
return false;
}
valueOut.m_type = userDataPtr->m_type;
valueOut.m_type = (userDataPtr)->m_type;
valueOut.m_length = userDataPtr->m_bytes.size();
valueOut.m_data1 = userDataPtr->m_bytes.size()? &userDataPtr->m_bytes[0] : 0;
return true;
}
int PhysicsDirect::getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const {
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (!bodyJointsPtr || !(*bodyJointsPtr)) {
return -1;
}
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex];
if (!userDataCachePtr) {
return -1;
}
int *userDataId = (userDataCachePtr)->m_keyToUserDataIdMap[key];
int PhysicsDirect::getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const {
int* userDataId = m_data->m_userDataHandleLookup.find(SharedMemoryUserDataHashKey(key, bodyUniqueId, linkIndex, visualShapeIndex));
if (!userDataId) {
return -1;
}
return *userDataId;
}
int PhysicsDirect::getNumUserData(int bodyUniqueId, int linkIndex) const {
int PhysicsDirect::getNumUserData(int bodyUniqueId) const {
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (!bodyJointsPtr || !(*bodyJointsPtr)) {
return 0;
}
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex];
if (!userDataCachePtr) {
return 0;
}
return (userDataCachePtr)->m_userDataMap.size();
return (*bodyJointsPtr)->m_userDataIds.size();
}
void PhysicsDirect::getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const {
void PhysicsDirect::getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const {
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (!bodyJointsPtr || !(*bodyJointsPtr)) {
*keyOut = 0;
*userDataIdOut = -1;
return;
}
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex];
if (!userDataCachePtr || userDataIndex >= (userDataCachePtr)->m_userDataMap.size())
if (!bodyJointsPtr || !(*bodyJointsPtr) || userDataIndex <= 0 || userDataIndex > (*bodyJointsPtr)->m_userDataIds.size())
{
*keyOut = 0;
*userDataIdOut = -1;
return;
}
*userDataIdOut = (userDataCachePtr)->m_userDataMap.getKeyAtIndex(userDataIndex).getUid1();
SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap.getAtIndex(userDataIndex);
*keyOut = (userDataPtr)->m_key.c_str();
int userDataId = (*bodyJointsPtr)->m_userDataIds[userDataIndex];
SharedMemoryUserData *userData = m_data->m_userDataMap[userDataId];
*userDataIdOut = userDataId;
*keyOut = userData->m_key.c_str();
*linkIndexOut = userData->m_linkIndex;
*visualShapeIndexOut = userData->m_visualShapeIndex;
}

8
examples/SharedMemory/PhysicsDirect.h
View File

@ -115,10 +115,10 @@ public:
virtual void setTimeOut(double timeOutInSeconds);
virtual double getTimeOut() const;
virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const;
virtual int getNumUserData(int bodyUniqueId, int linkIndex) const;
virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const;
virtual bool getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const;
virtual int getNumUserData(int bodyUniqueId) const;
virtual void getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const;
virtual void pushProfileTiming(const char* timingName);
virtual void popProfileTiming();

18
examples/SharedMemory/PhysicsLoopBack.cpp
View File

@ -231,20 +231,20 @@ double PhysicsLoopBack::getTimeOut() const
return m_data->m_physicsClient->getTimeOut();
}
bool PhysicsLoopBack::getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const {
return m_data->m_physicsClient->getCachedUserData(bodyUniqueId, linkIndex, userDataId, valueOut);
bool PhysicsLoopBack::getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const {
return m_data->m_physicsClient->getCachedUserData(userDataId, valueOut);
}
int PhysicsLoopBack::getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const {
return m_data->m_physicsClient->getCachedUserDataId(bodyUniqueId, linkIndex, key);
int PhysicsLoopBack::getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const {
return m_data->m_physicsClient->getCachedUserDataId(bodyUniqueId, linkIndex, visualShapeIndex, key);
}
int PhysicsLoopBack::getNumUserData(int bodyUniqueId, int linkIndex) const {
return m_data->m_physicsClient->getNumUserData(bodyUniqueId, linkIndex);
int PhysicsLoopBack::getNumUserData(int bodyUniqueId) const {
return m_data->m_physicsClient->getNumUserData(bodyUniqueId);
}
void PhysicsLoopBack::getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const {
m_data->m_physicsClient->getUserDataInfo(bodyUniqueId, linkIndex, userDataIndex, keyOut, userDataIdOut);
void PhysicsLoopBack::getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const {
m_data->m_physicsClient->getUserDataInfo(bodyUniqueId, userDataIndex, keyOut, userDataIdOut, linkIndexOut, visualShapeIndexOut);
}
void PhysicsLoopBack::pushProfileTiming(const char* timingName)
@ -254,4 +254,4 @@ void PhysicsLoopBack::pushProfileTiming(const char* timingName)
void PhysicsLoopBack::popProfileTiming()
{
m_data->m_physicsClient->popProfileTiming();
}
}

8
examples/SharedMemory/PhysicsLoopBack.h
View File

@ -89,10 +89,10 @@ public:
virtual void setTimeOut(double timeOutInSeconds);
virtual double getTimeOut() const;
virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const;
virtual int getNumUserData(int bodyUniqueId, int linkIndex) const;
virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const;
virtual bool getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const;
virtual int getNumUserData(int bodyUniqueId) const;
virtual void getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const;
virtual void pushProfileTiming(const char* timingName);
virtual void popProfileTiming();

197
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -222,72 +222,6 @@ struct InternalCollisionShapeData
#include "SharedMemoryUserData.h"
/**
* Holds all custom user data entries for a link.
*/
struct InternalLinkUserData {
// Used to look up user data entry handles for string keys.
btHashMap<btHashString, int> m_keyToHandleMap;
b3ResizablePool<b3PoolBodyHandle<SharedMemoryUserData> > m_userData;
// Adds or replaces a user data entry.
// Returns the user data handle.
const int add(const char* key, const char* bytes, int len, int type)
{
// If an entry with the key already exists, just update the value.
int userDataId = getUserDataId(key);
if (userDataId != -1) {
SharedMemoryUserData* userData = m_userData.getHandle(userDataId);
b3Assert(userData);
userData->replaceValue(bytes, len, type);
return userDataId;
}
userDataId = m_userData.allocHandle();
SharedMemoryUserData* userData = m_userData.getHandle(userDataId);
userData->m_key = key;
userData->replaceValue(bytes, len, type);
m_keyToHandleMap.insert(userData->m_key.c_str(), userDataId);
return userDataId;
}
// Returns the user data handle for a specified key or -1 if not found.
const int getUserDataId(const char* key) const
{
const int* userDataIdPtr = m_keyToHandleMap.find(key);
if (userDataIdPtr)
{
return *userDataIdPtr;
}
return -1;
}
// Removes a user data entry given the handle.
// Returns true when the entry was removed, false otherwise.
const bool remove(int userDataId)
{
const SharedMemoryUserData* userData = m_userData.getHandle(userDataId);
if (!userData)
{
return false;
}
m_keyToHandleMap.remove(userData->m_key.c_str());
m_userData.freeHandle(userDataId);
return true;
}
// Returns the user data given the user data id. null otherwise.
const SharedMemoryUserData* getUserData(const int userDataId) const
{
return m_userData.getHandle(userDataId);
}
void getUserDataIds(b3AlignedObjectArray<int> &userDataIds) const
{
m_userData.getUsedHandles(userDataIds);
}
};
struct InternalBodyData
{
btMultiBody* m_multiBody;
@ -303,7 +237,7 @@ struct InternalBodyData
btAlignedObjectArray<btGeneric6DofSpring2Constraint*> m_rigidBodyJoints;
btAlignedObjectArray<std::string> m_rigidBodyJointNames;
btAlignedObjectArray<std::string> m_rigidBodyLinkNames;
btHashMap<btHashInt, InternalLinkUserData*> m_linkUserDataMap;
btAlignedObjectArray<int> m_userDataHandles;
#ifdef B3_ENABLE_TINY_AUDIO
b3HashMap<btHashInt, SDFAudioSource> m_audioSources;
@ -328,10 +262,7 @@ struct InternalBodyData
m_rigidBodyJoints.clear();
m_rigidBodyJointNames.clear();
m_rigidBodyLinkNames.clear();
for(int i=0; i<m_linkUserDataMap.size(); i++) {
delete *m_linkUserDataMap.getAtIndex(i);
}
m_linkUserDataMap.clear();
m_userDataHandles.clear();
}
};
@ -1607,8 +1538,8 @@ struct PhysicsServerCommandProcessorInternalData
b3ResizablePool< InternalBodyHandle > m_bodyHandles;
b3ResizablePool<InternalCollisionShapeHandle> m_userCollisionShapeHandles;
b3ResizablePool<InternalVisualShapeHandle> m_userVisualShapeHandles;
b3ResizablePool<b3PoolBodyHandle<SharedMemoryUserData> > m_userDataHandles;
btHashMap<SharedMemoryUserDataHashKey, int> m_userDataHandleLookup;
b3PluginManager m_pluginManager;
@ -5089,34 +5020,11 @@ bool PhysicsServerCommandProcessor::processSyncUserDataCommand(const struct Shar
bool hasStatus = true;
BT_PROFILE("CMD_SYNC_USER_DATA");
b3UserDataGlobalIdentifier *userDataIdentifiers = (b3UserDataGlobalIdentifier *)bufferServerToClient;
int numIdentifiers = 0;
b3AlignedObjectArray<int> bodyHandles;
m_data->m_bodyHandles.getUsedHandles(bodyHandles);
for (int i=0; i<bodyHandles.size(); i++) {
int bodyHandle = bodyHandles[i];
InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyHandle);
if (!body) {
continue;
}
for (int j=0; j<body->m_linkUserDataMap.size(); j++) {
const int linkIndex = body->m_linkUserDataMap.getKeyAtIndex(j).getUid1();
InternalLinkUserData **userDataPtr = body->m_linkUserDataMap.getAtIndex(j);
if (!userDataPtr) {
continue;
}
b3AlignedObjectArray<int> userDataIds;
(*userDataPtr)->getUserDataIds(userDataIds);
for (int k=0; k<userDataIds.size(); k++) {
b3UserDataGlobalIdentifier &identifier = userDataIdentifiers[numIdentifiers++];
identifier.m_bodyUniqueId = bodyHandle;
identifier.m_linkIndex = linkIndex;
identifier.m_userDataId = userDataIds[k];
}
}
}
b3AlignedObjectArray<int> userDataHandles;
m_data->m_userDataHandles.getUsedHandles(userDataHandles);
memcpy(bufferServerToClient, &userDataHandles[0], sizeof(int) * userDataHandles.size());
serverStatusOut.m_syncUserDataArgs.m_numUserDataIdentifiers = numIdentifiers;
serverStatusOut.m_syncUserDataArgs.m_numUserDataIdentifiers = userDataHandles.size();
serverStatusOut.m_type = CMD_SYNC_USER_DATA_COMPLETED;
return hasStatus;
}
@ -5127,21 +5035,16 @@ bool PhysicsServerCommandProcessor::processRequestUserDataCommand(const struct S
BT_PROFILE("CMD_REQUEST_USER_DATA");
serverStatusOut.m_type = CMD_REQUEST_USER_DATA_FAILED;
InternalBodyData *body = m_data->m_bodyHandles.getHandle(clientCmd.m_userDataRequestArgs.m_bodyUniqueId);
if (!body) {
return hasStatus;
}
const int linkIndex = clientCmd.m_userDataRequestArgs.m_linkIndex;
InternalLinkUserData **userDataPtr = body->m_linkUserDataMap[linkIndex];
if (!userDataPtr) {
return hasStatus;
}
const SharedMemoryUserData *userData = (*userDataPtr)->getUserData(clientCmd.m_userDataRequestArgs.m_userDataId);
SharedMemoryUserData *userData = m_data->m_userDataHandles.getHandle(clientCmd.m_userDataRequestArgs.m_userDataId);
if (!userData) {
return hasStatus;
}
btAssert(bufferSizeInBytes >= userData->m_bytes.size());
serverStatusOut.m_userDataResponseArgs.m_userDataGlobalId = clientCmd.m_userDataRequestArgs;
btAssert(bufferSizeInBytes >= userData->m_bytes.size());
serverStatusOut.m_userDataResponseArgs.m_userDataId = clientCmd.m_userDataRequestArgs.m_userDataId;
serverStatusOut.m_userDataResponseArgs.m_bodyUniqueId = userData->m_bodyUniqueId;
serverStatusOut.m_userDataResponseArgs.m_linkIndex = userData->m_linkIndex;
serverStatusOut.m_userDataResponseArgs.m_visualShapeIndex = userData->m_visualShapeIndex;
serverStatusOut.m_userDataResponseArgs.m_valueType = userData->m_type;
serverStatusOut.m_userDataResponseArgs.m_valueLength = userData->m_bytes.size();
serverStatusOut.m_type = CMD_REQUEST_USER_DATA_COMPLETED;
@ -5164,27 +5067,43 @@ bool PhysicsServerCommandProcessor::processAddUserDataCommand(const struct Share
{
return hasStatus;
}
InternalBodyData *body = m_data->m_bodyHandles.getHandle(clientCmd.m_addUserDataRequestArgs.m_bodyUniqueId);
if (!body) {
return hasStatus;
}
const int linkIndex = clientCmd.m_addUserDataRequestArgs.m_linkIndex;
InternalLinkUserData **userDataPtr = body->m_linkUserDataMap[linkIndex];
if (!userDataPtr) {
InternalLinkUserData *userData = new InternalLinkUserData;
userDataPtr = &userData;
body->m_linkUserDataMap.insert(linkIndex, userData);
SharedMemoryUserDataHashKey userDataIdentifier(
clientCmd.m_addUserDataRequestArgs.m_key,
clientCmd.m_addUserDataRequestArgs.m_bodyUniqueId,
clientCmd.m_addUserDataRequestArgs.m_linkIndex,
clientCmd.m_addUserDataRequestArgs.m_visualShapeIndex);
int* userDataHandlePtr = m_data->m_userDataHandleLookup.find(userDataIdentifier);
int userDataHandle = userDataHandlePtr ? *userDataHandlePtr : m_data->m_userDataHandles.allocHandle();
SharedMemoryUserData *userData = m_data->m_userDataHandles.getHandle(userDataHandle);
if (!userData) {
return hasStatus;
}
const int userDataId = (*userDataPtr)->add(clientCmd.m_addUserDataRequestArgs.m_key,
bufferServerToClient,clientCmd.m_addUserDataRequestArgs.m_valueLength,
if (!userDataHandlePtr) {
userData->m_key = clientCmd.m_addUserDataRequestArgs.m_key;
userData->m_bodyUniqueId = clientCmd.m_addUserDataRequestArgs.m_bodyUniqueId;
userData->m_linkIndex = clientCmd.m_addUserDataRequestArgs.m_linkIndex;
userData->m_visualShapeIndex = clientCmd.m_addUserDataRequestArgs.m_visualShapeIndex;
m_data->m_userDataHandleLookup.insert(userDataIdentifier, userDataHandle);
body->m_userDataHandles.push_back(userDataHandle);
}
userData->replaceValue(bufferServerToClient,
clientCmd.m_addUserDataRequestArgs.m_valueLength,
clientCmd.m_addUserDataRequestArgs.m_valueType);
serverStatusOut.m_type = CMD_ADD_USER_DATA_COMPLETED;
serverStatusOut.m_userDataResponseArgs.m_userDataGlobalId.m_userDataId = userDataId;
serverStatusOut.m_userDataResponseArgs.m_userDataGlobalId.m_bodyUniqueId = clientCmd.m_addUserDataRequestArgs.m_bodyUniqueId;
serverStatusOut.m_userDataResponseArgs.m_userDataGlobalId.m_linkIndex = clientCmd.m_addUserDataRequestArgs.m_linkIndex;
serverStatusOut.m_userDataResponseArgs.m_userDataId = userDataHandle;
serverStatusOut.m_userDataResponseArgs.m_bodyUniqueId = clientCmd.m_addUserDataRequestArgs.m_bodyUniqueId;
serverStatusOut.m_userDataResponseArgs.m_linkIndex = clientCmd.m_addUserDataRequestArgs.m_linkIndex;
serverStatusOut.m_userDataResponseArgs.m_visualShapeIndex = clientCmd.m_addUserDataRequestArgs.m_visualShapeIndex;
serverStatusOut.m_userDataResponseArgs.m_valueLength = clientCmd.m_addUserDataRequestArgs.m_valueLength;
serverStatusOut.m_userDataResponseArgs.m_valueType = clientCmd.m_addUserDataRequestArgs.m_valueType;
strcpy(serverStatusOut.m_userDataResponseArgs.m_key, clientCmd.m_addUserDataRequestArgs.m_key);
@ -5199,19 +5118,20 @@ bool PhysicsServerCommandProcessor::processRemoveUserDataCommand(const struct Sh
BT_PROFILE("CMD_REMOVE_USER_DATA");
serverStatusOut.m_type = CMD_REMOVE_USER_DATA_FAILED;
InternalBodyData *body = m_data->m_bodyHandles.getHandle(clientCmd.m_removeUserDataRequestArgs.m_bodyUniqueId);
SharedMemoryUserData *userData = m_data->m_userDataHandles.getHandle(clientCmd.m_removeUserDataRequestArgs.m_userDataId);
if (!userData) {
return hasStatus;
}
InternalBodyData *body = m_data->m_bodyHandles.getHandle(userData->m_bodyUniqueId);
if (!body) {
return hasStatus;
}
const int linkIndex = clientCmd.m_removeUserDataRequestArgs.m_linkIndex;
InternalLinkUserData **userDataPtr = body->m_linkUserDataMap[linkIndex];
if (!userDataPtr) {
return hasStatus;
}
const bool removed = (*userDataPtr)->remove(clientCmd.m_removeUserDataRequestArgs.m_userDataId);
if (!removed) {
return hasStatus;
}
body->m_userDataHandles.remove(clientCmd.m_removeUserDataRequestArgs.m_userDataId);
m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData));
m_data->m_userDataHandles.freeHandle(clientCmd.m_removeUserDataRequestArgs.m_userDataId);
serverStatusOut.m_removeUserDataResponseArgs = clientCmd.m_removeUserDataRequestArgs;
serverStatusOut.m_type = CMD_REMOVE_USER_DATA_COMPLETED;
return hasStatus;
@ -8325,6 +8245,12 @@ bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct Shared
bodyHandle->m_rigidBody=0;
serverCmd.m_type = CMD_REMOVE_BODY_COMPLETED;
}
for (int i=0; i < bodyHandle->m_userDataHandles.size(); i++) {
int userDataHandle = bodyHandle->m_userDataHandles[i];
SharedMemoryUserData *userData = m_data->m_userDataHandles.getHandle(userDataHandle);
m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData));
m_data->m_userDataHandles.freeHandle(userDataHandle);
}
m_data->m_bodyHandles.freeHandle(bodyUniqueId);
}
@ -10561,6 +10487,9 @@ void PhysicsServerCommandProcessor::resetSimulation()
m_data->m_userCollisionShapeHandles.exitHandles();
m_data->m_userCollisionShapeHandles.initHandles();
m_data->m_userDataHandles.exitHandles();
m_data->m_userDataHandles.initHandles();
m_data->m_userDataHandleLookup.clear();
}

18
examples/SharedMemory/SharedMemoryCommands.h
View File

@ -993,13 +993,20 @@ struct b3StateSerializationArguments
struct SyncUserDataArgs
{
// User data identifiers stored in m_bulletStreamDataServerToClientRefactor
// as as array of b3UserDataGlobalIdentifier objects
// as as array of integers.
int m_numUserDataIdentifiers;
};
struct UserDataRequestArgs {
int m_userDataId;
};
struct UserDataResponseArgs
{
b3UserDataGlobalIdentifier m_userDataGlobalId;
int m_userDataId;
int m_bodyUniqueId;
int m_linkIndex;
int m_visualShapeIndex;
int m_valueType;
int m_valueLength;
char m_key[MAX_USER_DATA_KEY_LENGTH];
@ -1010,6 +1017,7 @@ struct AddUserDataRequestArgs
{
int m_bodyUniqueId;
int m_linkIndex;
int m_visualShapeIndex;
int m_valueType;
int m_valueLength;
char m_key[MAX_USER_DATA_KEY_LENGTH];
@ -1073,9 +1081,9 @@ struct SharedMemoryCommand
struct b3CustomCommand m_customCommandArgs;
struct b3StateSerializationArguments m_loadStateArguments;
struct RequestCollisionShapeDataArgs m_requestCollisionShapeDataArguments;
struct b3UserDataGlobalIdentifier m_userDataRequestArgs;
struct UserDataRequestArgs m_userDataRequestArgs;
struct AddUserDataRequestArgs m_addUserDataRequestArgs;
struct b3UserDataGlobalIdentifier m_removeUserDataRequestArgs;
struct UserDataRequestArgs m_removeUserDataRequestArgs;
};
};
@ -1150,7 +1158,7 @@ struct SharedMemoryStatus
struct SendCollisionShapeDataArgs m_sendCollisionShapeArgs;
struct SyncUserDataArgs m_syncUserDataArgs;
struct UserDataResponseArgs m_userDataResponseArgs;
struct b3UserDataGlobalIdentifier m_removeUserDataResponseArgs;
struct UserDataRequestArgs m_removeUserDataResponseArgs;
};
};

10
examples/SharedMemory/SharedMemoryPublic.h
View File

@ -7,7 +7,8 @@
//Please don't replace an existing magic number:
//instead, only ADD a new one at the top, comment-out previous one
#define SHARED_MEMORY_MAGIC_NUMBER 201806150
#define SHARED_MEMORY_MAGIC_NUMBER 201807040
//#define SHARED_MEMORY_MAGIC_NUMBER 201806150
//#define SHARED_MEMORY_MAGIC_NUMBER 201806020
//#define SHARED_MEMORY_MAGIC_NUMBER 201801170
//#define SHARED_MEMORY_MAGIC_NUMBER 201801080
@ -287,13 +288,6 @@ struct b3UserDataValue
char* m_data1;
};
struct b3UserDataGlobalIdentifier
{
int m_bodyUniqueId;
int m_linkIndex;
int m_userDataId;
};
struct b3UserConstraint
{
int m_parentBodyIndex;

52
examples/SharedMemory/SharedMemoryUserData.h
View File

@ -3,6 +3,7 @@
#include <string>
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btHashMap.h"
#include "SharedMemoryPublic.h"
struct SharedMemoryUserData
@ -10,20 +11,22 @@ struct SharedMemoryUserData
std::string m_key;
int m_type;
int m_bodyUniqueId;
int m_linkIndex;
int m_visualShapeIndex;
btAlignedObjectArray<char> m_bytes;
SharedMemoryUserData()
:m_type(-1)
:m_type(-1), m_bodyUniqueId(-1), m_linkIndex(-1), m_visualShapeIndex(-1)
{
}
// Takes ownership of the passed key and value arguments.
SharedMemoryUserData(const char* key)
:m_key(key)
SharedMemoryUserData(const char* key, int bodyUniqueId, int linkIndex, int visualShapeIndex)
:m_key(key), m_type(-1), m_bodyUniqueId(bodyUniqueId), m_linkIndex(linkIndex), m_visualShapeIndex(visualShapeIndex)
{
}
// Takes ownership of the data pointed to by newValue.
void replaceValue(const char* bytes, int len, int type)
{
m_type = type;
@ -45,4 +48,43 @@ struct SharedMemoryUserData
}
};
struct SharedMemoryUserDataHashKey {
unsigned int m_hash;
btHashString m_key;
btHashInt m_bodyUniqueId;
btHashInt m_linkIndex;
btHashInt m_visualShapeIndex;
SIMD_FORCE_INLINE unsigned int getHash()const {
return m_hash;
}
SharedMemoryUserDataHashKey() : m_hash(0) {}
SharedMemoryUserDataHashKey(const struct SharedMemoryUserData *userData)
: m_key(userData->m_key.c_str()),
m_bodyUniqueId(userData->m_bodyUniqueId),
m_linkIndex(userData->m_linkIndex),
m_visualShapeIndex(userData->m_visualShapeIndex) {
calculateHash();
}
SharedMemoryUserDataHashKey(const char *key, int bodyUniqueId, int linkIndex, int visualShapeIndex)
: m_key(key), m_bodyUniqueId(bodyUniqueId), m_linkIndex(linkIndex), m_visualShapeIndex(visualShapeIndex) {
calculateHash();
}
void calculateHash() {
m_hash = m_key.getHash() ^ m_bodyUniqueId.getHash() ^ m_linkIndex.getHash() ^ m_visualShapeIndex.getHash();
}
bool equals(const SharedMemoryUserDataHashKey& other) const {
return m_bodyUniqueId.equals(other.m_bodyUniqueId) &&
m_linkIndex.equals(other.m_linkIndex) &&
m_visualShapeIndex.equals(other.m_visualShapeIndex) &&
m_key.equals(other.m_key);
}
};
#endif //SHARED_MEMORY_USER_DATA_H

66
examples/pybullet/examples/userData.py
View File

@ -21,16 +21,16 @@ plane_id = client.loadURDF(PLANE_PATH)
print ("Plane ID: %s" % plane_id)
print ("Adding user data to plane")
MyKey1 = client.addUserData(plane_id, 0, "MyKey1", "MyValue1")
MyKey2 = client.addUserData(plane_id, 0, "MyKey2", "MyValue2")
MyKey3 = client.addUserData(plane_id, 0, "MyKey3", "MyValue3")
MyKey4 = client.addUserData(plane_id, 0, "MyKey4", "MyValue4")
MyKey1 = client.addUserData(plane_id, "MyKey1", "MyValue1")
MyKey2 = client.addUserData(plane_id, "MyKey2", "MyValue2")
MyKey3 = client.addUserData(plane_id, "MyKey3", "MyValue3")
MyKey4 = client.addUserData(plane_id, "MyKey4", "MyValue4")
print ("Retrieving cached user data")
print (client.getUserData(plane_id, 0, MyKey1))
print (client.getUserData(plane_id, 0, MyKey2))
print (client.getUserData(plane_id, 0, MyKey3))
print (client.getUserData(plane_id, 0, MyKey4))
print (client.getUserData(MyKey1))
print (client.getUserData(MyKey2))
print (client.getUserData(MyKey3))
print (client.getUserData(MyKey4))
print ("Disconnecting")
del client
@ -39,18 +39,18 @@ print ("Reconnecting")
client = bullet_client.BulletClient(connection_mode=CONNECTION_METHOD)
print ("Retrieving synced user data")
print (client.getUserData(plane_id, 0, MyKey1))
print (client.getUserData(plane_id, 0, MyKey2))
print (client.getUserData(plane_id, 0, MyKey3))
print (client.getUserData(plane_id, 0, MyKey4))
print (client.getUserData(MyKey1))
print (client.getUserData(MyKey2))
print (client.getUserData(MyKey3))
print (client.getUserData(MyKey4))
print ("Number of user data entries: %s" % client.getNumUserData(plane_id, 0))
print ("Number of user data entries: %s" % client.getNumUserData(plane_id))
print ("Overriding user data")
client.addUserData(plane_id, 0, "MyKey1", "MyNewValue")
client.addUserData(plane_id, "MyKey1", "MyNewValue")
print ("Cached overridden data")
print (client.getUserData(plane_id, 0, MyKey1))
print (client.getUserData(MyKey1))
print ("Disconnecting")
@ -61,50 +61,50 @@ client = bullet_client.BulletClient(connection_mode=CONNECTION_METHOD)
print ("Synced overridden data")
print (client.getUserData(plane_id, 0, MyKey1))
print (client.getUserData(MyKey1))
print ("Getting user data ID")
print ("Retrieved ID: %s, ID retrieved from addUserData: %s" % (client.getUserDataId(plane_id, 0, "MyKey2"), MyKey2))
print ("Retrieved ID: %s, ID retrieved from addUserData: %s" % (client.getUserDataId(plane_id, "MyKey2"), MyKey2))
print ("Removing user data")
client.removeUserData(plane_id, 0, MyKey2)
client.removeUserData(MyKey2)
print ("Retrieving cached removed data")
print (client.getUserData(plane_id, 0, MyKey2))
print (client.getUserData(MyKey2))
print ("Syncing")
client.syncUserData()
print ("Retrieving removed removed data")
print (client.getUserData(plane_id, 0, MyKey2))
print (client.getUserData(MyKey2))
print ("Iterating over all user data entries and printing results")
for i in range(client.getNumUserData(plane_id, 0)):
userDataId, key = client.getUserDataInfo(plane_id, 0, i)
print ("Info: (%s, %s)" % (userDataId, key))
print ("Value: %s" % client.getUserData(plane_id, 0, userDataId))
for i in range(client.getNumUserData(plane_id)):
userDataId, key, bodyId, linkIndex, visualShapeIndex = client.getUserDataInfo(plane_id, i)
print ("Info: (%s, %s, %s, %s, %s)" % (userDataId, key, bodyId, linkIndex, visualShapeIndex))
print ("Value: %s" % client.getUserData(userDataId))
print ("Removing body")
client.removeBody(plane_id)
print ("Retrieving user data")
print (client.getUserData(plane_id, 0, MyKey1))
print (client.getUserData(plane_id, 0, MyKey3))
print (client.getUserData(plane_id, 0, MyKey4))
print (client.getUserData(MyKey1))
print (client.getUserData(MyKey3))
print (client.getUserData(MyKey4))
print ("Syncing")
client.syncUserData()
print ("Retrieving user data")
print (client.getUserData(plane_id, 0, MyKey1))
print (client.getUserData(plane_id, 0, MyKey3))
print (client.getUserData(plane_id, 0, MyKey4))
print (client.getUserData(MyKey1))
print (client.getUserData(MyKey3))
print (client.getUserData(MyKey4))
plane_id2 = client.loadURDF(PLANE_PATH)
print ("Plane1: %s, plane2: %s" % (plane_id, plane_id2))
print ("Retrieving user data")
print (client.getUserData(plane_id, 0, MyKey1))
print (client.getUserData(plane_id, 0, MyKey3))
print (client.getUserData(plane_id, 0, MyKey4))
print (client.getUserData(MyKey1))
print (client.getUserData(MyKey3))
print (client.getUserData(MyKey4))

73
examples/pybullet/pybullet.c
View File

@ -713,18 +713,19 @@ static PyObject* pybullet_addUserData(PyObject* self, PyObject* args, PyObject*
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
int bodyUniqueId = -1;
int linkIndex = -2;
int linkIndex = -1;
int visualShapeIndex = -1;
const char* key = "";
const char* value = ""; // TODO: Change this to a PyObject and detect the type dynamically.
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "key", "value", "physicsClientId", NULL};
static char* kwlist[] = {"bodyUniqueId", "key", "value", "linkIndex", "visualShapeIndex", "physicsClientId", NULL};
b3SharedMemoryCommandHandle command;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
int userDataId;
int valueLen=-1;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiss|i", kwlist, &bodyUniqueId, &linkIndex, &key, &value, &physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iss|iii", kwlist, &bodyUniqueId, &key, &value, &linkIndex, &visualShapeIndex, &physicsClientId))
{
return NULL;
}
@ -736,7 +737,7 @@ static PyObject* pybullet_addUserData(PyObject* self, PyObject* args, PyObject*
}
valueLen = strlen(value)+1;
command = b3InitAddUserDataCommand(sm, bodyUniqueId, linkIndex, key, USER_DATA_VALUE_TYPE_STRING, valueLen, value);
command = b3InitAddUserDataCommand(sm, bodyUniqueId, linkIndex, visualShapeIndex, key, USER_DATA_VALUE_TYPE_STRING, valueLen, value);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
@ -754,16 +755,14 @@ static PyObject* pybullet_removeUserData(PyObject* self, PyObject* args, PyObjec
{
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
int bodyUniqueId = -1;
int linkIndex = -1;
int userDataId = -1;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "userDataId", "physicsClientId", NULL};
static char* kwlist[] = {"userDataId", "physicsClientId", NULL};
b3SharedMemoryCommandHandle command;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|i", kwlist, &bodyUniqueId, &linkIndex, &userDataId, &physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &userDataId, &physicsClientId))
{
return NULL;
}
@ -774,7 +773,7 @@ static PyObject* pybullet_removeUserData(PyObject* self, PyObject* args, PyObjec
return NULL;
}
command = b3InitRemoveUserDataCommand(sm, bodyUniqueId, linkIndex, userDataId);
command = b3InitRemoveUserDataCommand(sm, userDataId);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
@ -794,15 +793,16 @@ static PyObject* pybullet_getUserDataId(PyObject* self, PyObject* args, PyObject
int physicsClientId = 0;
int bodyUniqueId = -1;
int linkIndex = -1;
int visualShapeIndex = -1;
const char* key = "";
int userDataId;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "key", "physicsClientId", NULL};
static char* kwlist[] = {"bodyUniqueId", "key", "linkIndex", "visualShapeIndex", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iis|i", kwlist, &bodyUniqueId, &linkIndex, &key, &physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|iii", kwlist, &bodyUniqueId, &key, &linkIndex, &visualShapeIndex, &physicsClientId))
{
return NULL;
}
@ -813,7 +813,7 @@ static PyObject* pybullet_getUserDataId(PyObject* self, PyObject* args, PyObject
return NULL;
}
userDataId = b3GetUserDataId(sm, bodyUniqueId, linkIndex, key);
userDataId = b3GetUserDataId(sm, bodyUniqueId, linkIndex, visualShapeIndex, key);
return PyInt_FromLong(userDataId);
}
@ -821,16 +821,14 @@ static PyObject* pybullet_getUserData(PyObject* self, PyObject* args, PyObject*
{
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
int bodyUniqueId = -1;
int linkIndex = -1;
int userDataId = -1;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "userDataId", "physicsClientId", NULL};
static char* kwlist[] = {"userDataId", "physicsClientId", NULL};
struct b3UserDataValue value;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|i", kwlist, &bodyUniqueId, &linkIndex, &userDataId, &physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &userDataId, &physicsClientId))
{
return NULL;
}
@ -842,7 +840,7 @@ static PyObject* pybullet_getUserData(PyObject* self, PyObject* args, PyObject*
}
if (!b3GetUserData(sm, bodyUniqueId, linkIndex, userDataId, &value)) {
if (!b3GetUserData(sm, userDataId, &value)) {
Py_INCREF(Py_None);
return Py_None;
@ -861,15 +859,14 @@ static PyObject* pybullet_getNumUserData(PyObject* self, PyObject* args, PyObjec
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
int bodyUniqueId = -1;
int linkIndex = -1;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "physicsClientId", NULL};
static char* kwlist[] = {"bodyUniqueId", "physicsClientId", NULL};
int numUserData;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &bodyUniqueId, &linkIndex, &physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &bodyUniqueId, &physicsClientId))
{
return NULL;
}
@ -880,7 +877,7 @@ static PyObject* pybullet_getNumUserData(PyObject* self, PyObject* args, PyObjec
return NULL;
}
numUserData = b3GetNumUserData(sm, bodyUniqueId, linkIndex);
numUserData = b3GetNumUserData(sm, bodyUniqueId);
return PyInt_FromLong(numUserData);
}
@ -889,16 +886,17 @@ static PyObject* pybullet_getUserDataInfo(PyObject* self, PyObject* args, PyObje
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
int bodyUniqueId = -1;
int linkIndex = -1;
int userDataIndex = -1;
int linkIndex = -1;
int visualShapeIndex = -1;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "userDataIndex", "physicsClientId", NULL};
static char* kwlist[] = {"bodyUniqueId", "userDataIndex", "physicsClientId", NULL};
const char* key = 0;
int userDataId = -1;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|i", kwlist, &bodyUniqueId, &linkIndex, &userDataIndex, &physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &bodyUniqueId, &userDataIndex, &physicsClientId))
{
return NULL;
}
@ -909,16 +907,19 @@ static PyObject* pybullet_getUserDataInfo(PyObject* self, PyObject* args, PyObje
return NULL;
}
b3GetUserDataInfo(sm, bodyUniqueId, linkIndex, userDataIndex, &key, &userDataId);
b3GetUserDataInfo(sm, bodyUniqueId, userDataIndex, &key, &userDataId, &linkIndex, &visualShapeIndex);
if (key == 0 || userDataId == -1) {
PyErr_SetString(SpamError, "Could not get user data info.");
return NULL;
}
{
PyObject *userDataInfoTuple = PyTuple_New(2);
PyObject *userDataInfoTuple = PyTuple_New(5);
PyTuple_SetItem(userDataInfoTuple, 0, PyInt_FromLong(userDataId));
PyTuple_SetItem(userDataInfoTuple, 1, PyString_FromString(key));
PyTuple_SetItem(userDataInfoTuple, 2, PyInt_FromLong(bodyUniqueId));
PyTuple_SetItem(userDataInfoTuple, 3, PyInt_FromLong(linkIndex));
PyTuple_SetItem(userDataInfoTuple, 4, PyInt_FromLong(visualShapeIndex));
return userDataInfoTuple;
}
}
@ -9164,28 +9165,28 @@ static PyMethodDef SpamMethods[] = {
"Update user data, in case other clients made changes."},
{"addUserData", (PyCFunction)pybullet_addUserData, METH_VARARGS | METH_KEYWORDS,
"addUserData(bodyUniqueId, linkIndex, key, value, physicsClientId=0)\n"
"Adds or updates a user data entry to a link. Returns user data identifier."},
"addUserData(bodyUniqueId, key, value, linkIndex=-1, visualShapeIndex=-1, physicsClientId=0)\n"
"Adds or updates a user data entry. Returns user data identifier."},
{"getUserData", (PyCFunction)pybullet_getUserData, METH_VARARGS | METH_KEYWORDS,
"getUserData(bodyUniqueId, linkIndex, userDataId, physicsClientId=0)\n"
"getUserData(userDataId, physicsClientId=0)\n"
"Returns the user data value."},
{"removeUserData", (PyCFunction)pybullet_removeUserData, METH_VARARGS | METH_KEYWORDS,
"removeUserData(bodyUniqueId, linkIndex, userDataId, physicsClientId=0)\n"
"removeUserData(userDataId, physicsClientId=0)\n"
"Removes a user data entry."},
{"getUserDataId", (PyCFunction)pybullet_getUserDataId, METH_VARARGS | METH_KEYWORDS,
"getUserDataId(bodyUniqueId, linkIndex, key, physicsClientId=0)\n"
"Retrieves the userDataId on a link given the key."},
"getUserDataId(bodyUniqueId, key, linkIndex=-1, visualShapeIndex=-1, physicsClientId=0)\n"
"Retrieves the userDataId given the key and optionally link and visual shape index."},
{"getNumUserData", (PyCFunction)pybullet_getNumUserData, METH_VARARGS | METH_KEYWORDS,
"getNumUserData(bodyUniqueId, linkIndex, physicsClientId=0)\n"
"Retrieves the number of user data entries in a link."},
"getNumUserData(bodyUniqueId physicsClientId=0)\n"
"Retrieves the number of user data entries in a body."},
{"getUserDataInfo", (PyCFunction)pybullet_getUserDataInfo, METH_VARARGS | METH_KEYWORDS,
"getUserDataInfo(bodyUniqueId, linkIndex, userDataIndex, physicsClientId=0)\n"
"Retrieves the key and the identifier of a user data as (id, key)."},
"getUserDataInfo(bodyUniqueId, userDataIndex, physicsClientId=0)\n"
"Retrieves the key and the identifier of a user data as (userDataId, key, bodyUniqueId, linkIndex, visualShapeIndex)."},
{"removeBody", (PyCFunction)pybullet_removeBody, METH_VARARGS | METH_KEYWORDS,
"Remove a body by its body unique id."},

235
examples/pybullet/unittests/userDataTest.py
View File

@ -26,169 +26,170 @@ class TestUserDataMethods(unittest.TestCase):
def testAddUserData(self):
plane_id = self.client.loadURDF(PLANE_PATH)
uid1 = self.client.addUserData(plane_id, 0, "MyKey1", "MyValue1")
uid2 = self.client.addUserData(plane_id, 0, "MyKey2", "MyValue2")
uid3 = self.client.addUserData(plane_id, 0, "MyKey3", "MyValue3")
uid4 = self.client.addUserData(plane_id, 0, "MyKey4", "MyValue4")
uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1")
uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2")
uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3")
uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4")
# Retrieve user data and make sure it's correct.
self.assertEqual(b"MyValue1", self.client.getUserData(plane_id, 0, uid1))
self.assertEqual(b"MyValue2", self.client.getUserData(plane_id, 0, uid2))
self.assertEqual(b"MyValue3", self.client.getUserData(plane_id, 0, uid3))
self.assertEqual(b"MyValue4", self.client.getUserData(plane_id, 0, uid4))
self.assertEqual(b"MyValue1", self.client.getUserData(uid1))
self.assertEqual(b"MyValue2", self.client.getUserData(uid2))
self.assertEqual(b"MyValue3", self.client.getUserData(uid3))
self.assertEqual(b"MyValue4", self.client.getUserData(uid4))
# Disconnect/reconnect and make sure that the user data is synced back.
del self.client
self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY)
self.assertEqual(b"MyValue1", self.client.getUserData(plane_id, 0, uid1))
self.assertEqual(b"MyValue2", self.client.getUserData(plane_id, 0, uid2))
self.assertEqual(b"MyValue3", self.client.getUserData(plane_id, 0, uid3))
self.assertEqual(b"MyValue4", self.client.getUserData(plane_id, 0, uid4))
self.assertEqual(b"MyValue1", self.client.getUserData(uid1))
self.assertEqual(b"MyValue2", self.client.getUserData(uid2))
self.assertEqual(b"MyValue3", self.client.getUserData(uid3))
self.assertEqual(b"MyValue4", self.client.getUserData(uid4))
self.client.resetSimulation()
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid1))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid2))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid3))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid4))
self.assertEqual(None, self.client.getUserData(uid1))
self.assertEqual(None, self.client.getUserData(uid2))
self.assertEqual(None, self.client.getUserData(uid3))
self.assertEqual(None, self.client.getUserData(uid4))
def testGetNumUserData(self):
plane_id = self.client.loadURDF(PLANE_PATH)
uid1 = self.client.addUserData(plane_id, 0, "MyKey1", "MyValue1")
uid2 = self.client.addUserData(plane_id, 0, "MyKey2", "MyValue2")
uid3 = self.client.addUserData(plane_id, 0, "MyKey3", "MyValue3")
uid4 = self.client.addUserData(plane_id, 0, "MyKey4", "MyValue4")
uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1")
uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2")
uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3")
uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4")
self.assertEqual(4, self.client.getNumUserData(plane_id, 0))
self.assertEqual(4, self.client.getNumUserData(plane_id))
del self.client
self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY)
self.assertEqual(4, self.client.getNumUserData(plane_id, 0))
self.assertEqual(4, self.client.getNumUserData(plane_id))
def testReplaceUserData(self):
plane_id = self.client.loadURDF(PLANE_PATH)
uid = self.client.addUserData(plane_id, 0, "MyKey", "MyValue")
uid = self.client.addUserData(plane_id, "MyKey", "MyValue")
self.assertEqual(b"MyValue", self.client.getUserData(plane_id, 0, uid))
self.assertEqual(b"MyValue", self.client.getUserData(uid))
new_uid = self.client.addUserData(plane_id, 0, "MyKey", "MyNewValue")
new_uid = self.client.addUserData(plane_id, "MyKey", "MyNewValue")
self.assertEqual(uid, new_uid)
self.assertEqual(b"MyNewValue", self.client.getUserData(plane_id, 0, uid))
self.assertEqual(b"MyNewValue", self.client.getUserData(uid))
del self.client
self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY)
self.assertEqual(b"MyNewValue", self.client.getUserData(plane_id, 0, uid))
self.assertEqual(b"MyNewValue", self.client.getUserData(uid))
def testGetUserDataId(self):
plane_id = self.client.loadURDF(PLANE_PATH)
uid1 = self.client.addUserData(plane_id, 0, "MyKey1", "MyValue1")
uid2 = self.client.addUserData(plane_id, 0, "MyKey2", "MyValue2")
uid3 = self.client.addUserData(plane_id, 0, "MyKey3", "MyValue3")
uid4 = self.client.addUserData(plane_id, 0, "MyKey4", "MyValue4")
uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1")
uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2")
uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3")
uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4")
self.assertEqual(uid1, self.client.getUserDataId(plane_id, 0, "MyKey1"))
self.assertEqual(uid2, self.client.getUserDataId(plane_id, 0, "MyKey2"))
self.assertEqual(uid3, self.client.getUserDataId(plane_id, 0, "MyKey3"))
self.assertEqual(uid4, self.client.getUserDataId(plane_id, 0, "MyKey4"))
self.assertEqual(uid1, self.client.getUserDataId(plane_id, "MyKey1"))
self.assertEqual(uid2, self.client.getUserDataId(plane_id, "MyKey2"))
self.assertEqual(uid3, self.client.getUserDataId(plane_id, "MyKey3"))
self.assertEqual(uid4, self.client.getUserDataId(plane_id, "MyKey4"))
del self.client
self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY)
self.assertEqual(uid1, self.client.getUserDataId(plane_id, 0, "MyKey1"))
self.assertEqual(uid2, self.client.getUserDataId(plane_id, 0, "MyKey2"))
self.assertEqual(uid3, self.client.getUserDataId(plane_id, 0, "MyKey3"))
self.assertEqual(uid4, self.client.getUserDataId(plane_id, 0, "MyKey4"))
self.assertEqual(uid1, self.client.getUserDataId(plane_id, "MyKey1"))
self.assertEqual(uid2, self.client.getUserDataId(plane_id, "MyKey2"))
self.assertEqual(uid3, self.client.getUserDataId(plane_id, "MyKey3"))
self.assertEqual(uid4, self.client.getUserDataId(plane_id, "MyKey4"))
def testRemoveUserData(self):
plane_id = self.client.loadURDF(PLANE_PATH)
uid1 = self.client.addUserData(plane_id, 0, "MyKey1", "MyValue1")
uid2 = self.client.addUserData(plane_id, 0, "MyKey2", "MyValue2")
uid3 = self.client.addUserData(plane_id, 0, "MyKey3", "MyValue3")
uid4 = self.client.addUserData(plane_id, 0, "MyKey4", "MyValue4")
uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1")
uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2")
uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3")
uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4")
self.client.removeUserData(plane_id, 0, uid2)
self.client.removeUserData(uid2)
self.assertEqual(3, self.client.getNumUserData(plane_id, 0))
self.assertEqual(-1, self.client.getUserDataId(plane_id, 0, "MyKey2"))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid2))
self.assertEqual(b"MyValue1", self.client.getUserData(plane_id, 0, uid1))
self.assertEqual(b"MyValue3", self.client.getUserData(plane_id, 0, uid3))
self.assertEqual(b"MyValue4", self.client.getUserData(plane_id, 0, uid4))
self.assertEqual(3, self.client.getNumUserData(plane_id))
self.assertEqual(-1, self.client.getUserDataId(plane_id, "MyKey2"))
self.assertEqual(None, self.client.getUserData(uid2))
self.assertEqual(b"MyValue1", self.client.getUserData(uid1))
self.assertEqual(b"MyValue3", self.client.getUserData(uid3))
self.assertEqual(b"MyValue4", self.client.getUserData(uid4))
del self.client
self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY)
self.assertEqual(3, self.client.getNumUserData(plane_id, 0))
self.assertEqual(-1, self.client.getUserDataId(plane_id, 0, "MyKey2"))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid2))
self.assertEqual(b"MyValue1", self.client.getUserData(plane_id, 0, uid1))
self.assertEqual(b"MyValue3", self.client.getUserData(plane_id, 0, uid3))
self.assertEqual(b"MyValue4", self.client.getUserData(plane_id, 0, uid4))
self.assertEqual(3, self.client.getNumUserData(plane_id))
self.assertEqual(-1, self.client.getUserDataId(plane_id, "MyKey2"))
self.assertEqual(None, self.client.getUserData(uid2))
self.assertEqual(b"MyValue1", self.client.getUserData(uid1))
self.assertEqual(b"MyValue3", self.client.getUserData(uid3))
self.assertEqual(b"MyValue4", self.client.getUserData(uid4))
def testIterateAllUserData(self):
plane_id = self.client.loadURDF(PLANE_PATH)
uid1 = self.client.addUserData(plane_id, 0, "MyKey1", "MyValue1")
uid2 = self.client.addUserData(plane_id, 0, "MyKey2", "MyValue2")
uid3 = self.client.addUserData(plane_id, 0, "MyKey3", "MyValue3")
uid4 = self.client.addUserData(plane_id, 0, "MyKey4", "MyValue4")
uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1")
uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2")
uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3")
uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4")
entries = set()
for i in range(self.client.getNumUserData(plane_id, 0)):
userDataId, key = self.client.getUserDataInfo(plane_id, 0, i)
value = self.client.getUserData(plane_id, 0, userDataId);
entries.add((userDataId, key, value))
for i in range(self.client.getNumUserData(plane_id)):
userDataId, key, bodyId, linkIndex, visualShapeIndex = self.client.getUserDataInfo(plane_id, i)
value = self.client.getUserData(userDataId);
entries.add((userDataId, key, value, bodyId, linkIndex, visualShapeIndex))
self.assertTrue((uid1, b"MyKey1", b"MyValue1") in entries)
self.assertTrue((uid2, b"MyKey2", b"MyValue2") in entries)
self.assertTrue((uid3, b"MyKey3", b"MyValue3") in entries)
self.assertTrue((uid4, b"MyKey4", b"MyValue4") in entries)
self.assertTrue((uid1, b"MyKey1", b"MyValue1", plane_id, -1, -1) in entries)
self.assertTrue((uid2, b"MyKey2", b"MyValue2", plane_id, -1, -1) in entries)
self.assertTrue((uid3, b"MyKey3", b"MyValue3", plane_id, -1, -1) in entries)
self.assertTrue((uid4, b"MyKey4", b"MyValue4", plane_id, -1, -1) in entries)
self.assertEqual(4, len(entries))
def testRemoveBody(self):
plane_id = self.client.loadURDF(PLANE_PATH)
uid1 = self.client.addUserData(plane_id, 0, "MyKey1", "MyValue1")
uid2 = self.client.addUserData(plane_id, 0, "MyKey2", "MyValue2")
uid3 = self.client.addUserData(plane_id, 0, "MyKey3", "MyValue3")
uid4 = self.client.addUserData(plane_id, 0, "MyKey4", "MyValue4")
uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1")
uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2")
uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3")
uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4")
self.client.removeBody(plane_id)
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid1))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid2))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid3))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid4))
self.assertEqual(None, self.client.getUserData(uid1))
self.assertEqual(None, self.client.getUserData(uid2))
self.assertEqual(None, self.client.getUserData(uid3))
self.assertEqual(None, self.client.getUserData(uid4))
del self.client
self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY)
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid1))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid2))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid3))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid4))
self.assertEqual(None, self.client.getUserData(uid1))
self.assertEqual(None, self.client.getUserData(uid2))
self.assertEqual(None, self.client.getUserData(uid3))
self.assertEqual(None, self.client.getUserData(uid4))
def testMultipleBodies(self):
plane1 = self.client.loadURDF(PLANE_PATH)
plane2 = self.client.loadURDF(PLANE_PATH)
uid1 = self.client.addUserData(plane1, 0, "MyKey1", "This is plane 1 - 1")
uid2 = self.client.addUserData(plane1, 0, "MyKey2", "This is plane 1 - 2")
uid1 = self.client.addUserData(plane1, "MyKey1", "This is plane 1 - 1")
uid2 = self.client.addUserData(plane1, "MyKey2", "This is plane 1 - 2")
uid3 = self.client.addUserData(plane2, 0, "MyKey1", "This is plane 2 - 1")
uid4 = self.client.addUserData(plane2, 0, "MyKey2", "This is plane 2 - 2")
uid5 = self.client.addUserData(plane2, 0, "MyKey3", "This is plane 2 - 3")
uid3 = self.client.addUserData(plane2, "MyKey1", "This is plane 2 - 1")
uid4 = self.client.addUserData(plane2, "MyKey2", "This is plane 2 - 2")
uid5 = self.client.addUserData(plane2, "MyKey3", "This is plane 2 - 3")
self.assertEqual(b"This is plane 1 - 1", self.client.getUserData(plane1, 0, self.client.getUserDataId(plane1, 0, "MyKey1")))
self.assertEqual(b"This is plane 1 - 2", self.client.getUserData(plane1, 0, self.client.getUserDataId(plane1, 0, "MyKey2")))
self.assertEqual(b"This is plane 1 - 1", self.client.getUserData(self.client.getUserDataId(plane1, "MyKey1")))
self.assertEqual(b"This is plane 1 - 2", self.client.getUserData(self.client.getUserDataId(plane1, "MyKey2")))
self.assertEqual(b"This is plane 2 - 1", self.client.getUserData(plane2, 0, self.client.getUserDataId(plane2, 0, "MyKey1")))
self.assertEqual(b"This is plane 2 - 2", self.client.getUserData(plane2, 0, self.client.getUserDataId(plane2, 0, "MyKey2")))
self.assertEqual(b"This is plane 2 - 3", self.client.getUserData(plane2, 0, self.client.getUserDataId(plane2, 0, "MyKey3")))
self.assertEqual(b"This is plane 2 - 1", self.client.getUserData(self.client.getUserDataId(plane2, "MyKey1")))
self.assertEqual(b"This is plane 2 - 2", self.client.getUserData(self.client.getUserDataId(plane2, "MyKey2")))
self.assertEqual(b"This is plane 2 - 3", self.client.getUserData(self.client.getUserDataId(plane2, "MyKey3")))
def testMultipleLinks(self):
@ -198,14 +199,15 @@ class TestUserDataMethods(unittest.TestCase):
self.assertTrue(num_links > 1)
for link_index in range(num_links):
uid1 = self.client.addUserData(body_id, link_index, "MyKey1", "Value1 for link %s" % link_index)
uid2 = self.client.addUserData(body_id, link_index, "MyKey2", "Value2 for link %s" % link_index)
uid1 = self.client.addUserData(body_id, "MyKey1", "Value1 for link %s" % link_index, link_index)
uid2 = self.client.addUserData(body_id, "MyKey2", "Value2 for link %s" % link_index, link_index)
for link_index in range(num_links):
uid1 = self.client.getUserDataId(body_id, link_index, "MyKey1")
uid2 = self.client.getUserDataId(body_id, link_index, "MyKey2")
self.assertEqual(("Value1 for link %s" % link_index).encode(), self.client.getUserData(body_id, link_index, uid1))
self.assertEqual(("Value2 for link %s" % link_index).encode(), self.client.getUserData(body_id, link_index, uid2))
uid1 = self.client.getUserDataId(body_id, "MyKey1", link_index)
uid2 = self.client.getUserDataId(body_id, "MyKey2", link_index)
self.assertEqual(("Value1 for link %s" % link_index).encode(), self.client.getUserData(uid1))
self.assertEqual(("Value2 for link %s" % link_index).encode(), self.client.getUserData(uid2))
def testMultipleClients(self):
client1 = self.client
@ -215,25 +217,48 @@ class TestUserDataMethods(unittest.TestCase):
client2.syncBodyInfo()
# Add user data on client 1, check on client 1
uid = client1.addUserData(plane_id, 0, "MyKey", "MyValue")
self.assertEqual(None, client2.getUserData(plane_id, 0, uid))
uid = client1.addUserData(plane_id, "MyKey", "MyValue")
self.assertEqual(None, client2.getUserData(uid))
client2.syncUserData()
self.assertEqual(b"MyValue", client2.getUserData(plane_id, 0, uid))
self.assertEqual(b"MyValue", client2.getUserData(uid))
# Overwrite the value on client 2, check on client 1
client2.addUserData(plane_id, 0, "MyKey", "MyNewValue")
self.assertEqual(b"MyValue", client1.getUserData(plane_id, 0, uid))
client2.addUserData(plane_id, "MyKey", "MyNewValue")
self.assertEqual(b"MyValue", client1.getUserData(uid))
client1.syncUserData()
self.assertEqual(b"MyNewValue", client1.getUserData(plane_id, 0, uid))
self.assertEqual(b"MyNewValue", client1.getUserData(uid))
# Remove user data on client 1, check on client 2
client1.removeUserData(plane_id, 0, uid)
self.assertEqual(b"MyNewValue", client2.getUserData(plane_id, 0, uid))
client1.removeUserData(uid)
self.assertEqual(b"MyNewValue", client2.getUserData(uid))
client2.syncUserData()
self.assertEqual(None, client2.getUserData(plane_id, 0, uid))
self.assertEqual(None, client2.getUserData(uid))
del client2
def testUserDataOnVisualShapes(self):
body_id = self.client.loadURDF(ROBOT_PATH)
num_links = self.client.getNumJoints(body_id)
visual_shapes = self.client.getVisualShapeData(body_id)
self.assertTrue(num_links > 0)
self.assertTrue(len(visual_shapes) > 0)
user_data_entries = set()
for link_index in range(-1, num_links):
num_shapes = sum([1 for shape in visual_shapes if shape[1] == link_index])
for shape_index in range(num_shapes):
key = "MyKey"
value = "MyValue %s, %s" % (link_index, shape_index)
uid = self.client.addUserData(body_id, key, value, link_index, shape_index)
user_data_entries.add((uid, key, value.encode(), body_id, link_index, shape_index))
self.assertEqual(len(visual_shapes), self.client.getNumUserData(body_id))
for uid, key, value, body_id, link_index, shape_index in user_data_entries:
self.assertEqual(value, self.client.getUserData(uid))
self.assertEqual(uid, self.client.getUserDataId(body_id, key, link_index, shape_index))
if __name__ == "__main__":
unittest.main()

160
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
View File

@ -43,11 +43,11 @@ int gNumSplitImpulseRecoveries = 0;
//#define VERBOSE_RESIDUAL_PRINTF 1
///This is the scalar reference implementation of solving a single constraint row, the innerloop of the Projected Gauss Seidel/Sequential Impulse constraint solver
///Below are optional SSE2 and SSE4/FMA3 versions. We assume most hardware has SSE2. For SSE4/FMA3 we perform a CPU feature check.
static btScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
static btScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
{
btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm;
const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(bodyA.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA.internalGetDeltaAngularVelocity());
const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(bodyB.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB.internalGetDeltaAngularVelocity());
// const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn;
deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
@ -69,18 +69,18 @@ static btScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody
c.m_appliedImpulse = sum;
}
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
bodyA.internalApplyImpulse(c.m_contactNormal1*bodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
bodyB.internalApplyImpulse(c.m_contactNormal2*bodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
return deltaImpulse*(1./c.m_jacDiagABInv);
}
static btScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
static btScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
{
btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm;
const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(bodyA.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA.internalGetDeltaAngularVelocity());
const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(bodyB.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB.internalGetDeltaAngularVelocity());
deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
@ -94,8 +94,8 @@ static btScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverB
{
c.m_appliedImpulse = sum;
}
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
bodyA.internalApplyImpulse(c.m_contactNormal1*bodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
bodyB.internalApplyImpulse(c.m_contactNormal2*bodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
return deltaImpulse*(1./c.m_jacDiagABInv);
}
@ -150,14 +150,14 @@ static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 )
#endif
// Project Gauss Seidel or the equivalent Sequential Impulse
static btScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
static btScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
{
__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
btSimdScalar deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse), _mm_set1_ps(c.m_cfm)));
__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128));
__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128));
deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse);
@ -170,27 +170,27 @@ static btScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& body1, btS
__m128 upperMinApplied = _mm_sub_ps(upperLimit1, cpAppliedImp);
deltaImpulse = _mm_or_ps(_mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied));
c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1));
__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128);
__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128, body2.internalGetInvMass().mVec128);
__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128);
__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128, bodyB.internalGetInvMass().mVec128);
__m128 impulseMagnitude = deltaImpulse;
body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
bodyA.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
bodyA.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
bodyB.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
bodyB.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
return deltaImpulse.m_floats[0]/c.m_jacDiagABInv;
}
// Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3
static btScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
static btScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
{
#if defined (BT_ALLOW_SSE4)
__m128 tmp = _mm_set_ps1(c.m_jacDiagABInv);
__m128 deltaImpulse = _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm);
const __m128 lowerLimit = _mm_set_ps1(c.m_lowerLimit);
const __m128 upperLimit = _mm_set_ps1(c.m_upperLimit);
const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128));
const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128));
const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128));
deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse);
deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse);
tmp = _mm_add_ps(c.m_appliedImpulse, deltaImpulse); // sum
@ -198,27 +198,27 @@ static btScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody& bod
const __m128 maskUpper = _mm_cmpgt_ps(upperLimit, tmp);
deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), _mm_blendv_ps(_mm_sub_ps(upperLimit, c.m_appliedImpulse), deltaImpulse, maskUpper), maskLower);
c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, _mm_blendv_ps(upperLimit, tmp, maskUpper), maskLower);
body1.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128), deltaImpulse, body1.internalGetDeltaLinearVelocity().mVec128);
body1.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, body1.internalGetDeltaAngularVelocity().mVec128);
body2.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128), deltaImpulse, body2.internalGetDeltaLinearVelocity().mVec128);
body2.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, body2.internalGetDeltaAngularVelocity().mVec128);
bodyA.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128), deltaImpulse, bodyA.internalGetDeltaLinearVelocity().mVec128);
bodyA.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, bodyA.internalGetDeltaAngularVelocity().mVec128);
bodyB.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128), deltaImpulse, bodyB.internalGetDeltaLinearVelocity().mVec128);
bodyB.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, bodyB.internalGetDeltaAngularVelocity().mVec128);
btSimdScalar deltaImp = deltaImpulse;
return deltaImp.m_floats[0]*(1./c.m_jacDiagABInv);
#else
return gResolveSingleConstraintRowGeneric_sse2(body1,body2,c);
return gResolveSingleConstraintRowGeneric_sse2(bodyA,bodyB,c);
#endif
}
static btScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
static btScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
{
__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
btSimdScalar deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse), _mm_set1_ps(c.m_cfm)));
__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128));
__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128));
deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse);
@ -228,40 +228,40 @@ static btScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& body1,
__m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp);
deltaImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse));
c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum));
__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128);
__m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128);
__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128);
__m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128);
__m128 impulseMagnitude = deltaImpulse;
body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
bodyA.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
bodyA.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
bodyB.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
bodyB.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
return deltaImpulse.m_floats[0]/c.m_jacDiagABInv;
}
// Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3
static btScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
static btScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
{
#ifdef BT_ALLOW_SSE4
__m128 tmp = _mm_set_ps1(c.m_jacDiagABInv);
__m128 deltaImpulse = _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm);
const __m128 lowerLimit = _mm_set_ps1(c.m_lowerLimit);
const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128));
const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128));
const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128));
deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse);
deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse);
tmp = _mm_add_ps(c.m_appliedImpulse, deltaImpulse);
const __m128 mask = _mm_cmpgt_ps(tmp, lowerLimit);
deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), deltaImpulse, mask);
c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, tmp, mask);
body1.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128), deltaImpulse, body1.internalGetDeltaLinearVelocity().mVec128);
body1.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, body1.internalGetDeltaAngularVelocity().mVec128);
body2.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128), deltaImpulse, body2.internalGetDeltaLinearVelocity().mVec128);
body2.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, body2.internalGetDeltaAngularVelocity().mVec128);
bodyA.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128), deltaImpulse, bodyA.internalGetDeltaLinearVelocity().mVec128);
bodyA.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, bodyA.internalGetDeltaAngularVelocity().mVec128);
bodyB.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128), deltaImpulse, bodyB.internalGetDeltaLinearVelocity().mVec128);
bodyB.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, bodyB.internalGetDeltaAngularVelocity().mVec128);
btSimdScalar deltaImp = deltaImpulse;
return deltaImp.m_floats[0]*(1./c.m_jacDiagABInv);
#else
return gResolveSingleConstraintRowLowerLimit_sse2(body1,body2,c);
return gResolveSingleConstraintRowLowerLimit_sse2(bodyA,bodyB,c);
#endif //BT_ALLOW_SSE4
}
@ -270,32 +270,32 @@ static btScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBody&
btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& c)
{
return m_resolveSingleConstraintRowGeneric(body1, body2, c);
return m_resolveSingleConstraintRowGeneric(bodyA, bodyB, c);
}
// Project Gauss Seidel or the equivalent Sequential Impulse
btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& c)
{
return m_resolveSingleConstraintRowGeneric(body1, body2, c);
return m_resolveSingleConstraintRowGeneric(bodyA, bodyB, c);
}
btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& c)
{
return m_resolveSingleConstraintRowLowerLimit(body1, body2, c);
return m_resolveSingleConstraintRowLowerLimit(bodyA, bodyB, c);
}
btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& c)
{
return m_resolveSingleConstraintRowLowerLimit(body1, body2, c);
return m_resolveSingleConstraintRowLowerLimit(bodyA, bodyB, c);
}
static btScalar gResolveSplitPenetrationImpulse_scalar_reference(
btSolverBody& body1,
btSolverBody& body2,
btSolverBody& bodyA,
btSolverBody& bodyB,
const btSolverConstraint& c)
{
btScalar deltaImpulse = 0.f;
@ -304,8 +304,8 @@ static btScalar gResolveSplitPenetrationImpulse_scalar_reference(
{
gNumSplitImpulseRecoveries++;
deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm;
const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity());
const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity());
const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(bodyA.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(bodyA.internalGetTurnVelocity());
const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(bodyB.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(bodyB.internalGetTurnVelocity());
deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
@ -319,13 +319,13 @@ static btScalar gResolveSplitPenetrationImpulse_scalar_reference(
{
c.m_appliedPushImpulse = sum;
}
body1.internalApplyPushImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
body2.internalApplyPushImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
bodyA.internalApplyPushImpulse(c.m_contactNormal1*bodyA.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
bodyB.internalApplyPushImpulse(c.m_contactNormal2*bodyB.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
}
return deltaImpulse*(1./c.m_jacDiagABInv);
}
static btScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
static btScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& c)
{
#ifdef USE_SIMD
if (!c.m_rhsPenetration)
@ -337,8 +337,8 @@ static btScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody& body1,btSolve
__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse),_mm_set1_ps(c.m_cfm)));
__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128));
__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,bodyA.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,bodyA.internalGetTurnVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,bodyB.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,bodyB.internalGetTurnVelocity().mVec128));
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
@ -348,17 +348,17 @@ static btScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody& body1,btSolve
__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
c.m_appliedPushImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,body1.internalGetInvMass().mVec128);
__m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128,body2.internalGetInvMass().mVec128);
__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,bodyA.internalGetInvMass().mVec128);
__m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128,bodyB.internalGetInvMass().mVec128);
__m128 impulseMagnitude = deltaImpulse;
body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
body2.internalGetPushVelocity().mVec128 = _mm_add_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
bodyA.internalGetPushVelocity().mVec128 = _mm_add_ps(bodyA.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
bodyA.internalGetTurnVelocity().mVec128 = _mm_add_ps(bodyA.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
bodyB.internalGetPushVelocity().mVec128 = _mm_add_ps(bodyB.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
bodyB.internalGetTurnVelocity().mVec128 = _mm_add_ps(bodyB.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
btSimdScalar deltaImp = deltaImpulse;
return deltaImp.m_floats[0] * (1. / c.m_jacDiagABInv);
#else
return gResolveSplitPenetrationImpulse_scalar_reference(body1,body2,c);
return gResolveSplitPenetrationImpulse_scalar_reference(bodyA,bodyB,c);
#endif
}
@ -552,7 +552,7 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
btRigidBody* bodyA = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
solverConstraint.m_solverBodyIdA = solverBodyIdA;
solverConstraint.m_solverBodyIdB = solverBodyIdB;
@ -576,12 +576,12 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
solverConstraint.m_angularComponentA .setZero();
}
if (body1)
if (bodyA)
{
solverConstraint.m_contactNormal2 = -normalAxis;
btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal2);
solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
solverConstraint.m_angularComponentB = body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor();
solverConstraint.m_angularComponentB = bodyA->getInvInertiaTensorWorld()*ftorqueAxis1*bodyA->getAngularFactor();
} else
{
solverConstraint.m_contactNormal2.setZero();
@ -598,10 +598,10 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
denom0 = body0->getInvMass() + normalAxis.dot(vec);
}
if (body1)
if (bodyA)
{
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
denom1 = body1->getInvMass() + normalAxis.dot(vec);
denom1 = bodyA->getInvMass() + normalAxis.dot(vec);
}
btScalar denom = relaxation/(denom0+denom1);
solverConstraint.m_jacDiagABInv = denom;
@ -613,8 +613,8 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
btScalar rel_vel;
btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0))
+ solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0))
+ solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyA?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0))
+ solverConstraint.m_relpos2CrossNormal.dot(bodyA?solverBodyB.m_angularVelocity:btVector3(0,0,0));
rel_vel = vel1Dotn+vel2Dotn;
@ -666,7 +666,7 @@ void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint( btSo
btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
btRigidBody* bodyA = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
solverConstraint.m_solverBodyIdA = solverBodyIdA;
solverConstraint.m_solverBodyIdB = solverBodyIdB;
@ -685,13 +685,13 @@ void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint( btSo
{
btVector3 ftorqueAxis1 = normalAxis1;
solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
solverConstraint.m_angularComponentB = bodyA ? bodyA->getInvInertiaTensorWorld()*ftorqueAxis1*bodyA->getAngularFactor() : btVector3(0,0,0);
}
{
btVector3 iMJaA = body0?body0->getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal:btVector3(0,0,0);
btVector3 iMJaB = body1?body1->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0);
btVector3 iMJaB = bodyA?bodyA->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0);
btScalar sum = 0;
sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
@ -704,8 +704,8 @@ void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint( btSo
btScalar rel_vel;
btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0))
+ solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0))
+ solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyA?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0))
+ solverConstraint.m_relpos2CrossNormal.dot(bodyA?solverBodyB.m_angularVelocity:btVector3(0,0,0));
rel_vel = vel1Dotn+vel2Dotn;

35
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@ -744,8 +744,8 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
const btScalar DAMPING_K2_ANGULAR= m_angularDamping;
btVector3 base_vel = getBaseVel();
btVector3 base_omega = getBaseOmega();
const btVector3 base_vel = getBaseVel();
const btVector3 base_omega = getBaseOmega();
// Temporary matrices/vectors -- use scratch space from caller
// so that we don't have to keep reallocating every frame
@ -781,7 +781,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
// hhat is NOT stored for the base (but ahat is)
btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr;
v_ptr += num_links * 2 + 2;
// v_ptr += num_links * 2 + 2; // Disabled since v_ptr is not used in the rest of the code
//
// Y_i, invD_i
btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
@ -819,13 +819,13 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
}
else
{
btVector3 baseForce = isConstraintPass? m_baseConstraintForce : m_baseForce;
btVector3 baseTorque = isConstraintPass? m_baseConstraintTorque : m_baseTorque;
const btVector3& baseForce = isConstraintPass? m_baseConstraintForce : m_baseForce;
const btVector3& baseTorque = isConstraintPass? m_baseConstraintTorque : m_baseTorque;
//external forces
zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));
//adding damping terms (only)
btScalar linDampMult = 1., angDampMult = 1.;
const btScalar linDampMult = 1., angDampMult = 1.;
zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().safeNorm()),
linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().safeNorm()));
@ -969,14 +969,11 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
- m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
- spatCoriolisAcc[i].dot(hDof)
;
}
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
{
btScalar *D_row = &D[dof * m_links[i].m_dofCount];
btScalar *D_row = &D[dof * m_links[i].m_dofCount];
for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
{
btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
const btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2);
}
}
@ -999,8 +996,8 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
case btMultibodyLink::eSpherical:
case btMultibodyLink::ePlanar:
{
btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse();
const btMatrix3x3 D3x3(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
const btMatrix3x3 invD3x3(D3x3.inverse());
//unroll the loop?
for(int row = 0; row < 3; ++row)
@ -1026,7 +1023,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
{
btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
const btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
//
spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof];
}
@ -1037,7 +1034,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
//determine (h*D^{-1}) * h^{T}
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
{
btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
//
dyadTemp -= symmetricSpatialOuterProduct(hDof, spatForceVecTemps[dof]);
}
@ -1058,7 +1055,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
{
btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
//
spatForceVecTemps[0] += hDof * invD_times_Y[dof];
}
@ -1109,7 +1106,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
{
btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
//
Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof);
}
@ -1169,12 +1166,12 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
}
// transform base accelerations back to the world frame.
btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
const btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
output[0] = omegadot_out[0];
output[1] = omegadot_out[1];
output[2] = omegadot_out[2];
btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear()));
const btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear()));
output[3] = vdot_out[0];
output[4] = vdot_out[1];
output[5] = vdot_out[2];