Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

fix some warnings, disable gimpact by default in world importer,

Browse Source 
use DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS by default for now,
until we find the issue with some failing test cases in btMultiBody
fix a crashing issue in MyMultiBodyCreator.cpp (uninitialized variable)
disable excessive debug printf in URDF2Bullet
This commit is contained in:
Erwin Coumans
2015-07-27 13:28:47 -07:00
parent ad03dba2fa
commit 26531f3fbc
27 changed files with 248 additions and 290 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
Extras/Serialize/BulletFileLoader/bFile.cpp
View File

@@ -1601,11 +1601,11 @@ void bFile::writeChunks(FILE* fp, bool fixupPointers)
// Ouch! need to rebuild the struct // Ouch! need to rebuild the struct
short *oldStruct,*curStruct; short *oldStruct,*curStruct;
char *oldType, *newType; char *oldType, *newType;
int oldLen, curLen, reverseOld; int curLen, reverseOld;
oldStruct = fileDna->getStruct(dataChunk.dna_nr); oldStruct = fileDna->getStruct(dataChunk.dna_nr);
oldType = fileDna->getType(oldStruct[0]); oldType = fileDna->getType(oldStruct[0]);
oldLen = fileDna->getLength(oldStruct[0]); //int oldLen = fileDna->getLength(oldStruct[0]);
///don't try to convert Link block data, just memcpy it. Other data can be converted. ///don't try to convert Link block data, just memcpy it. Other data can be converted.
reverseOld = mMemoryDNA->getReverseType(oldType); reverseOld = mMemoryDNA->getReverseType(oldType);

3
Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.cpp
View File

@@ -18,8 +18,9 @@ subject to the following restrictions:
#include "../BulletFileLoader/btBulletFile.h" #include "../BulletFileLoader/btBulletFile.h"
#include "btBulletDynamicsCommon.h" #include "btBulletDynamicsCommon.h"
#ifndef USE_GIMPACT
#include "BulletCollision/Gimpact/btGImpactShape.h" #include "BulletCollision/Gimpact/btGImpactShape.h"
#endif
//#define USE_INTERNAL_EDGE_UTILITY //#define USE_INTERNAL_EDGE_UTILITY

9
Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp
View File

@@ -15,8 +15,9 @@ subject to the following restrictions:
#include "btWorldImporter.h" #include "btWorldImporter.h"
#include "btBulletDynamicsCommon.h" #include "btBulletDynamicsCommon.h"
#ifdef USE_GIMPACT
#include "BulletCollision/Gimpact/btGImpactShape.h" #include "BulletCollision/Gimpact/btGImpactShape.h"
#endif
btWorldImporter::btWorldImporter(btDynamicsWorld* world) btWorldImporter::btWorldImporter(btDynamicsWorld* world)
:m_dynamicsWorld(world), :m_dynamicsWorld(world),
m_verboseMode(0) m_verboseMode(0)
@@ -177,6 +178,7 @@ btCollisionShape* btWorldImporter::convertCollisionShape( btCollisionShapeData*
} }
case GIMPACT_SHAPE_PROXYTYPE: case GIMPACT_SHAPE_PROXYTYPE:
{ {
#ifdef USE_GIMPACT
btGImpactMeshShapeData* gimpactData = (btGImpactMeshShapeData*) shapeData; btGImpactMeshShapeData* gimpactData = (btGImpactMeshShapeData*) shapeData;
if (gimpactData->m_gimpactSubType == CONST_GIMPACT_TRIMESH_SHAPE) if (gimpactData->m_gimpactSubType == CONST_GIMPACT_TRIMESH_SHAPE)
{ {
@@ -195,6 +197,7 @@ btCollisionShape* btWorldImporter::convertCollisionShape( btCollisionShapeData*
{ {
printf("unsupported gimpact sub type\n"); printf("unsupported gimpact sub type\n");
} }
#endif//USE_GIMPACT
break; break;
} }
//The btCapsuleShape* API has issue passing the margin/scaling/halfextents unmodified through the API //The btCapsuleShape* API has issue passing the margin/scaling/halfextents unmodified through the API
@@ -1792,9 +1795,13 @@ btCollisionShape* btWorldImporter::createConvexTriangleMeshShape(btStridingMeshI
} }
btGImpactMeshShape* btWorldImporter::createGimpactShape(btStridingMeshInterface* trimesh) btGImpactMeshShape* btWorldImporter::createGimpactShape(btStridingMeshInterface* trimesh)
{ {
#ifdef USE_GIMPACT
btGImpactMeshShape* shape = new btGImpactMeshShape(trimesh); btGImpactMeshShape* shape = new btGImpactMeshShape(trimesh);
m_allocatedCollisionShapes.push_back(shape); m_allocatedCollisionShapes.push_back(shape);
return shape; return shape;
#else
return 0;
#endif
} }
btConvexHullShape* btWorldImporter::createConvexHullShape() btConvexHullShape* btWorldImporter::createConvexHullShape()

4
examples/BasicDemo/BasicExample.cpp
View File

@@ -73,7 +73,7 @@ void BasicExample::initPhysics()
{ {
btScalar mass(0.); btScalar mass(0.);
btRigidBody* body = createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1)); createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1));
} }
@@ -113,7 +113,7 @@ void BasicExample::initPhysics()
btScalar(2.0*j))); btScalar(2.0*j)));
btRigidBody* body = createRigidBody(mass,startTransform,colShape); createRigidBody(mass,startTransform,colShape);
} }

1
examples/BasicDemo/main.cpp
View File

