diff options
author | Erwin Coumans <erwincoumans@google.com> | 2019-05-07 19:43:33 -0700 |
---|---|---|
committer | Erwin Coumans <erwincoumans@google.com> | 2019-05-07 19:43:33 -0700 |
commit | 36ed4419bff1ddc05b0b22d9887b807a2afa2d8d (patch) | |
tree | d9304dc5c5418ad4eb4c3e7c6d755daf293cac24 | |
parent | de8013ff75945a9d88e0892b15a1a3d52dc0fc7f (diff) | |
parent | defa172d39c3ecdd9a0bebf1771067a15fc44468 (diff) | |
download | bullet3-36ed4419bff1ddc05b0b22d9887b807a2afa2d8d.tar.gz |
Merge branch 'master' of https://github.com/erwincoumans/bullet3
-rw-r--r-- | examples/ExampleBrowser/CMakeLists.txt | 5 | ||||
-rw-r--r-- | examples/RoboticsLearning/GripperGraspExample.cpp | 8 | ||||
-rw-r--r-- | examples/SharedMemory/PhysicsClientC_API.cpp | 4 | ||||
-rw-r--r-- | examples/SharedMemory/PhysicsClientC_API.h | 2 | ||||
-rw-r--r-- | examples/SharedMemory/PhysicsDirect.cpp | 2 | ||||
-rw-r--r-- | examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 2 | ||||
-rw-r--r-- | examples/SharedMemory/SharedMemoryPublic.h | 2 | ||||
-rw-r--r-- | examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp | 10 | ||||
-rw-r--r-- | examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h | 39 | ||||
-rw-r--r-- | examples/SharedMemory/grpc/ConvertGRPCBullet.cpp | 29 | ||||
-rw-r--r-- | examples/SharedMemory/plugins/grpcPlugin/grpcPlugin.cpp | 2 | ||||
-rw-r--r-- | examples/pybullet/pybullet.c | 33 | ||||
-rw-r--r-- | src/Bullet3Common/b3Vector3.h | 2 | ||||
-rw-r--r-- | src/BulletDynamics/Featherstone/btMultiBody.cpp | 1 | ||||
-rw-r--r-- | src/LinearMath/btVector3.h | 2 |
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)) |