virtual void processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId)
{
if (islandId<0)
{
///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
} else
{
//also add all non-contact constraints/joints for this island
btTypedConstraint** startConstraint = 0;
int numCurConstraints = 0;
int i;
//find the first constraint for this island
for (i=0;i<m_numConstraints;i++)
{
if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
{
startConstraint = &m_sortedConstraints[i];
break;
}
}
//count the number of constraints in this island
for (;i<m_numConstraints;i++)
{
if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
{
numCurConstraints++;
}
}
if (m_solverInfo->m_minimumSolverBatchSize<=1)
{
m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
} else
{
for (i=0;i<numBodies;i++)
m_bodies.push_back(bodies[i]);
for (i=0;i<numManifolds;i++)
m_manifolds.push_back(manifolds[i]);
for (i=0;i<numCurConstraints;i++)
m_constraints.push_back(startConstraint[i]);
if ((m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize)
{
processConstraints();
} else
{
//printf("deferred\n");
}
}
}
}
void PhysicsClientExample::enqueueCommand(const SharedMemoryCommand& orgCommand)
{
m_userCommandRequests.push_back(orgCommand);
SharedMemoryCommand& cmd = m_userCommandRequests[m_userCommandRequests.size()-1];
//b3Printf("User put command request %d on queue (queue length = %d)\n",cmd.m_type, m_userCommandRequests.size());
}
void OpenGLExampleBrowser::registerFileImporter(const char* extension, CommonExampleInterface::CreateFunc* createFunc)
{
FileImporterByExtension fi;
fi.m_extension = extension;
fi.m_createFunc = createFunc;
gFileImporterByExtension.push_back(fi);
}
void CreateRigidBody(XYZ position, XYZ rotation, XYZ size)
{
//create a dynamic rigidbody
btCollisionShape* colShape = new btBoxShape(btVector3(size.x / 2.0, size.y / 2.0, size.z / 2.0));
//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
collisionShapes.push_back(colShape);
/// Create Dynamic Objects
btTransform startTransform;
startTransform.setIdentity();
btScalar mass(1.f);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
colShape->calculateLocalInertia(mass,localInertia);
startTransform.setOrigin(btVector3(position.x, position.y, position.z));
btQuaternion rot;
rot.setEuler(rotation.x,rotation.y,rotation.z);
startTransform.setRotation(rot);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
boxBody = new btRigidBody(rbInfo);
dynamicsWorld->addRigidBody(boxBody);
}
void buildBoneTree(akSkeleton* skel, btAlignedObjectArray<akMatrix4>& bind, Blender::Bone* bone, UTuint8 parent)
{
//if(!(bone->flag & BONE_NO_DEFORM) || parent == AK_JOINT_NO_PARENT)
{
utHashedString jname = bone->name;
float* bmat = (float*)bone->arm_mat;
akMatrix4 mat(akVector4(bmat[4*0+0], bmat[4*0+1], bmat[4*0+2], bmat[4*0+3]),
akVector4(bmat[4*1+0], bmat[4*1+1], bmat[4*1+2], bmat[4*1+3]),
akVector4(bmat[4*2+0], bmat[4*2+1], bmat[4*2+2], bmat[4*2+3]),
akVector4(bmat[4*3+0], bmat[4*3+1], bmat[4*3+2], bmat[4*3+3]));
bind.push_back(mat);
parent = skel->addJoint(jname, parent);
if(bone->flag & BONE_NO_SCALE)
skel->getJoint(parent)->m_inheritScale = false;
}
Blender::Bone* chi = static_cast<Blender::Bone*>(bone->childbase.first);
while (chi)
{
// recurse
buildBoneTree(skel, bind, chi, parent);
chi = chi->next;
}
}
btRigidBody* localCreateRigidBody(float mass, const btTransform& startTransform,btCollisionShape* shape)
{
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic) shape->calculateLocalInertia(mass,localInertia);
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
btRigidBody::btRigidBodyConstructionInfo cInfo(mass,myMotionState,shape,localInertia);
box_body.push_back(new btRigidBody(cInfo));
box_body[box_body.size()-1]->setContactProcessingThreshold(m_defaultContactProcessingThreshold);
m_dynamicsWorld->addRigidBody(box_body[box_body.size()-1]);
return box_body[box_body.size()-1];
/*
btRigidBody* body = new btRigidBody(cInfo);
body->setContactProcessingThreshold(m_defaultContactProcessingThreshold);
m_dynamicsWorld->addRigidBody(body);
return body;
*/
}
void submitJob( IJob* job )
{
m_jobQueue.push_back( job );
lockQueue();
m_tailIndex++;
m_queueIsEmpty = false;
unlockQueue();
}
/* -----------------------------------------------------------------------
| build bullet box shape
|
| : default create 125 (5x5x5) dynamic object
----------------------------------------------------------------------- */
bool
buildBoxShapeArray(Ogre::SceneManager* sceneMgr, btDynamicsWorld* dynamicsWorld, btAlignedObjectArray<btCollisionShape*>& collisionShapes,
const btVector3& array_size, btScalar scale)
{
btTransform startTransform;
startTransform.setIdentity();
btScalar mass(1.f);
btVector3 localInertia(0,0,0);
btBoxShape* colShape = new btBoxShape(btVector3(scale, scale, scale));
btAssert(colShape);
colShape->calculateLocalInertia(mass,localInertia);
collisionShapes.push_back(colShape);
float start_x = - array_size.getX()/2;
float start_y = array_size.getY();
float start_z = - array_size.getZ()/2;
int index = 0;
for (int k=0;k<array_size.getY();k++)
{
for (int i=0;i<array_size.getX();i++)
{
for(int j=0;j<array_size.getZ();j++)
{
startTransform.setOrigin(scale * btVector3(
btScalar(2.0*i + start_x),
btScalar(20+2.0*k + start_y),
btScalar(2.0*j + start_z)));
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
body->setContactProcessingThreshold(BT_LARGE_FLOAT);
if (sceneMgr)
{
Ogre::Entity* ent = sceneMgr->createEntity("ent_" + Ogre::StringConverter::toString(index++),"Barrel.mesh");
Ogre::SceneNode* node = sceneMgr->getRootSceneNode()->createChildSceneNode("node_box_" + Ogre::StringConverter::toString(index++));
node->attachObject(ent);
node->setPosition(startTransform.getOrigin().getX(), startTransform.getOrigin().getY(), startTransform.getOrigin().getZ());
const Ogre::AxisAlignedBox& aabb = ent->getBoundingBox();
const Ogre::Vector3& boxScale = (aabb.getMaximum() - aabb.getMinimum())/2.0f;
node->scale(scale/boxScale.x, scale/boxScale.y, scale/boxScale.z);
body->setUserPointer((void*)node);
}
dynamicsWorld->addRigidBody(body);
}
}
}
return true;
}
void enqueueCommand(const SharedMemoryCommand& orgCommand)
{
m_userCommandRequests.push_back(orgCommand);
SharedMemoryCommand& cmd = m_userCommandRequests[m_userCommandRequests.size()-1];
cmd.m_sequenceNumber = m_sequenceNumberGenerator++;
cmd.m_timeStamp = m_realtimeClock.getTimeMicroseconds();
b3Printf("User put command request %d on queue (queue length = %d)\n",cmd.m_type, m_userCommandRequests.size());
}
void btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray<btVector3>& planeEquations , btAlignedObjectArray<btVector3>& verticesOut )
{
const int numbrushes = planeEquations.size();
// brute force:
for (int i=0;i<numbrushes;i++)
{
const btVector3& N1 = planeEquations[i];
for (int j=i+1;j<numbrushes;j++)
{
const btVector3& N2 = planeEquations[j];
for (int k=j+1;k<numbrushes;k++)
{
const btVector3& N3 = planeEquations[k];
btVector3 n2n3; n2n3 = N2.cross(N3);
btVector3 n3n1; n3n1 = N3.cross(N1);
btVector3 n1n2; n1n2 = N1.cross(N2);
if ( ( n2n3.length2() > btScalar(0.0001) ) &&
( n3n1.length2() > btScalar(0.0001) ) &&
( n1n2.length2() > btScalar(0.0001) ) )
{
//point P out of 3 plane equations:
// d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 )
//P = -------------------------------------------------------------------------
// N1 . ( N2 * N3 )
btScalar quotient = (N1.dot(n2n3));
if (btFabs(quotient) > btScalar(0.000001))
{
quotient = btScalar(-1.) / quotient;
n2n3 *= N1[3];
n3n1 *= N2[3];
n1n2 *= N3[3];
btVector3 potentialVertex = n2n3;
potentialVertex += n3n1;
potentialVertex += n1n2;
potentialVertex *= quotient;
//check if inside, and replace supportingVertexOut if needed
if (isPointInsidePlanes(planeEquations,potentialVertex,btScalar(0.01)))
{
verticesOut.push_back(potentialVertex);
}
}
}
}
}
}
}
/* -----------------------------------------------------------------------
| the function describe build btRigidBody from Ogre mesh :
|
| @prama in : Ogre entity (use as a single object)
| @pamra out : return true if build success and add collision shape to dynamicsworld
----------------------------------------------------------------------- */
bool
buildRigidBodyFromOgreEntity(Ogre::Entity* ent,
btDynamicsWorld* dynamicsWorld,
btAlignedObjectArray<btCollisionShape*>& collisionShapes,
void* &data)
{
void *vertices, *indices;
size_t vertexCount = 0, indexCount = 0;
getVertexBuffer(ent, vertices, vertexCount, indices, indexCount);
btScalar mass(1.0f);
btVector3 localInertia(0,0,0);
data = new btTriangleMesh();
btTriangleMesh* trimesh = static_cast<btTriangleMesh*>(data);
btAssert(trimesh);
Ogre::Vector3* vert = (Ogre::Vector3*)vertices;
Ogre::ulong* index = (Ogre::ulong*)indices;
for (size_t i=0 ; i<vertexCount ; i++)
{
int index0 = index[i*3];
int index1 = index[i*3+1];
int index2 = index[i*3+2];
btVector3 vertex0(vert[index0].x, vert[index0].y, vert[index0].z);
btVector3 vertex1(vert[index1].x, vert[index1].y, vert[index1].z);
btVector3 vertex2(vert[index2].x, vert[index2].y, vert[index2].z);
trimesh->addTriangle(vertex0,vertex1,vertex2);
}
delete [] vertices;
delete [] indices;
btCollisionShape* colShape = new btConvexTriangleMeshShape(trimesh);
const btVector3& scale = colShape->getLocalScaling();
colShape->calculateLocalInertia(mass,localInertia);
collisionShapes.push_back(colShape);
btTransform trans;
trans.setIdentity();
btDefaultMotionState* motionState = new btDefaultMotionState(trans);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,motionState,colShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
dynamicsWorld->addRigidBody(body);
// link RigidBody and SceneNode
Ogre::SceneNode* node = ent->getParentSceneNode();
body->setUserPointer((void*)node);
return true;
}
void ROSURDFImporter::getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const
{
childLinkIndices.resize(0);
int numChildren = m_data->m_links[linkIndex]->child_links.size();
for (int i=0;i<numChildren;i++)
{
int childIndex =m_data->m_links[linkIndex]->child_links[i]->m_link_index;
childLinkIndices.push_back(childIndex);
}
}
virtual void drawLine(const btVector3& from1,const btVector3& to1,const btVector3& color1)
{
//float from[4] = {from1[0],from1[1],from1[2],from1[3]};
//float to[4] = {to1[0],to1[1],to1[2],to1[3]};
//float color[4] = {color1[0],color1[1],color1[2],color1[3]};
//m_glApp->m_instancingRenderer->drawLine(from,to,color);
if (m_currentLineColor!=color1 || m_linePoints.size() >= BT_LINE_BATCH_SIZE)
{
flushLines();
m_currentLineColor = color1;
}
MyDebugVec3 from(from1);
MyDebugVec3 to(to1);
m_linePoints.push_back(from);
m_linePoints.push_back(to);
m_lineIndices.push_back(m_lineIndices.size());
m_lineIndices.push_back(m_lineIndices.size());
}
void btDbvt::extractLeaves(const btDbvtNode* node,btAlignedObjectArray<const btDbvtNode*>& leaves)
{
if(node->isinternal())
{
extractLeaves(node->childs[0],leaves);
extractLeaves(node->childs[1],leaves);
}
else
{
leaves.push_back(node);
}
}
// TODO: this routine appears to be numerically instable ...
void getVerticesInsidePlanes(const btAlignedObjectArray<btVector3>& planes,
btAlignedObjectArray<btVector3>& verticesOut,
std::set<int>& planeIndicesOut)
{
// Based on btGeometryUtil.cpp (Gino van den Bergen / Erwin Coumans)
verticesOut.resize(0);
planeIndicesOut.clear();
const int numPlanes = planes.size();
int i, j, k, l;
for (i = 0; i < numPlanes; i++)
{
const btVector3& N1 = planes[i];
for (j = i + 1; j < numPlanes; j++)
{
const btVector3& N2 = planes[j];
btVector3 n1n2 = N1.cross(N2);
if (n1n2.length2() > btScalar(0.0001))
{
for (k = j + 1; k < numPlanes; k++)
{
const btVector3& N3 = planes[k];
btVector3 n2n3 = N2.cross(N3);
btVector3 n3n1 = N3.cross(N1);
if ((n2n3.length2() > btScalar(0.0001)) && (n3n1.length2() > btScalar(0.0001) ))
{
btScalar quotient = (N1.dot(n2n3));
if (btFabs(quotient) > btScalar(0.0001))
{
btVector3 potentialVertex = (n2n3 * N1[3] + n3n1 * N2[3] + n1n2 * N3[3]) * (btScalar(-1.) / quotient);
for (l = 0; l < numPlanes; l++)
{
const btVector3& NP = planes[l];
if (btScalar(NP.dot(potentialVertex))+btScalar(NP[3]) > btScalar(0.000001))
break;
}
if (l == numPlanes)
{
// vertex (three plane intersection) inside all planes
verticesOut.push_back(potentialVertex);
planeIndicesOut.insert(i);
planeIndicesOut.insert(j);
planeIndicesOut.insert(k);
}
}
}
}
}
}
}
}
void BulletURDFImporter::getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const
{
childLinkIndices.resize(0);
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
if (linkPtr)
{
const UrdfLink* link = *linkPtr;
//int numChildren = m_data->m_urdfParser->getModel().m_links.getAtIndex(linkIndex)->
for (int i=0;i<link->m_childLinks.size();i++)
{
int childIndex =link->m_childLinks[i]->m_linkIndex;
childLinkIndices.push_back(childIndex);
}
}
}
void btGeometryUtil::getPlaneEquationsFromVertices(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& planeEquationsOut )
{
const int numvertices = vertices.size();
// brute force:
for (int i=0;i<numvertices;i++)
{
const btVector3& N1 = vertices[i];
for (int j=i+1;j<numvertices;j++)
{
const btVector3& N2 = vertices[j];
for (int k=j+1;k<numvertices;k++)
{
const btVector3& N3 = vertices[k];
btVector3 planeEquation,edge0,edge1;
edge0 = N2-N1;
edge1 = N3-N1;
btScalar normalSign = btScalar(1.);
for (int ww=0;ww<2;ww++)
{
planeEquation = normalSign * edge0.cross(edge1);
if (planeEquation.length2() > btScalar(0.0001))
{
planeEquation.normalize();
if (notExist(planeEquation,planeEquationsOut))
{
planeEquation[3] = -planeEquation.dot(N1);
//check if inside, and replace supportingVertexOut if needed
if (areVerticesBehindPlane(planeEquation,vertices,btScalar(0.01)))
{
planeEquationsOut.push_back(planeEquation);
}
}
}
normalSign = btScalar(-1.);
}
}
}
}
}
void PhysicsClient::initPhysics()
{
if (m_guiHelper && m_guiHelper->getParameterInterface())
{
bool isTrigger = false;
createButton("Load URDF",CMD_LOAD_URDF, isTrigger);
createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger);
createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM, isTrigger);
createButton("Get State",CMD_REQUEST_ACTUAL_STATE, isTrigger);
createButton("Send Desired State",CMD_SEND_DESIRED_STATE, isTrigger);
createButton("Create Box Collider",CMD_CREATE_BOX_COLLISION_SHAPE,isTrigger);
} else
{
m_userCommandRequests.push_back(CMD_LOAD_URDF);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
//m_userCommandRequests.push_back(CMD_SEND_DESIRED_STATE);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
//m_userCommandRequests.push_back(CMD_SET_JOINT_FEEDBACK);
m_userCommandRequests.push_back(CMD_CREATE_BOX_COLLISION_SHAPE);
//m_userCommandRequests.push_back(CMD_CREATE_RIGID_BODY);
m_userCommandRequests.push_back(CMD_STEP_FORWARD_SIMULATION);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
m_userCommandRequests.push_back(CMD_SHUTDOWN);
}
m_testBlock1 = (SharedMemoryExampleData*)m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
if (m_testBlock1)
{
// btAssert(m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER);
if (m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
{
b3Error("Error: please start server before client\n");
m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
m_testBlock1 = 0;
} else
{
b3Printf("Shared Memory status is OK\n");
}
} else
{
m_wantsTermination = true;
}
}
bool
buildGroundShape(Ogre::SceneManager* sceneMgr, btDynamicsWorld* dynamicsWorld, btAlignedObjectArray<btCollisionShape*>& collisionShapes)
{
btBoxShape* groundShape = new btBoxShape(btVector3(btScalar(150.0f),btScalar(1.0f),btScalar(150.0f)));
//btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0), 1);
//groundShape->setLocalScaling(btVector3(1,1,1));
btDefaultMotionState* groundMotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(0,-1,0)));
btRigidBody::btRigidBodyConstructionInfo groundRigidBodyCI(0,groundMotionState,groundShape,btVector3(0,0,0));
btRigidBody* groundRigidBody = new btRigidBody(groundRigidBodyCI);
groundRigidBody->setCcdMotionThreshold(1.0f);
groundRigidBody->setRestitution(0.0f);
groundRigidBody->setFriction(1.0f);
const btTransform& transform = groundRigidBody->getWorldTransform();
btVector3 trans = transform.getOrigin();
btQuaternion rot = transform.getRotation();
dynamicsWorld->addRigidBody(groundRigidBody);
collisionShapes.push_back(groundShape);
// build plane if scene manager exist
if (sceneMgr)
{
Ogre::Plane* plane = new Ogre::MovablePlane("Plane");
plane->d = 0;
plane->normal = Ogre::Vector3::UNIT_Y;
Ogre::MeshManager::getSingleton().createPlane("PlaneMesh", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, *plane,
256, 256, 1, 1, true, 1, 1, 1, Ogre::Vector3::UNIT_Z);
Ogre::Entity* ent = sceneMgr->createEntity("PlaneEntity", "PlaneMesh");
assert(ent);
ent->setCastShadows(false);
ent->setMaterialName("DefaultPlane");
Ogre::SceneNode* node = sceneMgr->getRootSceneNode()->createChildSceneNode("PlaneNode");
node->attachObject(ent);
node->setPosition(Ogre::Vector3::ZERO);
} // End if
return true;
}
void body(int bodytype, vec3 origin,vec3 size, vec3 inertia,float mass) {
btCollisionShape* aShape = NULL;
pos[0] = origin.x; pos[1] = origin.y; pos[2] = origin.z;
inertia.x = 1;
inertia.y = 1;
inertia.z = 1;
mass = 1;
switch (bodytype) {
case BODYTYPE_BOX:
aShape = new btBoxShape(btVector3(btScalar(size.x),btScalar(size.y),btScalar(size.z)));
break;
case BODYTYPE_CYLINDER:
aShape = new btCylinderShape(btVector3(btScalar(size.x),btScalar(size.y),btScalar(size.z)));
break;
case BODYTYPE_SPHERE:
aShape = new btSphereShape(btScalar(size.x));
break;
case BODYTYPE_CAPSULE:
aShape = new btCapsuleShape(btScalar(size.x),btScalar(size.y));
break;
case BODYTYPE_CONE:
aShape = new btConeShape(btScalar(size.x),btScalar(size.y));
break;
}
if (aShape) {
btScalar Mass(mass);
bool isDynamic = (Mass != 0.f);
btVector3 localInertia(inertia.x,inertia.y,inertia.z);
if (isDynamic)
aShape->calculateLocalInertia(mass,localInertia);
transform.setIdentity();
transform.setOrigin(btVector3(origin.x,origin.y,origin.z));
btDefaultMotionState* myMotionState = new btDefaultMotionState(transform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(Mass,myMotionState,aShape,localInertia);
body1 = new btRigidBody(rbInfo);
collisionShapes.push_back(aShape);
dynamicsWorld->addRigidBody(body1);
}
//return body1;
}
void btGImpactCollisionAlgorithm::gimpact_vs_shape_find_pairs(
btDispatcher* dispatcher,
const btTransform& trans0,
const btTransform& trans1,
const btGImpactShapeInterface* shape0,
const btCollisionShape* shape1,
btAlignedObjectArray<int>& collided_primitives
)
{
btAABB boxshape;
if(shape0->hasBoxSet())
{
btTransform trans1to0 = trans0.inverse();
trans1to0 *= trans1;
shape1->getAabb(trans1to0,boxshape.m_min,boxshape.m_max);
shape0->getBoxSet()->boxQuery(boxshape, collided_primitives);
}
else
{
shape1->getAabb(trans1,boxshape.m_min,boxshape.m_max);
btAABB boxshape0;
int i = shape0->getNumChildShapes();
while(i--)
{
shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max);
if(boxshape.has_collision(boxshape0))
{
collided_primitives.push_back(i);
}
}
}
}
void GetAllIndices(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, int parentIndex, btAlignedObjectArray<childParentIndex>& allIndices)
{
childParentIndex cp;
cp.m_index = urdfLinkIndex;
int mbIndex = cache.getMbIndexFromUrdfIndex(urdfLinkIndex);
cp.m_mbIndex = mbIndex;
cp.m_parentIndex = parentIndex;
int parentMbIndex = parentIndex>=0? cache.getMbIndexFromUrdfIndex(parentIndex) : -1;
cp.m_parentMBIndex = parentMbIndex;
allIndices.push_back(cp);
btAlignedObjectArray<int> urdfChildIndices;
u2b.getLinkChildIndices(urdfLinkIndex, urdfChildIndices);
int numChildren = urdfChildIndices.size();
for (int i = 0; i < numChildren; i++)
{
int urdfChildLinkIndex = urdfChildIndices[i];
GetAllIndices(u2b, cache, urdfChildLinkIndex, urdfLinkIndex, allIndices);
}
}
/// called whenever key terrain attribute is changed
void TerrainDemo::resetPhysics(void)
{
// remove old heightfield
clearWorld();
// reset gravity to point in appropriate direction
m_dynamicsWorld->setGravity(getUpVector(m_upAxis, 0.0, -s_gravity));
// get new heightfield of appropriate type
m_rawHeightfieldData =
getRawHeightfieldData(m_model, m_type, m_minHeight, m_maxHeight);
btAssert(m_rawHeightfieldData && "failed to create raw heightfield");
bool flipQuadEdges = false;
btHeightfieldTerrainShape * heightfieldShape =
new btHeightfieldTerrainShape(s_gridSize, s_gridSize,
m_rawHeightfieldData,
s_gridHeightScale,
m_minHeight, m_maxHeight,
m_upAxis, m_type, flipQuadEdges);
btAssert(heightfieldShape && "null heightfield");
// scale the shape
btVector3 localScaling = getUpVector(m_upAxis, s_gridSpacing, 1.0);
heightfieldShape->setLocalScaling(localScaling);
// stash this shape away
m_collisionShapes.push_back(heightfieldShape);
// set origin to middle of heightfield
btTransform tr;
tr.setIdentity();
tr.setOrigin(btVector3(0,-20,0));
// create ground object
float mass = 0.0;
localCreateRigidBody(mass, tr, heightfieldShape);
}
void Planar2D::initPhysics()
{
m_guiHelper->setUpAxis(1);
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new btDefaultCollisionConfiguration();
//m_collisionConfiguration->setConvexConvexMultipointIterations();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_simplexSolver = new btVoronoiSimplexSolver();
m_pdSolver = new btMinkowskiPenetrationDepthSolver();
m_convexAlgo2d = new btConvex2dConvex2dAlgorithm::CreateFunc(m_simplexSolver,m_pdSolver);
m_box2dbox2dAlgo = new btBox2dBox2dCollisionAlgorithm::CreateFunc();
m_dispatcher->registerCollisionCreateFunc(CONVEX_2D_SHAPE_PROXYTYPE,CONVEX_2D_SHAPE_PROXYTYPE,m_convexAlgo2d);
m_dispatcher->registerCollisionCreateFunc(BOX_2D_SHAPE_PROXYTYPE,CONVEX_2D_SHAPE_PROXYTYPE,m_convexAlgo2d);
m_dispatcher->registerCollisionCreateFunc(CONVEX_2D_SHAPE_PROXYTYPE,BOX_2D_SHAPE_PROXYTYPE,m_convexAlgo2d);
m_dispatcher->registerCollisionCreateFunc(BOX_2D_SHAPE_PROXYTYPE,BOX_2D_SHAPE_PROXYTYPE,m_box2dbox2dAlgo);
m_broadphase = new btDbvtBroadphase();
//m_broadphase = new btSimpleBroadphase();
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
m_solver = sol;
m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
//m_dynamicsWorld->getSolverInfo().m_erp = 1.f;
//m_dynamicsWorld->getSolverInfo().m_numIterations = 4;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->setGravity(btVector3(0,-10,0));
///create a few basic rigid bodies
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.),btScalar(50.),btScalar(150.)));
// btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,-43,0));
//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
{
btScalar mass(0.);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
groundShape->calculateLocalInertia(mass,localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
//add the body to the dynamics world
m_dynamicsWorld->addRigidBody(body);
}
{
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
btScalar u= btScalar(1*SCALING-0.04);
btVector3 points[3] = {btVector3(0,u,0),btVector3(-u,-u,0),btVector3(u,-u,0)};
btConvexShape* childShape0 = new btBoxShape(btVector3(btScalar(SCALING*1),btScalar(SCALING*1),btScalar(0.04)));
btConvexShape* colShape= new btConvex2dShape(childShape0);
//btCollisionShape* colShape = new btBox2dShape(btVector3(SCALING*1,SCALING*1,0.04));
btConvexShape* childShape1 = new btConvexHullShape(&points[0].getX(),3);
btConvexShape* colShape2= new btConvex2dShape(childShape1);
btConvexShape* childShape2 = new btCylinderShapeZ(btVector3(btScalar(SCALING*1),btScalar(SCALING*1),btScalar(0.04)));
btConvexShape* colShape3= new btConvex2dShape(childShape2);
m_collisionShapes.push_back(colShape);
m_collisionShapes.push_back(colShape2);
m_collisionShapes.push_back(colShape3);
m_collisionShapes.push_back(childShape0);
m_collisionShapes.push_back(childShape1);
m_collisionShapes.push_back(childShape2);
//btUniformScalingShape* colShape = new btUniformScalingShape(convexColShape,1.f);
colShape->setMargin(btScalar(0.03));
//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
/// Create Dynamic Objects
btTransform startTransform;
startTransform.setIdentity();
btScalar mass(1.f);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
colShape->calculateLocalInertia(mass,localInertia);
// float start_x = START_POS_X - ARRAY_SIZE_X/2;
// float start_y = START_POS_Y;
// float start_z = START_POS_Z - ARRAY_SIZE_Z/2;
btVector3 x(-ARRAY_SIZE_X, 8.0f,-20.f);
btVector3 y;
btVector3 deltaX(SCALING*1, SCALING*2,0.f);
btVector3 deltaY(SCALING*2, 0.0f,0.f);
for (int i = 0; i < ARRAY_SIZE_X; ++i)
{
y = x;
for (int j = i; j < ARRAY_SIZE_Y; ++j)
{
startTransform.setOrigin(y-btVector3(-10,0,0));
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(0,0,0);
switch (j%3)
{
#if 1
case 0:
rbInfo = btRigidBody::btRigidBodyConstructionInfo(mass,myMotionState,colShape,localInertia);
break;
case 1:
rbInfo = btRigidBody::btRigidBodyConstructionInfo(mass,myMotionState,colShape3,localInertia);
break;
#endif
default:
rbInfo = btRigidBody::btRigidBodyConstructionInfo(mass,myMotionState,colShape2,localInertia);
}
btRigidBody* body = new btRigidBody(rbInfo);
//body->setContactProcessingThreshold(colShape->getContactBreakingThreshold());
body->setActivationState(ISLAND_SLEEPING);
body->setLinearFactor(btVector3(1,1,0));
body->setAngularFactor(btVector3(0,0,1));
m_dynamicsWorld->addRigidBody(body);
body->setActivationState(ISLAND_SLEEPING);
// y += -0.8*deltaY;
y += deltaY;
}
x += deltaX;
}
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
//////////////////////Bullet Physics code////////////////////////////////
void init_physics(){
m_collisionConfiguration = new btFluidRigidCollisionConfiguration();
//'standard' Bullet configuration
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
m_solver = new btSequentialImpulseConstraintSolver();
m_fluidSolverCPU = new btFluidSphSolverDefault();
m_dynamicsWorld = new btFluidRigidDynamicsWorld(m_dispatcher, m_broadphase,
m_solver, m_collisionConfiguration, m_fluidSolverCPU);
m_fluidWorld = static_cast<btFluidRigidDynamicsWorld*>(m_dynamicsWorld);
m_fluidWorld->setGravity(btVector3(0.0 ,-9.8, 0.0));
m_fluidWorld->applyGravity();
btCollisionShape* groundShape = new btBoxShape( btVector3(100.0, 0.0, 100.0) );
m_collisionShapes.push_back(groundShape);
plane = new Plane(m_fluidWorld, groundShape, 0, 0, 0);
///////////////////// Demo_Drop/////////////////////////////
const btScalar VOL_BOUND = 52.0f;
btVector3 volMin(-VOL_BOUND, -10.0f, -VOL_BOUND);
btVector3 volMax(VOL_BOUND, VOL_BOUND * 2.0f, VOL_BOUND);
{
btFluidSph* fluid;
fluid = new btFluidSph(m_fluidWorld->getGlobalParameters(),
MIN_FLUID_PARTICLES);
btFluidSphParametersLocal FL = fluid->getLocalParameters();
FL.setDefaultParameters();
FL.m_aabbBoundaryMin = volMin;
FL.m_aabbBoundaryMax = volMax;
FL.m_particleRadius = 0.8f;
FL.m_enableAabbBoundary = 1;
FL.m_restDensity *= 3.0f;
FL.m_sphParticleMass *= 2.0f;
FL.m_stiffness /= 3.0f;
fluid->setLocalParameters(FL);
fluid->setGridCellSize(m_fluidWorld->getGlobalParameters() );
m_fluids.push_back(fluid);
// Also create an emitter
//*
{
btFluidEmitter* emitter = new btFluidEmitter();
m_emitter = emitter;
emitter->m_fluid = fluid;
const btScalar OFFSET(1.0);
emitter->m_positions.push_back(btVector3(0, 0, 0));
emitter->m_positions.push_back(btVector3(0, OFFSET, 0));
emitter->m_positions.push_back(btVector3(0, -OFFSET, 0));
emitter->m_positions.push_back(btVector3(OFFSET, 0, 0));
emitter->m_positions.push_back(btVector3(-OFFSET, 0, 0));
emitter->m_speed = 1.f;
emitter->m_center.setValue(12.f, 2.f, 12.f);
emitter->m_rotation.setEuler(90, -45, 0);
//
const btScalar INIT_BOUND = 50.0f;
btVector3 initMin(-INIT_BOUND, 0.f, 0.f);
btVector3 initMax(INIT_BOUND, 45.f, INIT_BOUND);
emitter->addVolume(fluid, initMin, initMax, fluid->getEmitterSpacing(m_fluidWorld->getGlobalParameters()) * 0.87 );
//
m_fluidWorld->addSphEmitter(emitter);
}
//*/
//*/
for (int i = 0; i < m_fluids.size(); ++i) {
const bool ENABLE_CCD = true;
if (ENABLE_CCD)
m_fluids[i]->setCcdMotionThreshold(
m_fluids[i]->getLocalParameters().m_particleRadius);
m_fluidWorld->addFluidSph(m_fluids[i]);
}
}
}
ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName)
:CommonMultiBodyBase(helper),
m_grav(-10),
m_upAxis(2)
{
m_data = new ImportUrdfInternalData;
if (option==1)
{
m_useMultiBody = true;
} else
{
m_useMultiBody = false;
}
static int count = 0;
if (fileName)
{
setFileName(fileName);
} else
{
gFileNameArray.clear();
//load additional urdf file names from file
FILE* f = fopen("urdf_files.txt","r");
if (f)
{
int result;
//warning: we don't avoid string buffer overflow in this basic example in fscanf
char fileName[1024];
do
{
result = fscanf(f,"%s",fileName);
b3Printf("urdf_files.txt entry %s",fileName);
if (result==1)
{
gFileNameArray.push_back(fileName);
}
} while (result==1);
fclose(f);
}
if (gFileNameArray.size()==0)
{
gFileNameArray.push_back("r2d2.urdf");
}
int numFileNames = gFileNameArray.size();
if (count>=numFileNames)
{
count=0;
}
sprintf(m_fileName,"%s",gFileNameArray[count++].c_str());
}
}
void ImportUrdfSetup::initPhysics()
{
m_guiHelper->setUpAxis(m_upAxis);
this->createEmptyDynamicsWorld();
//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawContactPoints
+btIDebugDraw::DBG_DrawAabb
);//+btIDebugDraw::DBG_DrawConstraintLimits);
if (m_guiHelper->getParameterInterface())
{
SliderParams slider("Gravity", &m_grav);
slider.m_minVal = -10;
slider.m_maxVal = 10;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
BulletURDFImporter u2b(m_guiHelper, 0);
bool loadOk = u2b.loadURDF(m_fileName);
#ifdef TEST_MULTIBODY_SERIALIZATION
//test to serialize a multibody to disk or shared memory, with base, link and joint names
btSerializer* s = new btDefaultSerializer;
#endif //TEST_MULTIBODY_SERIALIZATION
if (loadOk)
{
//printTree(u2b,u2b.getRootLinkIndex());
//u2b.printTree();
btTransform identityTrans;
identityTrans.setIdentity();
{
//todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
//int rootLinkIndex = u2b.getRootLinkIndex();
//b3Printf("urdf root link index = %d\n",rootLinkIndex);
MyMultiBodyCreator creation(m_guiHelper);
ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix());
m_data->m_rb = creation.getRigidBody();
m_data->m_mb = creation.getBulletMultiBody();
btMultiBody* mb = m_data->m_mb;
for (int i = 0; i < u2b.getNumAllocatedCollisionShapes(); i++)
{
m_collisionShapes.push_back(u2b.getAllocatedCollisionShape(i));
}
if (m_useMultiBody && mb )
{
std::string* name = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
m_nameMemory.push_back(name);
#ifdef TEST_MULTIBODY_SERIALIZATION
s->registerNameForPointer(name->c_str(),name->c_str());
#endif//TEST_MULTIBODY_SERIALIZATION
mb->setBaseName(name->c_str());
//create motors for each btMultiBody joint
int numLinks = mb->getNumLinks();
for (int i=0;i<numLinks;i++)
{
int mbLinkIndex = i;
int urdfLinkIndex = creation.m_mb2urdfLink[mbLinkIndex];
std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex));
std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str());
#ifdef TEST_MULTIBODY_SERIALIZATION
s->registerNameForPointer(jointName->c_str(),jointName->c_str());
s->registerNameForPointer(linkName->c_str(),linkName->c_str());
#endif//TEST_MULTIBODY_SERIALIZATION
m_nameMemory.push_back(jointName);
m_nameMemory.push_back(linkName);
mb->getLink(i).m_linkName = linkName->c_str();
mb->getLink(i).m_jointName = jointName->c_str();
if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute
||mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::ePrismatic
)
{
if (m_data->m_numMotors<MAX_NUM_MOTORS)
{
char motorName[1024];
sprintf(motorName,"%s q'", jointName->c_str());
btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors];
*motorVel = 0.f;
SliderParams slider(motorName,motorVel);
slider.m_minVal=-4;
slider.m_maxVal=4;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
float maxMotorImpulse = 10.1f;
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,0,0,maxMotorImpulse);
//motor->setMaxAppliedImpulse(0);
m_data->m_jointMotors[m_data->m_numMotors]=motor;
m_dynamicsWorld->addMultiBodyConstraint(motor);
m_data->m_numMotors++;
}
}
}
} else
{
if (1)
{
//create motors for each generic joint
int num6Dof = creation.getNum6DofConstraints();
for (int i=0;i<num6Dof;i++)
{
btGeneric6DofSpring2Constraint* c = creation.get6DofConstraint(i);
if (c->getUserConstraintPtr())
{
GenericConstraintUserInfo* jointInfo = (GenericConstraintUserInfo*)c->getUserConstraintPtr();
if ((jointInfo->m_urdfJointType ==URDFRevoluteJoint) ||
(jointInfo->m_urdfJointType ==URDFPrismaticJoint) ||
(jointInfo->m_urdfJointType ==URDFContinuousJoint))
{
int urdfLinkIndex = jointInfo->m_urdfIndex;
std::string jointName = u2b.getJointName(urdfLinkIndex);
char motorName[1024];
sprintf(motorName,"%s q'", jointName.c_str());
btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors];
*motorVel = 0.f;
SliderParams slider(motorName,motorVel);
slider.m_minVal=-4;
slider.m_maxVal=4;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
m_data->m_generic6DofJointMotors[m_data->m_numMotors]=c;
bool motorOn = true;
c->enableMotor(jointInfo->m_jointAxisIndex,motorOn);
c->setMaxMotorForce(jointInfo->m_jointAxisIndex,10000);
c->setTargetVelocity(jointInfo->m_jointAxisIndex,0);
m_data->m_numMotors++;
}
}
}
}
}
}
//the btMultiBody support is work-in-progress :-)
for (int i=0;i<m_dynamicsWorld->getNumMultiBodyConstraints();i++)
{
m_dynamicsWorld->getMultiBodyConstraint(i)->finalizeMultiDof();
}
bool createGround=true;
if (createGround)
{
btVector3 groundHalfExtents(20,20,20);
groundHalfExtents[m_upAxis]=1.f;
btBoxShape* box = new btBoxShape(groundHalfExtents);
m_collisionShapes.push_back(box);
box->initializePolyhedralFeatures();
m_guiHelper->createCollisionShapeGraphicsObject(box);
btTransform start; start.setIdentity();
btVector3 groundOrigin(0,0,0);
groundOrigin[m_upAxis]=-2.5;
start.setOrigin(groundOrigin);
btRigidBody* body = createRigidBody(0,start,box);
//m_dynamicsWorld->removeRigidBody(body);
// m_dynamicsWorld->addRigidBody(body,2,1);
btVector3 color(0.5,0.5,0.5);
m_guiHelper->createRigidBodyGraphicsObject(body,color);
}
}
#ifdef TEST_MULTIBODY_SERIALIZATION
m_dynamicsWorld->serialize(s);
b3ResourcePath p;
char resourcePath[1024];
if (p.findResourcePath("r2d2_multibody.bullet",resourcePath,1024))
{
FILE* f = fopen(resourcePath,"wb");
fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f);
fclose(f);
}
#endif//TEST_MULTIBODY_SERIALIZATION
}
void ExampleEntriesAll::registerExampleEntry(int menuLevel, const char* name,const char* description, CommonExampleInterface::CreateFunc* createFunc, int option)
{
ExampleEntry e( menuLevel,name,description, createFunc, option);
gAdditionalRegisteredExamples.push_back(e);
}
void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut)
{
GLInstanceGraphicsShape* glmesh = 0;
btConvexShape* convexColShape = 0;
switch (visual->m_geometry.m_type)
{
case URDF_GEOM_CYLINDER:
{
btAlignedObjectArray<btVector3> vertices;
//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
int numSteps = 32;
for (int i = 0; i<numSteps; i++)
{
btScalar cylRadius = visual->m_geometry.m_cylinderRadius;
btScalar cylLength = visual->m_geometry.m_cylinderLength;
btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.);
vertices.push_back(vert);
vert[2] = -cylLength / 2.;
vertices.push_back(vert);
}
btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
cylZShape->setMargin(0.001);
convexColShape = cylZShape;
break;
}
case URDF_GEOM_BOX:
{
btVector3 extents = visual->m_geometry.m_boxSize;
btBoxShape* boxShape = new btBoxShape(extents*0.5f);
//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
convexColShape = boxShape;
convexColShape->setMargin(0.001);
break;
}
case URDF_GEOM_SPHERE:
{
btScalar radius = visual->m_geometry.m_sphereRadius;
btSphereShape* sphereShape = new btSphereShape(radius);
convexColShape = sphereShape;
convexColShape->setMargin(0.001);
break;
break;
}
case URDF_GEOM_MESH:
{
if (visual->m_name.length())
{
//b3Printf("visual->name=%s\n", visual->m_name.c_str());
}
if (1)//visual->m_geometry)
{
if (visual->m_geometry.m_meshFileName.length())
{
const char* filename = visual->m_geometry.m_meshFileName.c_str();
//b3Printf("mesh->filename=%s\n", filename);
char fullPath[1024];
int fileType = 0;
char tmpPathPrefix[1024];
std::string xml_string;
int maxPathLen = 1024;
b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen);
char visualPathPrefix[1024];
sprintf(visualPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix);
sprintf(fullPath, "%s%s", urdfPathPrefix, filename);
b3FileUtils::toLower(fullPath);
if (strstr(fullPath, ".dae"))
{
fileType = MY_FILE_COLLADA;
}
if (strstr(fullPath, ".stl"))
{
fileType = MY_FILE_STL;
}
if (strstr(fullPath,".obj"))
{
fileType = MY_FILE_OBJ;
}
sprintf(fullPath, "%s%s", urdfPathPrefix, filename);
FILE* f = fopen(fullPath, "rb");
if (f)
{
fclose(f);
switch (fileType)
{
case MY_FILE_OBJ:
{
//glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
b3ImportMeshData meshData;
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fullPath, meshData))
{
if (meshData.m_textureImage)
{
MyTexture2 texData;
texData.m_width = meshData.m_textureWidth;
texData.m_height = meshData.m_textureHeight;
texData.textureData = meshData.m_textureImage;
texturesOut.push_back(texData);
}
glmesh = meshData.m_gfxShape;
}
break;
}
case MY_FILE_STL:
{
glmesh = LoadMeshFromSTL(fullPath);
break;
}
case MY_FILE_COLLADA:
{
btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
btTransform upAxisTrans; upAxisTrans.setIdentity();
float unitMeterScaling = 1;
int upAxis = 2;
LoadMeshFromCollada(fullPath,
visualShapes,
visualShapeInstances,
upAxisTrans,
unitMeterScaling,
upAxis);
glmesh = new GLInstanceGraphicsShape;
// int index = 0;
glmesh->m_indices = new b3AlignedObjectArray<int>();
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
for (int i = 0; i<visualShapeInstances.size(); i++)
{
ColladaGraphicsInstance* instance = &visualShapeInstances[i];
GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
b3AlignedObjectArray<GLInstanceVertex> verts;
verts.resize(gfxShape->m_vertices->size());
int baseIndex = glmesh->m_vertices->size();
for (int i = 0; i<gfxShape->m_vertices->size(); i++)
{
verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0];
verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1];
verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2];
verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0];
verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1];
verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0];
verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1];
verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];
verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];
}
int curNumIndices = glmesh->m_indices->size();
int additionalIndices = gfxShape->m_indices->size();
glmesh->m_indices->resize(curNumIndices + additionalIndices);
for (int k = 0; k<additionalIndices; k++)
{
glmesh->m_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex;
}
//compensate upAxisTrans and unitMeterScaling here
btMatrix4x4 upAxisMat;
upAxisMat.setIdentity();
// upAxisMat.setPureRotation(upAxisTrans.getRotation());
btMatrix4x4 unitMeterScalingMat;
unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling));
btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform;
//btMatrix4x4 worldMat = instance->m_worldTransform;
int curNumVertices = glmesh->m_vertices->size();
int additionalVertices = verts.size();
glmesh->m_vertices->reserve(curNumVertices + additionalVertices);
for (int v = 0; v<verts.size(); v++)
{
btVector3 pos(verts[v].xyzw[0], verts[v].xyzw[1], verts[v].xyzw[2]);
pos = worldMat*pos;
verts[v].xyzw[0] = float(pos[0]);
verts[v].xyzw[1] = float(pos[1]);
verts[v].xyzw[2] = float(pos[2]);
glmesh->m_vertices->push_back(verts[v]);
}
}
glmesh->m_numIndices = glmesh->m_indices->size();
glmesh->m_numvertices = glmesh->m_vertices->size();
//glmesh = LoadMeshFromCollada(fullPath);
break;
}
default:
{
b3Warning("Error: unsupported file type for Visual mesh: %s\n", fullPath);
btAssert(0);
}
}
if (glmesh && glmesh->m_vertices && (glmesh->m_numvertices>0))
{
//apply the geometry scaling
for (int i=0;i<glmesh->m_vertices->size();i++)
{
glmesh->m_vertices->at(i).xyzw[0] *= visual->m_geometry.m_meshScale[0];
glmesh->m_vertices->at(i).xyzw[1] *= visual->m_geometry.m_meshScale[1];
glmesh->m_vertices->at(i).xyzw[2] *= visual->m_geometry.m_meshScale[2];
}
}
else
{
b3Warning("issue extracting mesh from COLLADA/STL file %s\n", fullPath);
}
}
else
{
b3Warning("mesh geometry not found %s\n", fullPath);
}
}
}
break;
}
default:
{
b3Warning("Error: unknown visual geometry type\n");
}
}
//if we have a convex, tesselate into localVertices/localIndices
if ((glmesh==0) && convexColShape)
{
btShapeHull* hull = new btShapeHull(convexColShape);
hull->buildHull(0.0);
{
// int strideInBytes = 9*sizeof(float);
int numVertices = hull->numVertices();
int numIndices = hull->numIndices();
glmesh = new GLInstanceGraphicsShape;
// int index = 0;
glmesh->m_indices = new b3AlignedObjectArray<int>();
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
for (int i = 0; i < numVertices; i++)
{
GLInstanceVertex vtx;
btVector3 pos = hull->getVertexPointer()[i];
vtx.xyzw[0] = pos.x();
vtx.xyzw[1] = pos.y();
vtx.xyzw[2] = pos.z();
vtx.xyzw[3] = 1.f;
pos.normalize();
vtx.normal[0] = pos.x();
vtx.normal[1] = pos.y();
vtx.normal[2] = pos.z();
vtx.uv[0] = 0.5f;
vtx.uv[1] = 0.5f;
glmesh->m_vertices->push_back(vtx);
}
btAlignedObjectArray<int> indices;
for (int i = 0; i < numIndices; i++)
{
glmesh->m_indices->push_back(hull->getIndexPointer()[i]);
}
glmesh->m_numvertices = glmesh->m_vertices->size();
glmesh->m_numIndices = glmesh->m_indices->size();
}
delete hull;
delete convexColShape;
convexColShape = 0;
}
if (glmesh && glmesh->m_numIndices>0 && glmesh->m_numvertices >0)
{
int baseIndex = verticesOut.size();
for (int i = 0; i < glmesh->m_indices->size(); i++)
{
indicesOut.push_back(glmesh->m_indices->at(i) + baseIndex);
}
for (int i = 0; i < glmesh->m_vertices->size(); i++)
{
GLInstanceVertex& v = glmesh->m_vertices->at(i);
btVector3 vert(v.xyzw[0],v.xyzw[1],v.xyzw[2]);
btVector3 vt = visualTransform*vert;
v.xyzw[0] = vt[0];
v.xyzw[1] = vt[1];
v.xyzw[2] = vt[2];
btVector3 triNormal(v.normal[0],v.normal[1],v.normal[2]);
triNormal = visualTransform.getBasis()*triNormal;
v.normal[0] = triNormal[0];
v.normal[1] = triNormal[1];
v.normal[2] = triNormal[2];
verticesOut.push_back(v);
}
}
delete glmesh;
}
void initBullet(void)
{
#ifdef USE_GPU_SOLVER
g_dx11Solver = new btDX11SoftBodySolver( g_pd3dDevice, DXUTGetD3D11DeviceContext() );
g_solver = g_dx11Solver;
#ifdef USE_GPU_COPY
g_softBodyOutput = new btSoftBodySolverOutputDXtoDX( g_pd3dDevice, DXUTGetD3D11DeviceContext() );
#else // #ifdef USE_GPU_COPY
g_softBodyOutput = new btSoftBodySolverOutputDXtoCPU;
#endif // #ifdef USE_GPU_COPY
#else
#ifdef USE_SIMDAWARE_SOLVER
g_dx11SIMDSolver = new btDX11SIMDAwareSoftBodySolver( g_pd3dDevice, DXUTGetD3D11DeviceContext() );
g_solver = g_dx11SIMDSolver;
g_softBodyOutput = new btSoftBodySolverOutputDXtoCPU;
#ifdef USE_GPU_COPY
g_softBodyOutput = new btSoftBodySolverOutputDXtoDX( g_pd3dDevice, DXUTGetD3D11DeviceContext() );
#else // #ifdef USE_GPU_COPY
g_softBodyOutput = new btSoftBodySolverOutputDXtoCPU;
#endif // #ifdef USE_GPU_COPY
#else
g_cpuSolver = new btCPUSoftBodySolver;
g_solver = g_cpuSolver;
g_softBodyOutput = new btSoftBodySolverOutputCPUtoCPU;
//g_defaultSolver = new btDefaultSoftBodySolver;
//g_solver = g_defaultSolver;
#endif
#endif
if (g_dx11SIMDSolver)
g_dx11SIMDSolver->setEnableUpdateBounds(true);
if (g_dx11Solver)
g_dx11Solver->setEnableUpdateBounds(true);
// Initialise CPU physics device
//m_collisionConfiguration = new btDefaultCollisionConfiguration();
m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
m_solver = sol;
m_dynamicsWorld = new btSoftRigidDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration, g_solver);
m_dynamicsWorld->setGravity(btVector3(0,-10,0));
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,-50,0));
m_dynamicsWorld->getWorldInfo().air_density = (btScalar)1.2;
m_dynamicsWorld->getWorldInfo().water_density = 0;
m_dynamicsWorld->getWorldInfo().water_offset = 0;
m_dynamicsWorld->getWorldInfo().water_normal = btVector3(0,0,0);
m_dynamicsWorld->getWorldInfo().m_gravity.setValue(0,-10,0);
#if 0
{
btScalar mass(0.);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
groundShape->calculateLocalInertia(mass,localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
//add the body to the dynamics world
m_dynamicsWorld->addRigidBody(body);
}
#endif
#if 0
{
btScalar mass(0.);
//btScalar mass(1.);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btCollisionShape *capsuleShape = new btCapsuleShape(5, 10);
capsuleShape->setMargin( 0.5 );
my_capsule.set_collision_shape(capsuleShape);
btVector3 localInertia(0,0,0);
if (isDynamic)
capsuleShape->calculateLocalInertia(mass,localInertia);
m_collisionShapes.push_back(capsuleShape);
btTransform capsuleTransform;
capsuleTransform.setIdentity();
#ifdef TABLETEST
capsuleTransform.setOrigin(btVector3(0, 10, -11));
const btScalar pi = 3.141592654;
capsuleTransform.setRotation(btQuaternion(0, 0, pi/2));
#else
capsuleTransform.setOrigin(btVector3(0, 20, -10));
const btScalar pi = 3.141592654;
//capsuleTransform.setRotation(btQuaternion(0, 0, pi/2));
capsuleTransform.setRotation(btQuaternion(0, 0, 0));
#endif
btDefaultMotionState* myMotionState = new btDefaultMotionState(capsuleTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,capsuleShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
body->setFriction( 0.8f );
my_capsule.set_collision_object(body);
m_dynamicsWorld->addRigidBody(body);
//cap_1.collisionShape = body;
capCollider = body;
}
#endif
createFlag( clothWidth, clothHeight, m_flags );
// Create output buffer descriptions for ecah flag
// These describe where the simulation should send output data to
for( int flagIndex = 0; flagIndex < m_flags.size(); ++flagIndex )
{
// In this case we have a DX11 output buffer with a vertex at index 0, 8, 16 and so on as well as a normal at 3, 11, 19 etc.
// Copies will be performed GPU-side directly into the output buffer
#ifdef USE_GPU_COPY
btDX11VertexBufferDescriptor *vertexBufferDescriptor = new btDX11VertexBufferDescriptor(DXUTGetD3D11DeviceContext(), cloths[flagIndex].pVB[0], cloths[flagIndex].g_pVB_UAV, 0, 8, 3, 8);
cloths[flagIndex].m_vertexBufferDescriptor = vertexBufferDescriptor;
#else // #ifdef USE_GPU_COPY
btCPUVertexBufferDescriptor *vertexBufferDescriptor = new btCPUVertexBufferDescriptor(cloths[flagIndex].cpu_buffer, 0, 8, 3, 8);
cloths[flagIndex].m_vertexBufferDescriptor = vertexBufferDescriptor;
#endif // #ifdef USE_GPU_COPY
}
g_solver->optimize( m_dynamicsWorld->getSoftBodyArray() );
}
