fix some warnings, disable gimpact by default in world importer,
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:
@@ -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);
|
||||||
|
|
||||||
|
|||||||
@@ -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
|
||||||
|
|||||||
@@ -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()
|
||||||
|
|||||||
@@ -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);
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -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;
|
||||||
|
|
||||||
|
|||||||
@@ -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
|
||||||
|
|||||||
@@ -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);
|
||||||
|
|||||||
@@ -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);
|
||||||
|
|||||||
@@ -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",
|
||||||
|
|||||||
@@ -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;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -231,7 +231,7 @@ void ImportUrdfSetup::initPhysics()
|
|||||||
|
|
||||||
if (loadOk)
|
if (loadOk)
|
||||||
{
|
{
|
||||||
printTree(u2b,u2b.getRootLinkIndex());
|
//printTree(u2b,u2b.getRootLinkIndex());
|
||||||
|
|
||||||
//u2b.printTree();
|
//u2b.printTree();
|
||||||
|
|
||||||
|
|||||||
@@ -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;
|
||||||
|
|||||||
@@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -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,
|
||||||
|
|||||||
@@ -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);
|
||||||
|
|||||||
@@ -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);
|
||||||
|
|
||||||
|
|||||||
@@ -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])
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -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();
|
||||||
|
|||||||
@@ -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)},
|
||||||
|
|||||||
@@ -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;
|
||||||
//
|
//
|
||||||
@@ -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();
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -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);
|
||||||
|
|
||||||
|
|||||||
@@ -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
|
||||||
|
|
||||||
|
|||||||
@@ -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]);
|
||||||
|
|||||||
@@ -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"
|
||||||
|
|||||||
@@ -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");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -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;
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user