@@ -24,7 +24,6 @@ subject to the following restrictions:
int main(int argc, char* argv[]) int main(int argc, char* argv[])
{ {
struct PhysicsInterface* pint = 0;
DummyGUIHelper noGfx; DummyGUIHelper noGfx;

2
examples/ExampleBrowser/CMakeLists.txt
View File

@@ -15,9 +15,9 @@ SET(App_ExampleBrowser_SRCS
main.cpp main.cpp
ExampleEntries.cpp ExampleEntries.cpp
ExampleEntries.h ExampleEntries.h
../SharedMemory/PhysicsClientC_API.cpp
../SharedMemory/PhysicsServer.cpp ../SharedMemory/PhysicsServer.cpp
../SharedMemory/PhysicsClient.cpp ../SharedMemory/PhysicsClient.cpp
../SharedMemory/PhysicsClientC_API.cpp
../SharedMemory/PhysicsServerExample.cpp ../SharedMemory/PhysicsServerExample.cpp
../SharedMemory/PhysicsClientExample.cpp ../SharedMemory/PhysicsClientExample.cpp
../SharedMemory/RobotControlExample.cpp ../SharedMemory/RobotControlExample.cpp

4
examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.cpp
View File

@@ -167,8 +167,8 @@ void GwenUserInterface::setExampleDescription(const char* message)
m_data->m_exampleInfoTextOutput->Clear(); m_data->m_exampleInfoTextOutput->Clear();
int fixedWidth = m_data->m_exampleInfoTextOutput->GetBounds().w-25; int fixedWidth = m_data->m_exampleInfoTextOutput->GetBounds().w-25;
int wrapLen = int(wrapmessage.length());
for (int endPos=0;endPos<=wrapmessage.length();endPos++) for (int endPos=0;endPos<=wrapLen;endPos++)
{ {
std::string sub = wrapmessage.substr(startPos,(endPos-startPos)); std::string sub = wrapmessage.substr(startPos,(endPos-startPos));
Gwen::Point pt = m_data->pRenderer->MeasureText(m_data->pCanvas->GetSkin()->GetDefaultFont(),sub); Gwen::Point pt = m_data->pRenderer->MeasureText(m_data->pCanvas->GetSkin()->GetDefaultFont(),sub);

3
examples/ExampleBrowser/OpenGLExampleBrowser.cpp
View File

@@ -240,7 +240,6 @@ void openFileDemo(const char* filename)
options.m_fileName = filename; options.m_fileName = filename;
char fullPath[1024]; char fullPath[1024];
int fileType = 0;
sprintf(fullPath, "%s", filename); sprintf(fullPath, "%s", filename);
b3FileUtils::toLower(fullPath); b3FileUtils::toLower(fullPath);
if (strstr(fullPath, ".urdf")) if (strstr(fullPath, ".urdf"))
@@ -296,7 +295,6 @@ void selectDemo(int demoIndex)
{ {
if (gui) if (gui)
{ {
bool isLeft = true;
gui->setStatusBarMessage("Status: OK", false); gui->setStatusBarMessage("Status: OK", false);
} }
b3Printf("Selected demo: %s",gAllExamples->getExampleName(demoIndex)); b3Printf("Selected demo: %s",gAllExamples->getExampleName(demoIndex));
@@ -429,7 +427,6 @@ struct MyMenuItemHander :public Gwen::Event::Handler
Gwen::String laa = Gwen::Utility::UnicodeToString(la); Gwen::String laa = Gwen::Utility::UnicodeToString(la);
//const char* ha = laa.c_str(); //const char* ha = laa.c_str();
bool handled = false;
selectDemo(sCurrentHightlighted); selectDemo(sCurrentHightlighted);
saveCurrentDemoEntry(sCurrentDemoIndex, startFileName); saveCurrentDemoEntry(sCurrentDemoIndex, startFileName);

2
examples/ExampleBrowser/main.cpp
View File

@@ -16,7 +16,7 @@
int main(int argc, char* argv[]) int main(int argc, char* argv[])
{ {
b3CommandLineArgs args(argc,argv); b3CommandLineArgs args(argc,argv);
b3Clock clock; b3Clock clock;
ExampleEntries examples; ExampleEntries examples;

7
examples/ExampleBrowser/premake4.lua
View File

@@ -48,9 +48,12 @@
files { files {
"**.cpp", "*.cpp",
"**.h", "*.h",
"GwenGUISupport/*.cpp",
"GwenGUISupport/*.h",
"../SharedMemory/PhysicsClientC_API.cpp", "../SharedMemory/PhysicsClientC_API.cpp",
"../SharedMemory/PhysicsClientC_API.h",
"../SharedMemory/PhysicsServerExample.cpp", "../SharedMemory/PhysicsServerExample.cpp",
"../SharedMemory/PhysicsClientExample.cpp", "../SharedMemory/PhysicsClientExample.cpp",
"../SharedMemory/RobotControlExample.cpp", "../SharedMemory/RobotControlExample.cpp",

3
examples/Importers/ImportBullet/SerializeSetup.cpp
View File

@@ -59,8 +59,6 @@ void SerializeSetup::initPhysics()
int numPrefixes = sizeof(prefix)/sizeof(const char*); int numPrefixes = sizeof(prefix)/sizeof(const char*);
char relativeFileName[1024]; char relativeFileName[1024];
FILE* f=0; FILE* f=0;
bool fileFound = false;
int result = 0;
for (int i=0;!f && i<numPrefixes;i++) for (int i=0;!f && i<numPrefixes;i++)
{ {
@@ -68,7 +66,6 @@ void SerializeSetup::initPhysics()
f = fopen(relativeFileName,"rb"); f = fopen(relativeFileName,"rb");
if (f) if (f)
{ {
fileFound = true;
break; break;
} }
} }

2
examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
View File

@@ -231,7 +231,7 @@ void ImportUrdfSetup::initPhysics()
if (loadOk) if (loadOk)
{ {
printTree(u2b,u2b.getRootLinkIndex()); //printTree(u2b,u2b.getRootLinkIndex());
//u2b.printTree(); //u2b.printTree();

2
examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
View File

@@ -156,8 +156,8 @@ class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createRevoluteJoint(in
userInfo->m_urdfJointType = URDFRevoluteJoint; userInfo->m_urdfJointType = URDFRevoluteJoint;
userInfo->m_lowerJointLimit = jointLowerLimit; userInfo->m_lowerJointLimit = jointLowerLimit;
userInfo->m_upperJointLimit = jointUpperLimit; userInfo->m_upperJointLimit = jointUpperLimit;
userInfo->m_urdfIndex = urdfLinkIndex;
} }
userInfo->m_urdfIndex = urdfLinkIndex;
dof6->setUserConstraintPtr(userInfo); dof6->setUserConstraintPtr(userInfo);
m_6DofConstraints.push_back(dof6); m_6DofConstraints.push_back(dof6);
return dof6; return dof6;

61
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -10,7 +10,6 @@
#include "URDFImporterInterface.h" #include "URDFImporterInterface.h"
#include "MultiBodyCreationInterface.h" #include "MultiBodyCreationInterface.h"
#include <string> #include <string>
#include "Bullet3Common/b3Logging.h"
static int bodyCollisionFilterGroup=btBroadphaseProxy::CharacterFilter; static int bodyCollisionFilterGroup=btBroadphaseProxy::CharacterFilter;
static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphaseProxy::CharacterFilter); static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphaseProxy::CharacterFilter);
@@ -35,42 +34,6 @@ static btVector3 selectColor2()
return color; return color;
} }
void printTree(const URDFImporterInterface& u2b, int linkIndex, int indentationLevel)
{
btScalar mass;
btVector3 localInertia;
btTransform inertialFrame;
u2b.getMassAndInertia(linkIndex,mass,localInertia,inertialFrame);
std::string name = u2b.getLinkName(linkIndex);
for(int j=0;j<indentationLevel;j++)
b3Printf(" "); //indent
b3Printf("link %s mass=%f\n",name.c_str(),mass);
for(int j=0;j<indentationLevel;j++)
b3Printf(" "); //indent
b3Printf("local inertia:%f,%f,%f\n",localInertia[0],
localInertia[1],
localInertia[2]);
btAlignedObjectArray<int> childIndices;
u2b.getLinkChildIndices(linkIndex,childIndices);
int numChildren = childIndices.size();
indentationLevel+=2;
int count = 0;
for (int i=0;i<numChildren;i++)
{
int childLinkIndex = childIndices[i];
std::string name = u2b.getLinkName(childLinkIndex);
for(int j=0;j<indentationLevel;j++)
b3Printf(" "); //indent
b3Printf("child(%d).name=%s with childIndex=%d\n",(count++)+1, name.c_str(),childLinkIndex);
// first grandchild
printTree(u2b,childLinkIndex,indentationLevel);
}
}
struct URDF2BulletCachedData struct URDF2BulletCachedData
@@ -182,7 +145,7 @@ void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedDat
void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix) void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix)
{ {
b3Printf("start converting/extracting data from URDF interface\n"); //b3Printf("start converting/extracting data from URDF interface\n");
btTransform linkTransformInWorldSpace; btTransform linkTransformInWorldSpace;
linkTransformInWorldSpace.setIdentity(); linkTransformInWorldSpace.setIdentity();
@@ -194,9 +157,9 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex); int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex);
btRigidBody* parentRigidBody = 0; btRigidBody* parentRigidBody = 0;
std::string name = u2b.getLinkName(urdfLinkIndex); //std::string name = u2b.getLinkName(urdfLinkIndex);
b3Printf("link name=%s urdf link index=%d\n",name.c_str(),urdfLinkIndex); //b3Printf("link name=%s urdf link index=%d\n",name.c_str(),urdfLinkIndex);
b3Printf("mb link index = %d\n",mbLinkIndex); //b3Printf("mb link index = %d\n",mbLinkIndex);
btTransform parentLocalInertialFrame; btTransform parentLocalInertialFrame;
parentLocalInertialFrame.setIdentity(); parentLocalInertialFrame.setIdentity();
@@ -205,11 +168,11 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
if (urdfParentIndex==-2) if (urdfParentIndex==-2)
{ {
b3Printf("root link has no parent\n"); //b3Printf("root link has no parent\n");
} else } else
{ {
b3Printf("urdf parent index = %d\n",urdfParentIndex); //b3Printf("urdf parent index = %d\n",urdfParentIndex);
b3Printf("mb parent index = %d\n",mbParentIndex); //b3Printf("mb parent index = %d\n",mbParentIndex);
parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex); parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex);
u2b.getMassAndInertia(urdfParentIndex, parentMass,parentLocalInertiaDiagonal,parentLocalInertialFrame); u2b.getMassAndInertia(urdfParentIndex, parentMass,parentLocalInertiaDiagonal,parentLocalInertialFrame);
@@ -305,7 +268,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
{ {
//todo: adjust the center of mass transform and pivot axis properly //todo: adjust the center of mass transform and pivot axis properly
b3Printf("Fixed joint (btMultiBody)\n"); //b3Printf("Fixed joint (btMultiBody)\n");
btQuaternion rot = offsetInA.inverse().getRotation();//parent2joint.inverse().getRotation(); btQuaternion rot = offsetInA.inverse().getRotation();//parent2joint.inverse().getRotation();
cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
rot*offsetInB.getRotation(), offsetInA.getOrigin(),-offsetInB.getOrigin(),disableParentCollision); rot*offsetInB.getRotation(), offsetInA.getOrigin(),-offsetInB.getOrigin(),disableParentCollision);
@@ -314,7 +277,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
} else } else
{ {
b3Printf("Fixed joint\n"); //b3Printf("Fixed joint\n");
btGeneric6DofSpring2Constraint* dof6 = creation.createFixedJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB); btGeneric6DofSpring2Constraint* dof6 = creation.createFixedJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
@@ -343,7 +306,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
if (enableConstraints) if (enableConstraints)
world1->addConstraint(dof6,true); world1->addConstraint(dof6,true);
b3Printf("Revolute/Continuous joint\n"); //b3Printf("Revolute/Continuous joint\n");
} }
break; break;
} }
@@ -371,13 +334,13 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
if (enableConstraints) if (enableConstraints)
world1->addConstraint(dof6,true); world1->addConstraint(dof6,true);
b3Printf("Prismatic\n"); //b3Printf("Prismatic\n");
} }
break; break;
} }
default: default:
{ {
b3Printf("Error: unsupported joint type in URDF (%d)\n", jointType); //b3Printf("Error: unsupported joint type in URDF (%d)\n", jointType);
btAssert(0); btAssert(0);
} }
} }

1
examples/Importers/ImportURDFDemo/URDF2Bullet.h
View File

