summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorErwin Coumans <erwincoumans@google.com>2019-05-07 19:43:33 -0700
committerErwin Coumans <erwincoumans@google.com>2019-05-07 19:43:33 -0700
commit36ed4419bff1ddc05b0b22d9887b807a2afa2d8d (patch)
treed9304dc5c5418ad4eb4c3e7c6d755daf293cac24
parentde8013ff75945a9d88e0892b15a1a3d52dc0fc7f (diff)
parentdefa172d39c3ecdd9a0bebf1771067a15fc44468 (diff)
downloadbullet3-36ed4419bff1ddc05b0b22d9887b807a2afa2d8d.tar.gz
Merge branch 'master' of https://github.com/erwincoumans/bullet3
-rw-r--r--examples/ExampleBrowser/CMakeLists.txt5
-rw-r--r--examples/RoboticsLearning/GripperGraspExample.cpp8
-rw-r--r--examples/SharedMemory/PhysicsClientC_API.cpp4
-rw-r--r--examples/SharedMemory/PhysicsClientC_API.h2
-rw-r--r--examples/SharedMemory/PhysicsDirect.cpp2
-rw-r--r--examples/SharedMemory/PhysicsServerCommandProcessor.cpp2
-rw-r--r--examples/SharedMemory/SharedMemoryPublic.h2
-rw-r--r--examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp10
-rw-r--r--examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h39
-rw-r--r--examples/SharedMemory/grpc/ConvertGRPCBullet.cpp29
-rw-r--r--examples/SharedMemory/plugins/grpcPlugin/grpcPlugin.cpp2
-rw-r--r--examples/pybullet/pybullet.c33
-rw-r--r--src/Bullet3Common/b3Vector3.h2
-rw-r--r--src/BulletDynamics/Featherstone/btMultiBody.cpp1
-rw-r--r--src/LinearMath/btVector3.h2
15 files changed, 125 insertions, 18 deletions
diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt
index 89531eb77..d17ed4210 100644
--- a/examples/ExampleBrowser/CMakeLists.txt
+++ b/examples/ExampleBrowser/CMakeLists.txt
@@ -94,6 +94,9 @@ LINK_LIBRARIES(
BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK
)
+
+add_definitions(-DINCLUDE_CLOTH_DEMOS)
+
IF (WIN32)
INCLUDE_DIRECTORIES(
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/glad
@@ -346,6 +349,8 @@ SET(BulletExampleBrowser_SRCS
../MultiBody/InvertedPendulumPDControl.cpp
../MultiBody/InvertedPendulumPDControl.h
../MultiBody/MultiBodyConstraintFeedback.cpp
+ ../SoftDemo/SoftDemo.cpp
+ ../SoftDemo/SoftDemo.h
../MultiBody/MultiDofDemo.cpp
../MultiBody/MultiDofDemo.h
../RigidBody/RigidBodySoftContact.cpp
diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp
index a7c80c1f4..8f5ec1a8e 100644
--- a/examples/RoboticsLearning/GripperGraspExample.cpp
+++ b/examples/RoboticsLearning/GripperGraspExample.cpp
@@ -326,7 +326,10 @@ public:
m_robotSim.loadURDF("plane.urdf", args);
}
m_robotSim.setGravity(btVector3(0, 0, -10));
- m_robotSim.loadSoftBody("bunny.obj", 0.1, 0.1, 0.02);
+ b3RobotSimulatorLoadSoftBodyArgs args(0.1, 1, 0.02);
+ args.m_startPosition.setValue(0, 0, 5);
+ args.m_startOrientation.setValue(1, 0, 0, 1);
+ m_robotSim.loadSoftBody("bunny.obj", args);
b3JointInfo revoluteJoint1;
revoluteJoint1.m_parentFrame[0] = -0.055;
@@ -402,7 +405,8 @@ public:
m_robotSim.loadURDF("plane.urdf", args);
}
m_robotSim.setGravity(btVector3(0, 0, -10));
- m_robotSim.loadSoftBody("bunny.obj", 0.3, 10.0, 0.1);
+ b3RobotSimulatorLoadSoftBodyArgs args(0.3, 10, 0.1);
+ m_robotSim.loadSoftBody("bunny.obj", args);
}
}
virtual void exitPhysics()
diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp
index c2cf772ae..c316cbeb2 100644
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/examples/SharedMemory/PhysicsClientC_API.cpp
@@ -315,7 +315,7 @@ B3_SHARED_API int b3LoadSoftBodySetCollisionMargin(b3SharedMemoryCommandHandle c
}
-B3_SHARED_API int b3LoadSoftbodySetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX, double startPosY, double startPosZ)
+B3_SHARED_API int b3LoadSoftBodySetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX, double startPosY, double startPosZ)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
@@ -326,7 +326,7 @@ B3_SHARED_API int b3LoadSoftbodySetStartPosition(b3SharedMemoryCommandHandle com
return 0;
}
-B3_SHARED_API int b3LoadSoftbodySetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW)
+B3_SHARED_API int b3LoadSoftBodySetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h
index 290c8b36b..9ba2dc37c 100644
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/examples/SharedMemory/PhysicsClientC_API.h
@@ -614,6 +614,8 @@ extern "C"
B3_SHARED_API int b3LoadSoftBodySetScale(b3SharedMemoryCommandHandle commandHandle, double scale);
B3_SHARED_API int b3LoadSoftBodySetMass(b3SharedMemoryCommandHandle commandHandle, double mass);
B3_SHARED_API int b3LoadSoftBodySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin);
+ B3_SHARED_API int b3LoadSoftBodySetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX, double startPosY, double startPosZ);
+ B3_SHARED_API int b3LoadSoftBodySetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW);
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API void b3VREventsSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter);
diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp
index 2c3b9a43f..5bc538151 100644
--- a/examples/SharedMemory/PhysicsDirect.cpp
+++ b/examples/SharedMemory/PhysicsDirect.cpp
@@ -1546,7 +1546,7 @@ int PhysicsDirect::getNumUserData(int bodyUniqueId) 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) || userDataIndex <= 0 || userDataIndex > (*bodyJointsPtr)->m_userDataIds.size())
+ if (!bodyJointsPtr || !(*bodyJointsPtr) || userDataIndex < 0 || userDataIndex > (*bodyJointsPtr)->m_userDataIds.size())
{
*keyOut = 0;
*userDataIdOut = -1;
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index 8c87ecd7b..9164c5bd4 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -7376,6 +7376,8 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
psb->generateBendingConstraints(2, pm);
psb->m_cfg.piterations = 20;
psb->m_cfg.kDF = 0.5;
+ //turn on softbody vs softbody collision
+ psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS;
psb->randomizeConstraints();
psb->rotate(initialOrn);
psb->translate(initialPos);
diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h
index b2c2f6c68..6c62c788e 100644
--- a/examples/SharedMemory/SharedMemoryPublic.h
+++ b/examples/SharedMemory/SharedMemoryPublic.h
@@ -952,7 +952,7 @@ struct b3ForwardDynamicsAnalyticsIslandData
double m_remainingLeastSquaresResidual;
};
-#define MAX_ISLANDS_ANALYTICS 1024
+#define MAX_ISLANDS_ANALYTICS 64
struct b3ForwardDynamicsAnalyticsArgs
{
diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
index 9efed7f7e..27bc23163 100644
--- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
+++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
@@ -1134,7 +1134,7 @@ void b3RobotSimulatorClientAPI_NoDirect::submitProfileTiming(const std::string&
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
}
-void b3RobotSimulatorClientAPI_NoDirect::loadSoftBody(const std::string& fileName, double scale, double mass, double collisionMargin)
+void b3RobotSimulatorClientAPI_NoDirect::loadSoftBody(const std::string& fileName, const struct b3RobotSimulatorLoadSoftBodyArgs& args)
{
if (!isConnected())
{
@@ -1143,9 +1143,11 @@ void b3RobotSimulatorClientAPI_NoDirect::loadSoftBody(const std::string& fileNam
}
b3SharedMemoryCommandHandle command = b3LoadSoftBodyCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
- b3LoadSoftBodySetScale(command, scale);
- b3LoadSoftBodySetMass(command, mass);
- b3LoadSoftBodySetCollisionMargin(command, collisionMargin);
+ b3LoadSoftBodySetStartPosition(command, args.m_startPosition[0], args.m_startPosition[1], args.m_startPosition[2]);
+ b3LoadSoftBodySetStartOrientation(command, args.m_startOrientation[0], args.m_startOrientation[1], args.m_startOrientation[2], args.m_startOrientation[3]);
+ b3LoadSoftBodySetScale(command, args.m_scale);
+ b3LoadSoftBodySetMass(command, args.m_mass);
+ b3LoadSoftBodySetCollisionMargin(command, args.m_collisionMargin);
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
}
diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
index 297c7e203..d8fd179c0 100644
--- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
+++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
@@ -52,6 +52,43 @@ struct b3RobotSimulatorLoadSdfFileArgs
}
};
+struct b3RobotSimulatorLoadSoftBodyArgs
+{
+ btVector3 m_startPosition;
+ btQuaternion m_startOrientation;
+ double m_scale;
+ double m_mass;
+ double m_collisionMargin;
+
+ b3RobotSimulatorLoadSoftBodyArgs(const btVector3 &startPos, const btQuaternion &startOrn, const double &scale, const double &mass, const double &collisionMargin)
+ : m_startPosition(startPos),
+ m_startOrientation(startOrn),
+ m_scale(scale),
+ m_mass(mass),
+ m_collisionMargin(collisionMargin)
+ {
+ }
+
+ b3RobotSimulatorLoadSoftBodyArgs(const btVector3 &startPos, const btQuaternion &startOrn)
+ {
+ b3RobotSimulatorLoadSoftBodyArgs(startPos, startOrn, 1.0, 1.0, 0.02);
+ }
+
+ b3RobotSimulatorLoadSoftBodyArgs()
+ {
+ b3RobotSimulatorLoadSoftBodyArgs(btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1));
+ }
+
+ b3RobotSimulatorLoadSoftBodyArgs(double scale, double mass, double collisionMargin)
+ : m_startPosition(btVector3(0, 0, 0)),
+ m_startOrientation(btQuaternion(0, 0, 0, 1)),
+ m_scale(scale),
+ m_mass(mass),
+ m_collisionMargin(collisionMargin)
+ {
+ }
+};
+
struct b3RobotSimulatorLoadFileResults
{
btAlignedObjectArray<int> m_uniqueObjectIds;
@@ -647,7 +684,7 @@ public:
int getConstraintUniqueId(int serialIndex);
- void loadSoftBody(const std::string &fileName, double scale, double mass, double collisionMargin);
+ void loadSoftBody(const std::string &fileName, const struct b3RobotSimulatorLoadSoftBodyArgs &args);
virtual void setGuiHelper(struct GUIHelperInterface *guiHelper);
virtual struct GUIHelperInterface *getGuiHelper();
diff --git a/examples/SharedMemory/grpc/ConvertGRPCBullet.cpp b/examples/SharedMemory/grpc/ConvertGRPCBullet.cpp
index 86acbe0d2..f23ce858f 100644
--- a/examples/SharedMemory/grpc/ConvertGRPCBullet.cpp
+++ b/examples/SharedMemory/grpc/ConvertGRPCBullet.cpp
@@ -1261,7 +1261,7 @@ bool convertGRPCToStatus(const PyBulletStatus& grpcReply, SharedMemoryStatus& se
serverStatus.m_sendActualStateArgs.m_numDegreeOfFreedomU = numDegreeOfFreedomU;
serverStatus.m_sendActualStateArgs.m_stateDetails = (SendActualStateSharedMemoryStorage*)bufferServerToClient;
-
+
for (int i = 0; i < numDegreeOfFreedomQ; i++)
{
serverStatus.m_sendActualStateArgs.m_stateDetails->m_actualStateQ[i] = stat->actualstateq(i);
@@ -1274,7 +1274,7 @@ bool convertGRPCToStatus(const PyBulletStatus& grpcReply, SharedMemoryStatus& se
{
serverStatus.m_sendActualStateArgs.m_rootLocalInertialFrame[i] = stat->rootlocalinertialframe(i);
}
- for (int i = 0; i < numLinks * 6; i++)
+ for (int i = 0; i < numLinks * 7; i++)
{
serverStatus.m_sendActualStateArgs.m_stateDetails->m_linkLocalInertialFrames[i] = stat->linklocalinertialframes(i);
}
@@ -1447,6 +1447,29 @@ bool convertGRPCToStatus(const PyBulletStatus& grpcReply, SharedMemoryStatus& se
serverStatus.m_sendPixelDataArguments.m_startingPixelIndex = cam->startingpixelindex();
break;
}
+ case CMD_GET_DYNAMICS_INFO_COMPLETED:
+ {
+ converted = true;
+ const ::pybullet_grpc::GetDynamicsStatus* stat = &grpcReply.getdynamicsstatus();
+ serverStatus.m_dynamicsInfo.m_mass = stat->mass();
+ serverStatus.m_dynamicsInfo.m_lateralFrictionCoeff = stat->lateralfriction();
+ serverStatus.m_dynamicsInfo.m_spinningFrictionCoeff = stat->spinningfriction();
+ serverStatus.m_dynamicsInfo.m_rollingFrictionCoeff = stat->rollingfriction();
+ serverStatus.m_dynamicsInfo.m_restitution = stat->restitution();
+ serverStatus.m_dynamicsInfo.m_linearDamping = stat->lineardamping();
+ serverStatus.m_dynamicsInfo.m_angularDamping = stat->angulardamping();
+ serverStatus.m_dynamicsInfo.m_contactStiffness = stat->contactstiffness();
+ serverStatus.m_dynamicsInfo.m_contactDamping = stat->contactdamping();
+ serverStatus.m_dynamicsInfo.m_localInertialDiagonal[0] = stat->localinertiadiagonal().x();
+ serverStatus.m_dynamicsInfo.m_localInertialDiagonal[1] = stat->localinertiadiagonal().y();
+ serverStatus.m_dynamicsInfo.m_localInertialDiagonal[2] = stat->localinertiadiagonal().z();
+ serverStatus.m_dynamicsInfo.m_frictionAnchor = stat->frictionanchor();
+ serverStatus.m_dynamicsInfo.m_ccdSweptSphereRadius = stat->ccdsweptsphereradius();
+ serverStatus.m_dynamicsInfo.m_contactProcessingThreshold = stat->contactprocessingthreshold();
+ serverStatus.m_dynamicsInfo.m_activationState = stat->activationstate();
+ break;
+ }
+
default:
{
#endif //ALLOW_GRPC_STATUS_CONVERSION
@@ -1706,7 +1729,7 @@ bool convertStatusToGRPC(const SharedMemoryStatus& serverStatus, char* bufferSer
{
stat->add_rootlocalinertialframe(rootLocalInertialFramePtr[i]);
}
- for (int i = 0; i < numLinks * 6; i++)
+ for (int i = 0; i < numLinks * 7; i++)
{
stat->add_linklocalinertialframes(linkLocalInertialFrames[i]);
}
diff --git a/examples/SharedMemory/plugins/grpcPlugin/grpcPlugin.cpp b/examples/SharedMemory/plugins/grpcPlugin/grpcPlugin.cpp
index eb6c04d9f..3bd0b2fc7 100644
--- a/examples/SharedMemory/plugins/grpcPlugin/grpcPlugin.cpp
+++ b/examples/SharedMemory/plugins/grpcPlugin/grpcPlugin.cpp
@@ -206,7 +206,9 @@ private:
{
GPR_ASSERT(status_ == FINISH);
// Once in the FINISH state, deallocate ourselves (CallData).
+ CallData::CallStatus tmpStatus = status_;
delete this;
+ return tmpStatus;
}
return status_;
}
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index 9946da3cf..5b8ca6e5b 100644
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -1942,7 +1942,7 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject*
int physicsClientId = 0;
int flags = 0;
- static char* kwlist[] = {"fileName", "scale", "mass", "collisionMargin", "physicsClientId", NULL};
+ static char* kwlist[] = {"fileName", "basePosition", "baseOrientation", "scale", "mass", "collisionMargin", "physicsClientId", NULL};
int bodyUniqueId = -1;
const char* fileName = "";
@@ -1952,10 +1952,36 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject*
b3PhysicsClientHandle sm = 0;
- if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|dddi", kwlist, &fileName, &scale, &mass, &collisionMargin, &physicsClientId))
+ double startPos[3] = {0.0, 0.0, 0.0};
+ double startOrn[4] = {0.0, 0.0, 0.0, 1.0};
+
+
+ PyObject* basePosObj = 0;
+ PyObject* baseOrnObj = 0;
+
+ if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOdddi", kwlist, &fileName, &basePosObj, &baseOrnObj, &scale, &mass, &collisionMargin, &physicsClientId))
{
return NULL;
}
+ else
+ {
+ if (basePosObj)
+ {
+ if (!pybullet_internalSetVectord(basePosObj, startPos))
+ {
+ PyErr_SetString(SpamError, "Cannot convert basePosition.");
+ return NULL;
+ }
+ }
+ if (baseOrnObj)
+ {
+ if (!pybullet_internalSetVector4d(baseOrnObj, startOrn))
+ {
+ PyErr_SetString(SpamError, "Cannot convert baseOrientation.");
+ return NULL;
+ }
+ }
+ }
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
@@ -1971,6 +1997,9 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject*
b3SharedMemoryCommandHandle command =
b3LoadSoftBodyCommandInit(sm, fileName);
+ b3LoadSoftBodySetStartPosition(command, startPos[0], startPos[1], startPos[2]);
+ b3LoadSoftBodySetStartOrientation(command, startOrn[0], startOrn[1], startOrn[2], startOrn[3]);
+
if (scale > 0)
{
b3LoadSoftBodySetScale(command, scale);
diff --git a/src/Bullet3Common/b3Vector3.h b/src/Bullet3Common/b3Vector3.h
index 56e6c1331..a70d68d6e 100644
--- a/src/Bullet3Common/b3Vector3.h
+++ b/src/Bullet3Common/b3Vector3.h
@@ -36,7 +36,7 @@ subject to the following restrictions:
#pragma warning(disable : 4556) // value of intrinsic immediate argument '4294967239' is out of range '0 - 255'
#endif
-#define B3_SHUFFLE(x, y, z, w) ((w) << 6 | (z) << 4 | (y) << 2 | (x))
+#define B3_SHUFFLE(x, y, z, w) (((w) << 6 | (z) << 4 | (y) << 2 | (x)) & 0xff)
//#define b3_pshufd_ps( _a, _mask ) (__m128) _mm_shuffle_epi32((__m128i)(_a), (_mask) )
#define b3_pshufd_ps(_a, _mask) _mm_shuffle_ps((_a), (_a), (_mask))
#define b3_splat3_ps(_a, _i) b3_pshufd_ps((_a), B3_SHUFFLE(_i, _i, _i, 3))
diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp
index 44a7ecaf9..8df15b58c 100644
--- a/src/BulletDynamics/Featherstone/btMultiBody.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp
@@ -344,6 +344,7 @@ void btMultiBody::finalizeMultiDof()
m_deltaV.resize(6 + m_dofCount);
m_realBuf.resize(6 + m_dofCount + m_dofCount * m_dofCount + 6 + m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels")
m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices)
+ m_matrixBuf.resize(m_links.size() + 1);
for (int i = 0; i < m_vectorBuf.size(); i++)
{
m_vectorBuf[i].setValue(0, 0, 0);
diff --git a/src/LinearMath/btVector3.h b/src/LinearMath/btVector3.h
index 61fd8d1e4..d65ed9808 100644
--- a/src/LinearMath/btVector3.h
+++ b/src/LinearMath/btVector3.h
@@ -36,7 +36,7 @@ subject to the following restrictions:
#pragma warning(disable : 4556) // value of intrinsic immediate argument '4294967239' is out of range '0 - 255'
#endif
-#define BT_SHUFFLE(x, y, z, w) ((w) << 6 | (z) << 4 | (y) << 2 | (x))
+#define BT_SHUFFLE(x, y, z, w) (((w) << 6 | (z) << 4 | (y) << 2 | (x)) & 0xff)
//#define bt_pshufd_ps( _a, _mask ) (__m128) _mm_shuffle_epi32((__m128i)(_a), (_mask) )
#define bt_pshufd_ps(_a, _mask) _mm_shuffle_ps((_a), (_a), (_mask))
#define bt_splat3_ps(_a, _i) bt_pshufd_ps((_a), BT_SHUFFLE(_i, _i, _i, 3))