@@ -13,7 +13,6 @@ class URDFImporterInterface;
class MultiBodyCreationInterface; class MultiBodyCreationInterface;
void printTree(const URDFImporterInterface& u2b, int linkIndex, int identationLevel=0);
void ConvertURDF2Bullet(const URDFImporterInterface& u2b, void ConvertURDF2Bullet(const URDFImporterInterface& u2b,

4
examples/MultiBody/MultiDofDemo.cpp
View File

@@ -320,7 +320,7 @@ void MultiDofDemo::addColliders_testMultiDof(btMultiBody *pMultiBody, btMultiBod
{ {
// float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1}; // float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()}; btScalar quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
if (1) if (1)
@@ -360,7 +360,7 @@ void MultiDofDemo::addColliders_testMultiDof(btMultiBody *pMultiBody, btMultiBod
btVector3 posr = local_origin[i+1]; btVector3 posr = local_origin[i+1];
// float pos[4]={posr.x(),posr.y(),posr.z(),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()}; 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* box = new btBoxShape(linkHalfExtents); btCollisionShape* box = new btBoxShape(linkHalfExtents);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i); btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);

9
examples/MultiBody/TestJointTorqueSetup.cpp
View File

@@ -96,7 +96,6 @@ void TestJointTorqueSetup::initPhysics()
m_guiHelper->createCollisionShapeGraphicsObject(box); m_guiHelper->createCollisionShapeGraphicsObject(box);
btTransform start; start.setIdentity(); btTransform start; start.setIdentity();
btVector3 groundOrigin(-0.4f, 3.f, 0.f); btVector3 groundOrigin(-0.4f, 3.f, 0.f);
btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
groundOrigin[upAxis] -=.5; groundOrigin[upAxis] -=.5;
groundOrigin[2]-=0.6; groundOrigin[2]-=0.6;
start.setOrigin(groundOrigin); start.setOrigin(groundOrigin);
@@ -269,11 +268,11 @@ void TestJointTorqueSetup::initPhysics()
local_origin.resize(pMultiBody->getNumLinks() + 1); local_origin.resize(pMultiBody->getNumLinks() + 1);
world_to_local[0] = pMultiBody->getWorldToBaseRot(); world_to_local[0] = pMultiBody->getWorldToBaseRot();
local_origin[0] = pMultiBody->getBasePos(); local_origin[0] = pMultiBody->getBasePos();
double friction = 1; // double friction = 1;
{ {
// float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1}; // float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()}; // btScalar quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
if (1) if (1)
@@ -326,7 +325,7 @@ void TestJointTorqueSetup::initPhysics()
btVector3 posr = local_origin[i+1]; btVector3 posr = local_origin[i+1];
// float pos[4]={posr.x(),posr.y(),posr.z(),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()}; 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; btCollisionShape* shape =0;
if (i==0) if (i==0)
@@ -422,7 +421,7 @@ void TestJointTorqueSetup::stepSimulation(float deltaTime)
m_multiBody->getBaseOmega()[2] m_multiBody->getBaseOmega()[2]
); );
*/ */
btScalar jointVel =m_multiBody->getJointVel(0); // btScalar jointVel =m_multiBody->getJointVel(0);
// b3Printf("child angvel = %f",jointVel); // b3Printf("child angvel = %f",jointVel);

4
examples/OpenGLWindow/GLInstancingRenderer.cpp
View File

@@ -1170,7 +1170,7 @@ struct PointerCaster
#if 0
static void b3CreateFrustum( static void b3CreateFrustum(
float left, float left,
float right, float right,
@@ -1202,7 +1202,7 @@ static void b3CreateFrustum(
frustum[3*4+3] = float(0); frustum[3*4+3] = float(0);
} }
#endif
static void b3Matrix4x4Mul(GLfloat aIn[4][4], GLfloat bIn[4][4], GLfloat result[4][4]) static void b3Matrix4x4Mul(GLfloat aIn[4][4], GLfloat bIn[4][4], GLfloat result[4][4])
{ {

13
examples/OpenGLWindow/SimpleCamera.cpp
View File

@@ -10,13 +10,13 @@ struct SimpleCameraInternalData
:m_cameraTargetPosition(b3MakeVector3(0,0,0)), :m_cameraTargetPosition(b3MakeVector3(0,0,0)),
m_cameraDistance(20), m_cameraDistance(20),
m_cameraUp(b3MakeVector3(0,1,0)), m_cameraUp(b3MakeVector3(0,1,0)),
m_cameraUpAxis(1),
m_cameraForward(b3MakeVector3(1,0,0)), m_cameraForward(b3MakeVector3(1,0,0)),
m_frustumZNear(0.01), m_cameraUpAxis(1),
m_frustumZFar(1000),
m_yaw(20), m_yaw(20),
m_pitch(0), m_pitch(0),
m_aspect(1) m_aspect(1),
m_frustumZNear(0.01),
m_frustumZFar(1000)
{ {
} }
b3Vector3 m_cameraTargetPosition; b3Vector3 m_cameraTargetPosition;
@@ -83,7 +83,7 @@ static void b3CreateFrustum(
#if 0
static void b3CreateDiagonalMatrix(float value, float result[4][4]) static void b3CreateDiagonalMatrix(float value, float result[4][4])
{ {
for (int i=0;i<4;i++) for (int i=0;i<4;i++)
@@ -100,7 +100,6 @@ static void b3CreateDiagonalMatrix(float value, float result[4][4])
} }
} }
} }
static void b3CreateOrtho(float left, float right, float bottom, float top, float zNear, float zFar, float result[4][4]) static void b3CreateOrtho(float left, float right, float bottom, float top, float zNear, float zFar, float result[4][4])
{ {
b3CreateDiagonalMatrix(1.f,result); b3CreateDiagonalMatrix(1.f,result);
@@ -112,7 +111,7 @@ static void b3CreateOrtho(float left, float right, float bottom, float top, floa
result[3][1] = - (top + bottom) / (top - bottom); result[3][1] = - (top + bottom) / (top - bottom);
result[3][2] = - (zFar + zNear) / (zFar - zNear); result[3][2] = - (zFar + zNear) / (zFar - zNear);
} }
#endif
static void b3CreateLookAt(const b3Vector3& eye, const b3Vector3& center,const b3Vector3& up, float result[16]) static void b3CreateLookAt(const b3Vector3& eye, const b3Vector3& center,const b3Vector3& up, float result[16])
{ {
b3Vector3 f = (center - eye).normalized(); b3Vector3 f = (center - eye).normalized();

9
examples/OpenGLWindow/SimpleOpenGL2App.cpp
View File

@@ -325,9 +325,8 @@ void SimpleOpenGL2App::drawText3D( const char* txt, float worldPosX, float world
float camPos[4]; float camPos[4];
cam->getCameraPosition(camPos); cam->getCameraPosition(camPos);
b3Vector3 cp= b3MakeVector3(camPos[0],camPos[2],camPos[1]); //b3Vector3 cp= b3MakeVector3(camPos[0],camPos[2],camPos[1]);
b3Vector3 p = b3MakeVector3(worldPosX,worldPosY,worldPosZ); // b3Vector3 p = b3MakeVector3(worldPosX,worldPosY,worldPosZ);
float dx=0;
glEnable(GL_BLEND); glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
@@ -399,11 +398,11 @@ void SimpleOpenGL2App::drawText3D( const char* txt, float worldPosX, float world
float z = 2.f*winz-1.f;//*(far float z = 2.f*winz-1.f;//*(far
float identity[16]={1,0,0,0, /*float identity[16]={1,0,0,0,
0,1,0,0, 0,1,0,0,
0,0,1,0, 0,0,1,0,
0,0,0,1}; 0,0,0,1};
*/
PrimVertex vertexData[4] = { PrimVertex vertexData[4] = {
{ PrimVec4(-1.f+2.f*x0/float(screenWidth), 1.f-2.f*y0/float(screenHeight), z, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v0)}, { PrimVec4(-1.f+2.f*x0/float(screenWidth), 1.f-2.f*y0/float(screenHeight), z, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v0)},
{ PrimVec4(-1.f+2.f*x0/float(screenWidth), 1.f-2.f*y1/float(screenHeight), z, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v1)}, { PrimVec4(-1.f+2.f*x0/float(screenWidth), 1.f-2.f*y1/float(screenHeight), z, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v1)},

18
examples/OpenGLWindow/SimpleOpenGL3App.cpp
View File

@@ -224,8 +224,8 @@ void SimpleOpenGL3App::drawText3D( const char* txt, float worldPosX, float world
float camPos[4]; float camPos[4];
cam->getCameraPosition(camPos); cam->getCameraPosition(camPos);
b3Vector3 cp= b3MakeVector3(camPos[0],camPos[2],camPos[1]); //b3Vector3 cp= b3MakeVector3(camPos[0],camPos[2],camPos[1]);
b3Vector3 p = b3MakeVector3(worldPosX,worldPosY,worldPosZ); //b3Vector3 p = b3MakeVector3(worldPosX,worldPosY,worldPosZ);
//float dist = (cp-p).length(); //float dist = (cp-p).length();
//float dv = 0;//dist/1000.f; //float dv = 0;//dist/1000.f;
// //
@@ -545,8 +545,8 @@ void SimpleOpenGL3App::drawGrid(DrawGridData data)
}; };
//b3Vector3 gridColor = b3MakeVector3(0.5,0.5,0.5); //b3Vector3 gridColor = b3MakeVector3(0.5,0.5,0.5);
b3AlignedObjectArray<unsigned int> indices; b3AlignedObjectArray<unsigned int> indices;
b3AlignedObjectArray<b3Vector3> vertices; b3AlignedObjectArray<b3Vector3> vertices;
int lineIndex=0; int lineIndex=0;
for(int i=-gridSize;i<=gridSize;i++) for(int i=-gridSize;i<=gridSize;i++)
{ {
@@ -564,7 +564,7 @@ void SimpleOpenGL3App::drawGrid(DrawGridData data)
indices.push_back(lineIndex++); indices.push_back(lineIndex++);
vertices.push_back(to); vertices.push_back(to);
indices.push_back(lineIndex++); indices.push_back(lineIndex++);
m_instancingRenderer->drawLine(from,to,gridColor); // m_instancingRenderer->drawLine(from,to,gridColor);
} }
b3Assert(glGetError() ==GL_NO_ERROR); b3Assert(glGetError() ==GL_NO_ERROR);
@@ -583,16 +583,16 @@ void SimpleOpenGL3App::drawGrid(DrawGridData data)
indices.push_back(lineIndex++); indices.push_back(lineIndex++);
vertices.push_back(to); vertices.push_back(to);
indices.push_back(lineIndex++); indices.push_back(lineIndex++);
m_instancingRenderer->drawLine(from,to,gridColor); // m_instancingRenderer->drawLine(from,to,gridColor);
} }
} }
/*m_instancingRenderer->drawLines(&vertices[0].x, m_instancingRenderer->drawLines(&vertices[0].x,
gridColor, gridColor,
vertices.size(),sizeof(b3Vector3),&indices[0],indices.size(),1); vertices.size(),sizeof(b3Vector3),&indices[0],indices.size(),1);
*/
m_instancingRenderer->drawLine(b3MakeVector3(0,0,0),b3MakeVector3(1,0,0),b3MakeVector3(1,0,0),3); m_instancingRenderer->drawLine(b3MakeVector3(0,0,0),b3MakeVector3(1,0,0),b3MakeVector3(1,0,0),3);
m_instancingRenderer->drawLine(b3MakeVector3(0,0,0),b3MakeVector3(0,1,0),b3MakeVector3(0,1,0),3); m_instancingRenderer->drawLine(b3MakeVector3(0,0,0),b3MakeVector3(0,1,0),b3MakeVector3(0,1,0),3);
@@ -766,7 +766,7 @@ void SimpleOpenGL3App::dumpNextFrameToPng(const char* filename)
m_data->m_renderTexture->init(m_instancingRenderer->getScreenWidth(),this->m_instancingRenderer->getScreenHeight(),renderTextureId, RENDERTEXTURE_COLOR); m_data->m_renderTexture->init(m_instancingRenderer->getScreenWidth(),this->m_instancingRenderer->getScreenHeight(),renderTextureId, RENDERTEXTURE_COLOR);
} }
bool result = m_data->m_renderTexture->enable(); m_data->m_renderTexture->enable();
} }

6
examples/SimpleOpenGL3/main.cpp
View File

@@ -14,9 +14,9 @@ int main(int argc, char* argv[])
SimpleOpenGL3App* app = new SimpleOpenGL3App("SimpleOpenGL3App",1024,768); SimpleOpenGL3App* app = new SimpleOpenGL3App("SimpleOpenGL3App",1024,768);
app->m_instancingRenderer->setCameraDistance(13); app->m_instancingRenderer->getActiveCamera()->setCameraDistance(13);
app->m_instancingRenderer->setCameraPitch(0); app->m_instancingRenderer->getActiveCamera()->setCameraPitch(0);
app->m_instancingRenderer->setCameraTargetPosition(b3MakeVector3(0,0,0)); app->m_instancingRenderer->getActiveCamera()->setCameraTargetPosition(0,0,0);
assert(glGetError()==GL_NO_ERROR); assert(glGetError()==GL_NO_ERROR);

1
examples/ThirdPartyLibs/Gwen/Gwen.h
View File

@@ -24,7 +24,6 @@
THE SOFTWARE. THE SOFTWARE.
*/ */
#pragma once
#ifndef GWEN_GWEN_H #ifndef GWEN_GWEN_H
#define GWEN_GWEN_H #define GWEN_GWEN_H

2
examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
View File

@@ -171,7 +171,7 @@ updateVertex(
return it->second; return it->second;
} }
assert(in_positions.size() > (3*i.v_idx+2)); assert(static_cast<int>(in_positions.size()) > (3*i.v_idx+2));
positions.push_back(in_positions[3*i.v_idx+0]); positions.push_back(in_positions[3*i.v_idx+0]);
positions.push_back(in_positions[3*i.v_idx+1]); positions.push_back(in_positions[3*i.v_idx+1]);

346
src/Bullet3Common/b3Vector3.h
View File

@@ -3,8 +3,8 @@ Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans http://bulletphysic
This software is provided 'as-is', without any express or implied warranty. This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software. In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
@@ -22,8 +22,6 @@ subject to the following restrictions:
#include "b3MinMax.h" #include "b3MinMax.h"
#include "b3AlignedAllocator.h" #include "b3AlignedAllocator.h"
#ifdef B3_USE_DOUBLE_PRECISION #ifdef B3_USE_DOUBLE_PRECISION
#define b3Vector3Data b3Vector3DoubleData #define b3Vector3Data b3Vector3DoubleData
#define b3Vector3DataName "b3Vector3DoubleData" #define b3Vector3DataName "b3Vector3DoubleData"
@@ -99,7 +97,7 @@ public:
b3SimdFloat4 mVec128; b3SimdFloat4 mVec128;
float m_floats[4]; float m_floats[4];
struct {float x,y,z,w;}; struct {float x,y,z,w;};
}; };
#else #else
union union
@@ -133,9 +131,9 @@ public:
public: public:
/**@brief Add a vector to this one /**@brief Add a vector to this one
* @param The vector to add to this one */ * @param The vector to add to this one */
B3_FORCE_INLINE b3Vector3& operator+=(const b3Vector3& v) B3_FORCE_INLINE b3Vector3& operator+=(const b3Vector3& v)
{ {
@@ -144,7 +142,7 @@ public:
#elif defined(B3_USE_NEON) #elif defined(B3_USE_NEON)
mVec128 = vaddq_f32(mVec128, v.mVec128); mVec128 = vaddq_f32(mVec128, v.mVec128);
#else #else
m_floats[0] += v.m_floats[0]; m_floats[0] += v.m_floats[0];
m_floats[1] += v.m_floats[1]; m_floats[1] += v.m_floats[1];
m_floats[2] += v.m_floats[2]; m_floats[2] += v.m_floats[2];
#endif #endif
@@ -154,20 +152,20 @@ public:
/**@brief Subtract a vector from this one /**@brief Subtract a vector from this one
* @param The vector to subtract */ * @param The vector to subtract */
B3_FORCE_INLINE b3Vector3& operator-=(const b3Vector3& v) B3_FORCE_INLINE b3Vector3& operator-=(const b3Vector3& v)
{ {
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) #if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
mVec128 = _mm_sub_ps(mVec128, v.mVec128); mVec128 = _mm_sub_ps(mVec128, v.mVec128);
#elif defined(B3_USE_NEON) #elif defined(B3_USE_NEON)
mVec128 = vsubq_f32(mVec128, v.mVec128); mVec128 = vsubq_f32(mVec128, v.mVec128);
#else #else
m_floats[0] -= v.m_floats[0]; m_floats[0] -= v.m_floats[0];
m_floats[1] -= v.m_floats[1]; m_floats[1] -= v.m_floats[1];
m_floats[2] -= v.m_floats[2]; m_floats[2] -= v.m_floats[2];
#endif #endif
return *this; return *this;
} }
/**@brief Scale the vector /**@brief Scale the vector
* @param s Scale factor */ * @param s Scale factor */
B3_FORCE_INLINE b3Vector3& operator*=(const b3Scalar& s) B3_FORCE_INLINE b3Vector3& operator*=(const b3Scalar& s)
@@ -179,16 +177,16 @@ public:
#elif defined(B3_USE_NEON) #elif defined(B3_USE_NEON)
mVec128 = vmulq_n_f32(mVec128, s); mVec128 = vmulq_n_f32(mVec128, s);
#else #else
m_floats[0] *= s; m_floats[0] *= s;
m_floats[1] *= s; m_floats[1] *= s;
m_floats[2] *= s; m_floats[2] *= s;
#endif #endif
return *this; return *this;
} }
/**@brief Inversely scale the vector /**@brief Inversely scale the vector
* @param s Scale factor to divide by */ * @param s Scale factor to divide by */
B3_FORCE_INLINE b3Vector3& operator/=(const b3Scalar& s) B3_FORCE_INLINE b3Vector3& operator/=(const b3Scalar& s)
{ {
b3FullAssert(s != b3Scalar(0.0)); b3FullAssert(s != b3Scalar(0.0));
@@ -199,7 +197,7 @@ public:
vs = b3_pshufd_ps(vs, 0x00); // (S S S S) vs = b3_pshufd_ps(vs, 0x00); // (S S S S)
mVec128 = _mm_mul_ps(mVec128, vs); mVec128 = _mm_mul_ps(mVec128, vs);
return *this; return *this;
#else #else
return *this *= b3Scalar(1.0) / s; return *this *= b3Scalar(1.0) / s;
@@ -210,7 +208,7 @@ public:
* @param v The other vector in the dot product */ * @param v The other vector in the dot product */
B3_FORCE_INLINE b3Scalar dot(const b3Vector3& v) const B3_FORCE_INLINE b3Scalar dot(const b3Vector3& v) const
{ {
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) #if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
__m128 vd = _mm_mul_ps(mVec128, v.mVec128); __m128 vd = _mm_mul_ps(mVec128, v.mVec128);
__m128 z = _mm_movehl_ps(vd, vd); __m128 z = _mm_movehl_ps(vd, vd);
__m128 y = _mm_shuffle_ps(vd, vd, 0x55); __m128 y = _mm_shuffle_ps(vd, vd, 0x55);
@@ -219,12 +217,12 @@ public:
return _mm_cvtss_f32(vd); return _mm_cvtss_f32(vd);
#elif defined(B3_USE_NEON) #elif defined(B3_USE_NEON)
float32x4_t vd = vmulq_f32(mVec128, v.mVec128); float32x4_t vd = vmulq_f32(mVec128, v.mVec128);
float32x2_t x = vpadd_f32(vget_low_f32(vd), vget_low_f32(vd)); float32x2_t x = vpadd_f32(vget_low_f32(vd), vget_low_f32(vd));
x = vadd_f32(x, vget_high_f32(vd)); x = vadd_f32(x, vget_high_f32(vd));
return vget_lane_f32(x, 0); return vget_lane_f32(x, 0);
#else #else
return m_floats[0] * v.m_floats[0] + return m_floats[0] * v.m_floats[0] +
m_floats[1] * v.m_floats[1] + m_floats[1] * v.m_floats[1] +
m_floats[2] * v.m_floats[2]; m_floats[2] * v.m_floats[2];
#endif #endif
} }
@@ -249,7 +247,7 @@ public:
* This is symantically treating the vector like a point */ * This is symantically treating the vector like a point */
B3_FORCE_INLINE b3Scalar distance(const b3Vector3& v) const; B3_FORCE_INLINE b3Scalar distance(const b3Vector3& v) const;
B3_FORCE_INLINE b3Vector3& safeNormalize() B3_FORCE_INLINE b3Vector3& safeNormalize()
{ {
b3Vector3 absVec = this->absolute(); b3Vector3 absVec = this->absolute();
int maxIndex = absVec.maxAxis(); int maxIndex = absVec.maxAxis();
@@ -262,35 +260,35 @@ public:
return *this; return *this;
} }
/**@brief Normalize this vector /**@brief Normalize this vector
* x^2 + y^2 + z^2 = 1 */ * x^2 + y^2 + z^2 = 1 */
B3_FORCE_INLINE b3Vector3& normalize() B3_FORCE_INLINE b3Vector3& normalize()
{ {
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) #if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
// dot product first // dot product first
__m128 vd = _mm_mul_ps(mVec128, mVec128); __m128 vd = _mm_mul_ps(mVec128, mVec128);
__m128 z = _mm_movehl_ps(vd, vd); __m128 z = _mm_movehl_ps(vd, vd);
__m128 y = _mm_shuffle_ps(vd, vd, 0x55); __m128 y = _mm_shuffle_ps(vd, vd, 0x55);
vd = _mm_add_ss(vd, y); vd = _mm_add_ss(vd, y);
vd = _mm_add_ss(vd, z); vd = _mm_add_ss(vd, z);
#if 0 #if 0
vd = _mm_sqrt_ss(vd); vd = _mm_sqrt_ss(vd);
vd = _mm_div_ss(b3v1110, vd); vd = _mm_div_ss(b3v1110, vd);
vd = b3_splat_ps(vd, 0x80); vd = b3_splat_ps(vd, 0x80);
mVec128 = _mm_mul_ps(mVec128, vd); mVec128 = _mm_mul_ps(mVec128, vd);
#else #else
// NR step 1/sqrt(x) - vd is x, y is output // NR step 1/sqrt(x) - vd is x, y is output
y = _mm_rsqrt_ss(vd); // estimate y = _mm_rsqrt_ss(vd); // estimate
// one step NR // one step NR
z = b3v1_5; z = b3v1_5;
vd = _mm_mul_ss(vd, b3vHalf); // vd * 0.5 vd = _mm_mul_ss(vd, b3vHalf); // vd * 0.5
//x2 = vd; //x2 = vd;
vd = _mm_mul_ss(vd, y); // vd * 0.5 * y0 vd = _mm_mul_ss(vd, y); // vd * 0.5 * y0
vd = _mm_mul_ss(vd, y); // vd * 0.5 * y0 * y0 vd = _mm_mul_ss(vd, y); // vd * 0.5 * y0 * y0
z = _mm_sub_ss(z, vd); // 1.5 - vd * 0.5 * y0 * y0 z = _mm_sub_ss(z, vd); // 1.5 - vd * 0.5 * y0 * y0
y = _mm_mul_ss(y, z); // y0 * (1.5 - vd * 0.5 * y0 * y0) y = _mm_mul_ss(y, z); // y0 * (1.5 - vd * 0.5 * y0 * y0)
@@ -299,9 +297,9 @@ public:
#endif #endif
return *this; return *this;
#else #else
return *this /= length(); return *this /= length();
#endif #endif
} }
@@ -310,48 +308,48 @@ public:
B3_FORCE_INLINE b3Vector3 normalized() const; B3_FORCE_INLINE b3Vector3 normalized() const;
/**@brief Return a rotated version of this vector /**@brief Return a rotated version of this vector
* @param wAxis The axis to rotate about * @param wAxis The axis to rotate about
* @param angle The angle to rotate by */ * @param angle The angle to rotate by */
B3_FORCE_INLINE b3Vector3 rotate( const b3Vector3& wAxis, const b3Scalar angle ) const; B3_FORCE_INLINE b3Vector3 rotate( const b3Vector3& wAxis, const b3Scalar angle ) const;
/**@brief Return the angle between this and another vector /**@brief Return the angle between this and another vector
* @param v The other vector */ * @param v The other vector */
B3_FORCE_INLINE b3Scalar angle(const b3Vector3& v) const B3_FORCE_INLINE b3Scalar angle(const b3Vector3& v) const
{ {
b3Scalar s = b3Sqrt(length2() * v.length2()); b3Scalar s = b3Sqrt(length2() * v.length2());
b3FullAssert(s != b3Scalar(0.0)); b3FullAssert(s != b3Scalar(0.0));
return b3Acos(dot(v) / s); return b3Acos(dot(v) / s);
} }
/**@brief Return a vector will the absolute values of each element */ /**@brief Return a vector will the absolute values of each element */
B3_FORCE_INLINE b3Vector3 absolute() const B3_FORCE_INLINE b3Vector3 absolute() const
{ {
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) #if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
return b3MakeVector3(_mm_and_ps(mVec128, b3v3AbsfMask)); return b3MakeVector3(_mm_and_ps(mVec128, b3v3AbsfMask));
#elif defined(B3_USE_NEON) #elif defined(B3_USE_NEON)
return b3Vector3(vabsq_f32(mVec128)); return b3Vector3(vabsq_f32(mVec128));
#else #else
return b3MakeVector3( return b3MakeVector3(
b3Fabs(m_floats[0]), b3Fabs(m_floats[0]),
b3Fabs(m_floats[1]), b3Fabs(m_floats[1]),
b3Fabs(m_floats[2])); b3Fabs(m_floats[2]));
#endif #endif
} }
/**@brief Return the cross product between this and another vector /**@brief Return the cross product between this and another vector
* @param v The other vector */ * @param v The other vector */
B3_FORCE_INLINE b3Vector3 cross(const b3Vector3& v) const B3_FORCE_INLINE b3Vector3 cross(const b3Vector3& v) const
{ {
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) #if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
__m128 T, V; __m128 T, V;
T = b3_pshufd_ps(mVec128, B3_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0) T = b3_pshufd_ps(mVec128, B3_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0)
V = b3_pshufd_ps(v.mVec128, B3_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0) V = b3_pshufd_ps(v.mVec128, B3_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0)
V = _mm_mul_ps(V, mVec128); V = _mm_mul_ps(V, mVec128);
T = _mm_mul_ps(T, v.mVec128); T = _mm_mul_ps(T, v.mVec128);
V = _mm_sub_ps(V, T); V = _mm_sub_ps(V, T);
V = b3_pshufd_ps(V, B3_SHUFFLE(1, 2, 0, 3)); V = b3_pshufd_ps(V, B3_SHUFFLE(1, 2, 0, 3));
return b3MakeVector3(V); return b3MakeVector3(V);
#elif defined(B3_USE_NEON) #elif defined(B3_USE_NEON)
@@ -361,7 +359,7 @@ public:
float32x2_t Vlow = vget_low_f32(v.mVec128); float32x2_t Vlow = vget_low_f32(v.mVec128);
T = vcombine_f32(vext_f32(Tlow, vget_high_f32(mVec128), 1), Tlow); T = vcombine_f32(vext_f32(Tlow, vget_high_f32(mVec128), 1), Tlow);
V = vcombine_f32(vext_f32(Vlow, vget_high_f32(v.mVec128), 1), Vlow); V = vcombine_f32(vext_f32(Vlow, vget_high_f32(v.mVec128), 1), Vlow);
V = vmulq_f32(V, mVec128); V = vmulq_f32(V, mVec128);
T = vmulq_f32(T, v.mVec128); T = vmulq_f32(T, v.mVec128);
V = vsubq_f32(V, T); V = vsubq_f32(V, T);
@@ -369,7 +367,7 @@ public:
// form (Y, Z, X, _); // form (Y, Z, X, _);
V = vcombine_f32(vext_f32(Vlow, vget_high_f32(V), 1), Vlow); V = vcombine_f32(vext_f32(Vlow, vget_high_f32(V), 1), Vlow);
V = (float32x4_t)vandq_s32((int32x4_t)V, b3vFFF0Mask); V = (float32x4_t)vandq_s32((int32x4_t)V, b3vFFF0Mask);
return b3Vector3(V); return b3Vector3(V);
#else #else
return b3MakeVector3( return b3MakeVector3(
@@ -385,14 +383,14 @@ public:
// cross: // cross:
__m128 T = _mm_shuffle_ps(v1.mVec128, v1.mVec128, B3_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0) __m128 T = _mm_shuffle_ps(v1.mVec128, v1.mVec128, B3_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0)
__m128 V = _mm_shuffle_ps(v2.mVec128, v2.mVec128, B3_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0) __m128 V = _mm_shuffle_ps(v2.mVec128, v2.mVec128, B3_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0)
V = _mm_mul_ps(V, v1.mVec128); V = _mm_mul_ps(V, v1.mVec128);
T = _mm_mul_ps(T, v2.mVec128); T = _mm_mul_ps(T, v2.mVec128);
V = _mm_sub_ps(V, T); V = _mm_sub_ps(V, T);
V = _mm_shuffle_ps(V, V, B3_SHUFFLE(1, 2, 0, 3)); V = _mm_shuffle_ps(V, V, B3_SHUFFLE(1, 2, 0, 3));
// dot: // dot:
V = _mm_mul_ps(V, mVec128); V = _mm_mul_ps(V, mVec128);
__m128 z = _mm_movehl_ps(V, V); __m128 z = _mm_movehl_ps(V, V);
__m128 y = _mm_shuffle_ps(V, V, 0x55); __m128 y = _mm_shuffle_ps(V, V, 0x55);
@@ -408,7 +406,7 @@ public:
float32x2_t Vlow = vget_low_f32(v2.mVec128); float32x2_t Vlow = vget_low_f32(v2.mVec128);
T = vcombine_f32(vext_f32(Tlow, vget_high_f32(v1.mVec128), 1), Tlow); T = vcombine_f32(vext_f32(Tlow, vget_high_f32(v1.mVec128), 1), Tlow);
V = vcombine_f32(vext_f32(Vlow, vget_high_f32(v2.mVec128), 1), Vlow); V = vcombine_f32(vext_f32(Vlow, vget_high_f32(v2.mVec128), 1), Vlow);
V = vmulq_f32(V, v1.mVec128); V = vmulq_f32(V, v1.mVec128);
T = vmulq_f32(T, v2.mVec128); T = vmulq_f32(T, v2.mVec128);
V = vsubq_f32(V, T); V = vsubq_f32(V, T);
@@ -416,29 +414,29 @@ public:
// form (Y, Z, X, _); // form (Y, Z, X, _);
V = vcombine_f32(vext_f32(Vlow, vget_high_f32(V), 1), Vlow); V = vcombine_f32(vext_f32(Vlow, vget_high_f32(V), 1), Vlow);
// dot: // dot:
V = vmulq_f32(mVec128, V); V = vmulq_f32(mVec128, V);
float32x2_t x = vpadd_f32(vget_low_f32(V), vget_low_f32(V)); float32x2_t x = vpadd_f32(vget_low_f32(V), vget_low_f32(V));
x = vadd_f32(x, vget_high_f32(V)); x = vadd_f32(x, vget_high_f32(V));
return vget_lane_f32(x, 0); return vget_lane_f32(x, 0);
#else #else
return return
m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) + m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) +
m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) + m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) +
m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]); m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]);
#endif #endif
} }
/**@brief Return the axis with the smallest value /**@brief Return the axis with the smallest value
* Note return values are 0,1,2 for x, y, or z */ * Note return values are 0,1,2 for x, y, or z */
B3_FORCE_INLINE int minAxis() const B3_FORCE_INLINE int minAxis() const
{ {
return m_floats[0] < m_floats[1] ? (m_floats[0] <m_floats[2] ? 0 : 2) : (m_floats[1] <m_floats[2] ? 1 : 2); return m_floats[0] < m_floats[1] ? (m_floats[0] <m_floats[2] ? 0 : 2) : (m_floats[1] <m_floats[2] ? 1 : 2);
} }
/**@brief Return the axis with the largest value /**@brief Return the axis with the largest value
* Note return values are 0,1,2 for x, y, or z */ * Note return values are 0,1,2 for x, y, or z */
B3_FORCE_INLINE int maxAxis() const B3_FORCE_INLINE int maxAxis() const
{ {
return m_floats[0] < m_floats[1] ? (m_floats[1] <m_floats[2] ? 2 : 1) : (m_floats[0] <m_floats[2] ? 2 : 0); return m_floats[0] < m_floats[1] ? (m_floats[1] <m_floats[2] ? 2 : 1) : (m_floats[0] <m_floats[2] ? 2 : 0);
} }
@@ -448,12 +446,12 @@ public:
return absolute().minAxis(); return absolute().minAxis();
} }
B3_FORCE_INLINE int closestAxis() const B3_FORCE_INLINE int closestAxis() const
{ {
return absolute().maxAxis(); return absolute().maxAxis();
} }
B3_FORCE_INLINE void setInterpolate3(const b3Vector3& v0, const b3Vector3& v1, b3Scalar rt) B3_FORCE_INLINE void setInterpolate3(const b3Vector3& v0, const b3Vector3& v1, b3Scalar rt)
{ {
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) #if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
@@ -480,10 +478,10 @@ public:
#endif #endif
} }
/**@brief Return the linear interpolation between this and another vector /**@brief Return the linear interpolation between this and another vector
* @param v The other vector * @param v The other vector
* @param t The ration of this to v (t = 0 => return this, t=1 => return other) */ * @param t The ration of this to v (t = 0 => return this, t=1 => return other) */
B3_FORCE_INLINE b3Vector3 lerp(const b3Vector3& v, const b3Scalar& t) const B3_FORCE_INLINE b3Vector3 lerp(const b3Vector3& v, const b3Scalar& t) const
{ {
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) #if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
__m128 vt = _mm_load_ss(&t); // (t 0 0 0) __m128 vt = _mm_load_ss(&t); // (t 0 0 0)
@@ -491,23 +489,23 @@ public:
__m128 vl = _mm_sub_ps(v.mVec128, mVec128); __m128 vl = _mm_sub_ps(v.mVec128, mVec128);
vl = _mm_mul_ps(vl, vt); vl = _mm_mul_ps(vl, vt);
vl = _mm_add_ps(vl, mVec128); vl = _mm_add_ps(vl, mVec128);
return b3MakeVector3(vl); return b3MakeVector3(vl);
#elif defined(B3_USE_NEON) #elif defined(B3_USE_NEON)
float32x4_t vl = vsubq_f32(v.mVec128, mVec128); float32x4_t vl = vsubq_f32(v.mVec128, mVec128);
vl = vmulq_n_f32(vl, t); vl = vmulq_n_f32(vl, t);
vl = vaddq_f32(vl, mVec128); vl = vaddq_f32(vl, mVec128);
return b3Vector3(vl); return b3Vector3(vl);
#else #else
return return
b3MakeVector3( m_floats[0] + (v.m_floats[0] - m_floats[0]) * t, b3MakeVector3( m_floats[0] + (v.m_floats[0] - m_floats[0]) * t,
m_floats[1] + (v.m_floats[1] - m_floats[1]) * t, m_floats[1] + (v.m_floats[1] - m_floats[1]) * t,
m_floats[2] + (v.m_floats[2] - m_floats[2]) * t); m_floats[2] + (v.m_floats[2] - m_floats[2]) * t);
#endif #endif
} }
/**@brief Elementwise multiply this vector by the other /**@brief Elementwise multiply this vector by the other
* @param v The other vector */ * @param v The other vector */
B3_FORCE_INLINE b3Vector3& operator*=(const b3Vector3& v) B3_FORCE_INLINE b3Vector3& operator*=(const b3Vector3& v)
{ {
@@ -515,8 +513,8 @@ public:
mVec128 = _mm_mul_ps(mVec128, v.mVec128); mVec128 = _mm_mul_ps(mVec128, v.mVec128);
#elif defined(B3_USE_NEON) #elif defined(B3_USE_NEON)
mVec128 = vmulq_f32(mVec128, v.mVec128); mVec128 = vmulq_f32(mVec128, v.mVec128);
#else #else
m_floats[0] *= v.m_floats[0]; m_floats[0] *= v.m_floats[0];
m_floats[1] *= v.m_floats[1]; m_floats[1] *= v.m_floats[1];
m_floats[2] *= v.m_floats[2]; m_floats[2] *= v.m_floats[2];
#endif #endif
@@ -541,7 +539,7 @@ public:
/**@brief Set the w value */ /**@brief Set the w value */
B3_FORCE_INLINE void setW(b3Scalar _w) { m_floats[3] = _w;}; B3_FORCE_INLINE void setW(b3Scalar _w) { m_floats[3] = _w;};
//B3_FORCE_INLINE b3Scalar& operator[](int i) { return (&m_floats[0])[i]; } //B3_FORCE_INLINE b3Scalar& operator[](int i) { return (&m_floats[0])[i]; }
//B3_FORCE_INLINE const b3Scalar& operator[](int i) const { return (&m_floats[0])[i]; } //B3_FORCE_INLINE const b3Scalar& operator[](int i) const { return (&m_floats[0])[i]; }
///operator b3Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons. ///operator b3Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
B3_FORCE_INLINE operator b3Scalar *() { return &m_floats[0]; } B3_FORCE_INLINE operator b3Scalar *() { return &m_floats[0]; }
@@ -551,10 +549,10 @@ public:
{ {
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) #if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
return (0xf == _mm_movemask_ps((__m128)_mm_cmpeq_ps(mVec128, other.mVec128))); return (0xf == _mm_movemask_ps((__m128)_mm_cmpeq_ps(mVec128, other.mVec128)));
#else #else
return ((m_floats[3]==other.m_floats[3]) && return ((m_floats[3]==other.m_floats[3]) &&
(m_floats[2]==other.m_floats[2]) && (m_floats[2]==other.m_floats[2]) &&
(m_floats[1]==other.m_floats[1]) && (m_floats[1]==other.m_floats[1]) &&
(m_floats[0]==other.m_floats[0])); (m_floats[0]==other.m_floats[0]));
#endif #endif
} }
@@ -565,7 +563,7 @@ public:
} }
/**@brief Set each element to the max of the current values and the values of another b3Vector3 /**@brief Set each element to the max of the current values and the values of another b3Vector3
* @param other The other b3Vector3 to compare with * @param other The other b3Vector3 to compare with
*/ */
B3_FORCE_INLINE void setMax(const b3Vector3& other) B3_FORCE_INLINE void setMax(const b3Vector3& other)
{ {
@@ -582,7 +580,7 @@ public:
} }
/**@brief Set each element to the min of the current values and the values of another b3Vector3 /**@brief Set each element to the min of the current values and the values of another b3Vector3
* @param other The other b3Vector3 to compare with * @param other The other b3Vector3 to compare with
*/ */
B3_FORCE_INLINE void setMin(const b3Vector3& other) B3_FORCE_INLINE void setMin(const b3Vector3& other)
{ {
@@ -609,16 +607,16 @@ public:
void getSkewSymmetricMatrix(b3Vector3* v0,b3Vector3* v1,b3Vector3* v2) const void getSkewSymmetricMatrix(b3Vector3* v0,b3Vector3* v1,b3Vector3* v2) const
{ {
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) #if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
__m128 V = _mm_and_ps(mVec128, b3vFFF0fMask); __m128 V = _mm_and_ps(mVec128, b3vFFF0fMask);
__m128 V0 = _mm_xor_ps(b3vMzeroMask, V); __m128 V0 = _mm_xor_ps(b3vMzeroMask, V);
__m128 V2 = _mm_movelh_ps(V0, V); __m128 V2 = _mm_movelh_ps(V0, V);
__m128 V1 = _mm_shuffle_ps(V, V0, 0xCE); __m128 V1 = _mm_shuffle_ps(V, V0, 0xCE);
V0 = _mm_shuffle_ps(V0, V, 0xDB); V0 = _mm_shuffle_ps(V0, V, 0xDB);
V2 = _mm_shuffle_ps(V2, V, 0xF9); V2 = _mm_shuffle_ps(V2, V, 0xF9);
v0->mVec128 = V0; v0->mVec128 = V0;
v1->mVec128 = V1; v1->mVec128 = V1;
v2->mVec128 = V2; v2->mVec128 = V2;
@@ -634,19 +632,19 @@ public:
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) #if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
mVec128 = (__m128)_mm_xor_ps(mVec128, mVec128); mVec128 = (__m128)_mm_xor_ps(mVec128, mVec128);
#elif defined(B3_USE_NEON) #elif defined(B3_USE_NEON)
int32x4_t vi = vdupq_n_s32(0); int32x4_t vi = vdupq_n_s32(0);
mVec128 = vreinterpretq_f32_s32(vi); mVec128 = vreinterpretq_f32_s32(vi);
#else #else
setValue(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)); setValue(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
#endif #endif
} }
B3_FORCE_INLINE bool isZero() const B3_FORCE_INLINE bool isZero() const
{ {
return m_floats[0] == b3Scalar(0) && m_floats[1] == b3Scalar(0) && m_floats[2] == b3Scalar(0); return m_floats[0] == b3Scalar(0) && m_floats[1] == b3Scalar(0) && m_floats[2] == b3Scalar(0);
} }
B3_FORCE_INLINE bool fuzzyZero() const B3_FORCE_INLINE bool fuzzyZero() const
{ {
return length2() < B3_EPSILON; return length2() < B3_EPSILON;
} }
@@ -662,18 +660,18 @@ public:
B3_FORCE_INLINE void serializeDouble(struct b3Vector3DoubleData& dataOut) const; B3_FORCE_INLINE void serializeDouble(struct b3Vector3DoubleData& dataOut) const;
B3_FORCE_INLINE void deSerializeDouble(const struct b3Vector3DoubleData& dataIn); B3_FORCE_INLINE void deSerializeDouble(const struct b3Vector3DoubleData& dataIn);
/**@brief returns index of maximum dot product between this and vectors in array[] /**@brief returns index of maximum dot product between this and vectors in array[]
* @param array The other vectors * @param array The other vectors
* @param array_count The number of other vectors * @param array_count The number of other vectors
* @param dotOut The maximum dot product */ * @param dotOut The maximum dot product */
B3_FORCE_INLINE long maxDot( const b3Vector3 *array, long array_count, b3Scalar &dotOut ) const; B3_FORCE_INLINE long maxDot( const b3Vector3 *array, long array_count, b3Scalar &dotOut ) const;
/**@brief returns index of minimum dot product between this and vectors in array[] /**@brief returns index of minimum dot product between this and vectors in array[]
* @param array The other vectors * @param array The other vectors
* @param array_count The number of other vectors * @param array_count The number of other vectors
* @param dotOut The minimum dot product */ * @param dotOut The minimum dot product */
B3_FORCE_INLINE long minDot( const b3Vector3 *array, long array_count, b3Scalar &dotOut ) const; B3_FORCE_INLINE long minDot( const b3Vector3 *array, long array_count, b3Scalar &dotOut ) const;
/* create a vector as b3Vector3( this->dot( b3Vector3 v0 ), this->dot( b3Vector3 v1), this->dot( b3Vector3 v2 )) */ /* create a vector as b3Vector3( this->dot( b3Vector3 v0 ), this->dot( b3Vector3 v1), this->dot( b3Vector3 v2 )) */
B3_FORCE_INLINE b3Vector3 dot3( const b3Vector3 &v0, const b3Vector3 &v1, const b3Vector3 &v2 ) const B3_FORCE_INLINE b3Vector3 dot3( const b3Vector3 &v0, const b3Vector3 &v1, const b3Vector3 &v2 ) const
@@ -691,7 +689,7 @@ public:
a2 = _mm_and_ps( a2, b3vxyzMaskf); a2 = _mm_and_ps( a2, b3vxyzMaskf);
r = _mm_add_ps( r, b3CastdTo128f (_mm_move_sd( b3CastfTo128d(a2), b3CastfTo128d(b1) ))); r = _mm_add_ps( r, b3CastdTo128f (_mm_move_sd( b3CastfTo128d(a2), b3CastfTo128d(b1) )));
return b3MakeVector3(r); return b3MakeVector3(r);
#elif defined(B3_USE_NEON) #elif defined(B3_USE_NEON)
static const uint32x4_t xyzMask = (const uint32x4_t){ -1, -1, -1, 0 }; static const uint32x4_t xyzMask = (const uint32x4_t){ -1, -1, -1, 0 };
float32x4_t a0 = vmulq_f32( v0.mVec128, this->mVec128); float32x4_t a0 = vmulq_f32( v0.mVec128, this->mVec128);
@@ -702,15 +700,15 @@ public:
float32x2_t b0 = vadd_f32( vpadd_f32( vget_low_f32(a0), vget_low_f32(a1)), zLo.val[0] ); float32x2_t b0 = vadd_f32( vpadd_f32( vget_low_f32(a0), vget_low_f32(a1)), zLo.val[0] );
float32x2_t b1 = vpadd_f32( vpadd_f32( vget_low_f32(a2), vget_high_f32(a2)), vdup_n_f32(0.0f)); float32x2_t b1 = vpadd_f32( vpadd_f32( vget_low_f32(a2), vget_high_f32(a2)), vdup_n_f32(0.0f));
return b3Vector3( vcombine_f32(b0, b1) ); return b3Vector3( vcombine_f32(b0, b1) );
#else #else
return b3MakeVector3( dot(v0), dot(v1), dot(v2)); return b3MakeVector3( dot(v0), dot(v1), dot(v2));
#endif #endif
} }
}; };
/**@brief Return the sum of two vectors (Point symantics)*/ /**@brief Return the sum of two vectors (Point symantics)*/
B3_FORCE_INLINE b3Vector3 B3_FORCE_INLINE b3Vector3
operator+(const b3Vector3& v1, const b3Vector3& v2) operator+(const b3Vector3& v1, const b3Vector3& v2)
{ {
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) #if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
return b3MakeVector3(_mm_add_ps(v1.mVec128, v2.mVec128)); return b3MakeVector3(_mm_add_ps(v1.mVec128, v2.mVec128));
@@ -718,15 +716,15 @@ operator+(const b3Vector3& v1, const b3Vector3& v2)
return b3MakeVector3(vaddq_f32(v1.mVec128, v2.mVec128)); return b3MakeVector3(vaddq_f32(v1.mVec128, v2.mVec128));
#else #else
return b3MakeVector3( return b3MakeVector3(
v1.m_floats[0] + v2.m_floats[0], v1.m_floats[0] + v2.m_floats[0],
v1.m_floats[1] + v2.m_floats[1], v1.m_floats[1] + v2.m_floats[1],
v1.m_floats[2] + v2.m_floats[2]); v1.m_floats[2] + v2.m_floats[2]);
#endif #endif
} }
/**@brief Return the elementwise product of two vectors */ /**@brief Return the elementwise product of two vectors */
B3_FORCE_INLINE b3Vector3 B3_FORCE_INLINE b3Vector3
operator*(const b3Vector3& v1, const b3Vector3& v2) operator*(const b3Vector3& v1, const b3Vector3& v2)
{ {
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) #if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
return b3MakeVector3(_mm_mul_ps(v1.mVec128, v2.mVec128)); return b3MakeVector3(_mm_mul_ps(v1.mVec128, v2.mVec128));
@@ -734,14 +732,14 @@ operator*(const b3Vector3& v1, const b3Vector3& v2)
return b3MakeVector3(vmulq_f32(v1.mVec128, v2.mVec128)); return b3MakeVector3(vmulq_f32(v1.mVec128, v2.mVec128));
#else #else
return b3MakeVector3( return b3MakeVector3(
v1.m_floats[0] * v2.m_floats[0], v1.m_floats[0] * v2.m_floats[0],
v1.m_floats[1] * v2.m_floats[1], v1.m_floats[1] * v2.m_floats[1],
v1.m_floats[2] * v2.m_floats[2]); v1.m_floats[2] * v2.m_floats[2]);
#endif #endif
} }
/**@brief Return the difference between two vectors */ /**@brief Return the difference between two vectors */
B3_FORCE_INLINE b3Vector3 B3_FORCE_INLINE b3Vector3
operator-(const b3Vector3& v1, const b3Vector3& v2) operator-(const b3Vector3& v1, const b3Vector3& v2)
{ {
#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) #if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))
@@ -754,28 +752,28 @@ operator-(const b3Vector3& v1, const b3Vector3& v2)
return b3MakeVector3((float32x4_t)vandq_s32((int32x4_t)r, b3vFFF0Mask)); return b3MakeVector3((float32x4_t)vandq_s32((int32x4_t)r, b3vFFF0Mask));
#else #else
return b3MakeVector3( return b3MakeVector3(
v1.m_floats[0] - v2.m_floats[0], v1.m_floats[0] - v2.m_floats[0],
v1.m_floats[1] - v2.m_floats[1], v1.m_floats[1] - v2.m_floats[1],
v1.m_floats[2] - v2.m_floats[2]); v1.m_floats[2] - v2.m_floats[2]);
#endif #endif
} }
/**@brief Return the negative of the vector */ /**@brief Return the negative of the vector */
B3_FORCE_INLINE b3Vector3 B3_FORCE_INLINE b3Vector3
operator-(const b3Vector3& v) operator-(const b3Vector3& v)
{ {
#if (defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)) #if (defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE))
__m128 r = _mm_xor_ps(v.mVec128, b3vMzeroMask); __m128 r = _mm_xor_ps(v.mVec128, b3vMzeroMask);
return b3MakeVector3(_mm_and_ps(r, b3vFFF0fMask)); return b3MakeVector3(_mm_and_ps(r, b3vFFF0fMask));
#elif defined(B3_USE_NEON) #elif defined(B3_USE_NEON)
return b3MakeVector3((b3SimdFloat4)veorq_s32((int32x4_t)v.mVec128, (int32x4_t)b3vMzeroMask)); return b3MakeVector3((b3SimdFloat4)veorq_s32((int32x4_t)v.mVec128, (int32x4_t)b3vMzeroMask));
#else #else
return b3MakeVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]); return b3MakeVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]);
#endif #endif
} }
/**@brief Return the vector scaled by s */ /**@brief Return the vector scaled by s */
B3_FORCE_INLINE b3Vector3 B3_FORCE_INLINE b3Vector3
operator*(const b3Vector3& v, const b3Scalar& s) operator*(const b3Vector3& v, const b3Scalar& s)
{ {
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) #if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
@@ -791,10 +789,10 @@ operator*(const b3Vector3& v, const b3Scalar& s)
} }
/**@brief Return the vector scaled by s */ /**@brief Return the vector scaled by s */
B3_FORCE_INLINE b3Vector3 B3_FORCE_INLINE b3Vector3
operator*(const b3Scalar& s, const b3Vector3& v) operator*(const b3Scalar& s, const b3Vector3& v)
{ {
return v * s; return v * s;
} }
/**@brief Return the vector inversely scaled by s */ /**@brief Return the vector inversely scaled by s */
@@ -821,13 +819,13 @@ operator/(const b3Vector3& v1, const b3Vector3& v2)
#if (defined(B3_USE_SSE_IN_API)&& defined (B3_USE_SSE)) #if (defined(B3_USE_SSE_IN_API)&& defined (B3_USE_SSE))
__m128 vec = _mm_div_ps(v1.mVec128, v2.mVec128); __m128 vec = _mm_div_ps(v1.mVec128, v2.mVec128);
vec = _mm_and_ps(vec, b3vFFF0fMask); vec = _mm_and_ps(vec, b3vFFF0fMask);
return b3MakeVector3(vec); return b3MakeVector3(vec);
#elif defined(B3_USE_NEON) #elif defined(B3_USE_NEON)
float32x4_t x, y, v, m; float32x4_t x, y, v, m;
x = v1.mVec128; x = v1.mVec128;
y = v2.mVec128; y = v2.mVec128;
v = vrecpeq_f32(y); // v ~ 1/y v = vrecpeq_f32(y); // v ~ 1/y
m = vrecpsq_f32(y, v); // m = (2-v*y) m = vrecpsq_f32(y, v); // m = (2-v*y)
v = vmulq_f32(v, m); // vv = v*m ~~ 1/y v = vmulq_f32(v, m); // vv = v*m ~~ 1/y
@@ -838,47 +836,47 @@ operator/(const b3Vector3& v1, const b3Vector3& v2)
return b3Vector3(v); return b3Vector3(v);
#else #else
return b3MakeVector3( return b3MakeVector3(
v1.m_floats[0] / v2.m_floats[0], v1.m_floats[0] / v2.m_floats[0],
v1.m_floats[1] / v2.m_floats[1], v1.m_floats[1] / v2.m_floats[1],
v1.m_floats[2] / v2.m_floats[2]); v1.m_floats[2] / v2.m_floats[2]);
#endif #endif
} }
/**@brief Return the dot product between two vectors */ /**@brief Return the dot product between two vectors */
B3_FORCE_INLINE b3Scalar B3_FORCE_INLINE b3Scalar
b3Dot(const b3Vector3& v1, const b3Vector3& v2) b3Dot(const b3Vector3& v1, const b3Vector3& v2)
{ {
return v1.dot(v2); return v1.dot(v2);
} }
/**@brief Return the distance squared between two vectors */ /**@brief Return the distance squared between two vectors */
B3_FORCE_INLINE b3Scalar B3_FORCE_INLINE b3Scalar
b3Distance2(const b3Vector3& v1, const b3Vector3& v2) b3Distance2(const b3Vector3& v1, const b3Vector3& v2)
{ {
return v1.distance2(v2); return v1.distance2(v2);
} }
/**@brief Return the distance between two vectors */ /**@brief Return the distance between two vectors */
B3_FORCE_INLINE b3Scalar B3_FORCE_INLINE b3Scalar
b3Distance(const b3Vector3& v1, const b3Vector3& v2) b3Distance(const b3Vector3& v1, const b3Vector3& v2)
{ {
return v1.distance(v2); return v1.distance(v2);
} }
/**@brief Return the angle between two vectors */ /**@brief Return the angle between two vectors */
B3_FORCE_INLINE b3Scalar B3_FORCE_INLINE b3Scalar
b3Angle(const b3Vector3& v1, const b3Vector3& v2) b3Angle(const b3Vector3& v1, const b3Vector3& v2)
{ {
return v1.angle(v2); return v1.angle(v2);
} }
/**@brief Return the cross product of two vectors */ /**@brief Return the cross product of two vectors */
B3_FORCE_INLINE b3Vector3 B3_FORCE_INLINE b3Vector3
b3Cross(const b3Vector3& v1, const b3Vector3& v2) b3Cross(const b3Vector3& v1, const b3Vector3& v2)
{ {
return v1.cross(v2); return v1.cross(v2);
} }
B3_FORCE_INLINE b3Scalar B3_FORCE_INLINE b3Scalar
@@ -888,10 +886,10 @@ b3Triple(const b3Vector3& v1, const b3Vector3& v2, const b3Vector3& v3)
} }
/**@brief Return the linear interpolation between two vectors /**@brief Return the linear interpolation between two vectors
* @param v1 One vector * @param v1 One vector
* @param v2 The other vector * @param v2 The other vector
* @param t The ration of this to v (t = 0 => return v1, t=1 => return v2) */ * @param t The ration of this to v (t = 0 => return v1, t=1 => return v2) */
B3_FORCE_INLINE b3Vector3 B3_FORCE_INLINE b3Vector3
b3Lerp(const b3Vector3& v1, const b3Vector3& v2, const b3Scalar& t) b3Lerp(const b3Vector3& v1, const b3Vector3& v2, const b3Scalar& t)
{ {
return v1.lerp(v2, t); return v1.lerp(v2, t);
@@ -918,7 +916,7 @@ B3_FORCE_INLINE b3Vector3 b3Vector3::normalized() const
#else #else
return *this / length(); return *this / length();
#endif #endif
} }
B3_FORCE_INLINE b3Vector3 b3Vector3::rotate( const b3Vector3& wAxis, const b3Scalar _angle ) const B3_FORCE_INLINE b3Vector3 b3Vector3::rotate( const b3Vector3& wAxis, const b3Scalar _angle ) const
{ {
@@ -931,25 +929,25 @@ B3_FORCE_INLINE b3Vector3 b3Vector3::rotate( const b3Vector3& wAxis, const b3Sca
__m128 C = wAxis.cross( b3MakeVector3(mVec128) ).mVec128; __m128 C = wAxis.cross( b3MakeVector3(mVec128) ).mVec128;
O = _mm_and_ps(O, b3vFFF0fMask); O = _mm_and_ps(O, b3vFFF0fMask);
b3Scalar scos = b3Cos( _angle ); b3Scalar scos = b3Cos( _angle );
__m128 vsin = _mm_load_ss(&ssin); // (S 0 0 0) __m128 vsin = _mm_load_ss(&ssin); // (S 0 0 0)
__m128 vcos = _mm_load_ss(&scos); // (S 0 0 0) __m128 vcos = _mm_load_ss(&scos); // (S 0 0 0)
__m128 Y = b3_pshufd_ps(O, 0xC9); // (Y Z X 0) __m128 Y = b3_pshufd_ps(O, 0xC9); // (Y Z X 0)
__m128 Z = b3_pshufd_ps(O, 0xD2); // (Z X Y 0) __m128 Z = b3_pshufd_ps(O, 0xD2); // (Z X Y 0)
O = _mm_add_ps(O, Y); O = _mm_add_ps(O, Y);
vsin = b3_pshufd_ps(vsin, 0x80); // (S S S 0) vsin = b3_pshufd_ps(vsin, 0x80); // (S S S 0)
O = _mm_add_ps(O, Z); O = _mm_add_ps(O, Z);
vcos = b3_pshufd_ps(vcos, 0x80); // (S S S 0) vcos = b3_pshufd_ps(vcos, 0x80); // (S S S 0)
vsin = vsin * C; vsin = vsin * C;
O = O * wAxis.mVec128; O = O * wAxis.mVec128;
__m128 X = mVec128 - O; __m128 X = mVec128 - O;
O = O + vsin; O = O + vsin;
vcos = vcos * X; vcos = vcos * X;
O = O + vcos; O = O + vcos;
return b3MakeVector3(O); return b3MakeVector3(O);
#else #else
b3Vector3 o = wAxis * wAxis.dot( *this ); b3Vector3 o = wAxis * wAxis.dot( *this );
@@ -974,7 +972,7 @@ B3_FORCE_INLINE long b3Vector3::maxDot( const b3Vector3 *array, long array_
#endif #endif
if( array_count < scalar_cutoff ) if( array_count < scalar_cutoff )
#else #else
#endif//B3_USE_SSE || B3_USE_NEON #endif//B3_USE_SSE || B3_USE_NEON
{ {
b3Scalar maxDot = -B3_INFINITY; b3Scalar maxDot = -B3_INFINITY;
@@ -983,7 +981,7 @@ B3_FORCE_INLINE long b3Vector3::maxDot( const b3Vector3 *array, long array_
for( i = 0; i < array_count; i++ ) for( i = 0; i < array_count; i++ )
{ {
b3Scalar dot = array[i].dot(*this); b3Scalar dot = array[i].dot(*this);
if( dot > maxDot ) if( dot > maxDot )
{ {
maxDot = dot; maxDot = dot;
@@ -1016,27 +1014,27 @@ B3_FORCE_INLINE long b3Vector3::minDot( const b3Vector3 *array, long array_
#else #else
#error unhandled arch! #error unhandled arch!
#endif #endif
if( array_count < scalar_cutoff ) if( array_count < scalar_cutoff )
#endif//B3_USE_SSE || B3_USE_NEON #endif//B3_USE_SSE || B3_USE_NEON
{ {
b3Scalar minDot = B3_INFINITY; b3Scalar minDot = B3_INFINITY;
int i = 0; int i = 0;
int ptIndex = -1; int ptIndex = -1;
for( i = 0; i < array_count; i++ ) for( i = 0; i < array_count; i++ )
{ {
b3Scalar dot = array[i].dot(*this); b3Scalar dot = array[i].dot(*this);
if( dot < minDot ) if( dot < minDot )
{ {
minDot = dot; minDot = dot;
ptIndex = i; ptIndex = i;
} }
} }
dotOut = minDot; dotOut = minDot;
return ptIndex; return ptIndex;
} }
#if defined (B3_USE_SSE) || defined (B3_USE_NEON) #if defined (B3_USE_SSE) || defined (B3_USE_NEON)
@@ -1049,21 +1047,21 @@ class b3Vector4 : public b3Vector3
{ {
public: public:
B3_FORCE_INLINE b3Vector4 absolute4() const
B3_FORCE_INLINE b3Vector4 absolute4() const
{ {
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) #if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
return b3MakeVector4(_mm_and_ps(mVec128, b3vAbsfMask)); return b3MakeVector4(_mm_and_ps(mVec128, b3vAbsfMask));
#elif defined(B3_USE_NEON) #elif defined(B3_USE_NEON)
return b3Vector4(vabsq_f32(mVec128)); return b3Vector4(vabsq_f32(mVec128));
#else #else
return b3MakeVector4( return b3MakeVector4(
b3Fabs(m_floats[0]), b3Fabs(m_floats[0]),
b3Fabs(m_floats[1]), b3Fabs(m_floats[1]),
b3Fabs(m_floats[2]), b3Fabs(m_floats[2]),
b3Fabs(m_floats[3])); b3Fabs(m_floats[3]));
#endif #endif
@@ -1126,34 +1124,34 @@ public:
minIndex = 3; minIndex = 3;
minVal = m_floats[3]; minVal = m_floats[3];
} }
return minIndex; return minIndex;
} }
B3_FORCE_INLINE int closestAxis4() const B3_FORCE_INLINE int closestAxis4() const
{ {
return absolute4().maxAxis4(); return absolute4().maxAxis4();
} }
/**@brief Set x,y,z and zero w
/**@brief Set x,y,z and zero w
* @param x Value of x * @param x Value of x
* @param y Value of y * @param y Value of y
* @param z Value of z * @param z Value of z
*/ */
/* void getValue(b3Scalar *m) const
/* void getValue(b3Scalar *m) const
{ {
m[0] = m_floats[0]; m[0] = m_floats[0];
m[1] = m_floats[1]; m[1] = m_floats[1];
m[2] =m_floats[2]; m[2] =m_floats[2];
} }
*/ */
/**@brief Set the values /**@brief Set the values
* @param x Value of x * @param x Value of x
* @param y Value of y * @param y Value of y
* @param z Value of z * @param z Value of z
@@ -1299,7 +1297,7 @@ B3_FORCE_INLINE void b3Vector3::deSerialize(const struct b3Vector3Data& dataIn)
} }
inline b3Vector3 b3MakeVector3(b3Scalar x,b3Scalar y,b3Scalar z) inline b3Vector3 b3MakeVector3(b3Scalar x,b3Scalar y,b3Scalar z)
{ {
@@ -1309,7 +1307,7 @@ inline b3Vector3 b3MakeVector3(b3Scalar x,b3Scalar y,b3Scalar z)
} }
inline b3Vector3 b3MakeVector3(b3Scalar x,b3Scalar y,b3Scalar z, b3Scalar w) inline b3Vector3 b3MakeVector3(b3Scalar x,b3Scalar y,b3Scalar z, b3Scalar w)
{ {
b3Vector3 tmp; b3Vector3 tmp;
tmp.setValue(x,y,z); tmp.setValue(x,y,z);
tmp.w = w; tmp.w = w;
@@ -1323,7 +1321,7 @@ inline b3Vector4 b3MakeVector4(b3Scalar x,b3Scalar y,b3Scalar z,b3Scalar w)
return tmp; return tmp;
} }
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) #if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
inline b3Vector3 b3MakeVector3( b3SimdFloat4 v) inline b3Vector3 b3MakeVector3( b3SimdFloat4 v)
{ {

16
src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
View File

@@ -59,15 +59,13 @@ PHY_ScalarType hdt, bool flipQuadEdges
) )
{ {
// validation // validation
btAssert(heightStickWidth > 1 && "bad width"); btAssert(heightStickWidth > 1);// && "bad width");
btAssert(heightStickLength > 1 && "bad length"); btAssert(heightStickLength > 1);// && "bad length");
btAssert(heightfieldData && "null heightfield data"); btAssert(heightfieldData);// && "null heightfield data");
// btAssert(heightScale) -- do we care? Trust caller here // btAssert(heightScale) -- do we care? Trust caller here
btAssert(minHeight <= maxHeight && "bad min/max height"); btAssert(minHeight <= maxHeight);// && "bad min/max height");
btAssert(upAxis >= 0 && upAxis < 3 && btAssert(upAxis >= 0 && upAxis < 3);// && "bad upAxis--should be in range [0,2]");
"bad upAxis--should be in range [0,2]"); btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT);// && "Bad height data type enum");
btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT &&
"Bad height data type enum");
// initialize member variables // initialize member variables
m_shapeType = TERRAIN_SHAPE_PROXYTYPE; m_shapeType = TERRAIN_SHAPE_PROXYTYPE;
@@ -110,7 +108,7 @@ PHY_ScalarType hdt, bool flipQuadEdges
default: default:
{ {
//need to get valid m_upAxis //need to get valid m_upAxis
btAssert(0 && "Bad m_upAxis"); btAssert(0);// && "Bad m_upAxis");
} }
} }

2
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h
View File

@@ -19,7 +19,7 @@ subject to the following restrictions:
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
#include "btMultiBodySolverConstraint.h" #include "btMultiBodySolverConstraint.h"
//#define DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS #define DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
class btMultiBody; class btMultiBody;