int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const
{
btAlignedObjectArray<GLInstanceVertex> vertices;
btAlignedObjectArray<int> indices;
btTransform startTrans; startTrans.setIdentity();
int graphicsIndex = -1;
#if USE_ROS_URDF_PARSER
for (int v = 0; v < (int)m_data->m_links[linkIndex]->visual_array.size(); v++)
{
const Visual* vis = m_data->m_links[linkIndex]->visual_array[v].get();
btVector3 childPos(vis->origin.position.x, vis->origin.position.y, vis->origin.position.z);
btQuaternion childOrn(vis->origin.rotation.x, vis->origin.rotation.y, vis->origin.rotation.z, vis->origin.rotation.w);
btTransform childTrans;
childTrans.setOrigin(childPos);
childTrans.setRotation(childOrn);
convertURDFToVisualShape(vis, pathPrefix, inertialFrame.inverse()*childTrans, vertices, indices);
}
#else
const UrdfModel& model = m_data->m_urdfParser.getModel();
UrdfLink* const* linkPtr = model.m_links.getAtIndex(linkIndex);
if (linkPtr)
{
const UrdfLink* link = *linkPtr;
for (int v = 0; v < link->m_visualArray.size();v++)
{
const UrdfVisual& vis = link->m_visualArray[v];
btTransform childTrans = vis.m_linkLocalFrame;
btHashString matName(vis.m_materialName.c_str());
UrdfMaterial *const * matPtr = model.m_materials[matName];
if (matPtr)
{
UrdfMaterial *const mat = *matPtr;
//printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]);
m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor);
}
convertURDFToVisualShape(&vis, pathPrefix, inertialFrame.inverse()*childTrans, vertices, indices);
}
}
#endif
if (vertices.size() && indices.size())
{
graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size());
}
return graphicsIndex;
}
class btCompoundShape* BulletURDFImporter::convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
{
btCompoundShape* compoundShape = new btCompoundShape();
m_data->m_allocatedCollisionShapes.push_back(compoundShape);
compoundShape->setMargin(0.001);
#if USE_ROS_URDF_PARSER
for (int v=0;v<(int)m_data->m_links[linkIndex]->collision_array.size();v++)
{
const Collision* col = m_data->m_links[linkIndex]->collision_array[v].get();
btCollisionShape* childShape = convertURDFToCollisionShape(col ,pathPrefix);
if (childShape)
{
btVector3 childPos(col->origin.position.x, col->origin.position.y, col->origin.position.z);
btQuaternion childOrn(col->origin.rotation.x, col->origin.rotation.y, col->origin.rotation.z, col->origin.rotation.w);
btTransform childTrans;
childTrans.setOrigin(childPos);
childTrans.setRotation(childOrn);
compoundShape->addChildShape(localInertiaFrame.inverse()*childTrans,childShape);
}
}
#else
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
btAssert(linkPtr);
if (linkPtr)
{
UrdfLink* link = *linkPtr;
for (int v=0;v<link->m_collisionArray.size();v++)
{
const UrdfCollision& col = link->m_collisionArray[v];
btCollisionShape* childShape = convertURDFToCollisionShape(&col ,pathPrefix);
if (childShape)
{
btTransform childTrans = col.m_linkLocalFrame;
compoundShape->addChildShape(localInertiaFrame.inverse()*childTrans,childShape);
}
}
}
#endif
return compoundShape;
}
CollisionTutorialBullet2(GUIHelperInterface* guiHelper, int tutorialIndex)
:m_app(guiHelper->getAppInterface()),
m_guiHelper(guiHelper),
m_tutorialIndex(tutorialIndex),
m_collisionSdkHandle(0),
m_collisionWorldHandle(0),
m_stage(0),
m_counter(0),
m_timeSeriesCanvas0(0)
{
gTotalPoints = 0;
m_app->setUpAxis(1);
m_app->m_renderer->enableBlend(true);
switch (m_tutorialIndex)
{
case TUT_SPHERE_PLANE_RTB3:
case TUT_SPHERE_PLANE_BULLET2:
{
if (m_tutorialIndex==TUT_SPHERE_PLANE_BULLET2)
{
m_collisionSdkHandle = plCreateBullet2CollisionSdk();
} else
{
#ifndef DISABLE_REAL_TIME_BULLET3_COLLISION_SDK
m_collisionSdkHandle = plCreateRealTimeBullet3CollisionSdk();
#endif //DISABLE_REAL_TIME_BULLET3_COLLISION_SDK
}
if (m_collisionSdkHandle)
{
int maxNumObjsCapacity=1024;
int maxNumShapesCapacity=1024;
int maxNumPairsCapacity=16384;
btAlignedObjectArray<plCollisionObjectHandle> colliders;
m_collisionWorldHandle = plCreateCollisionWorld(m_collisionSdkHandle,maxNumObjsCapacity,maxNumShapesCapacity,maxNumPairsCapacity);
//create objects, do query etc
{
float radius = 1.f;
void* userPointer = 0;
{
for (int j=0;j<sNumCompounds;j++)
{
plCollisionShapeHandle compoundShape = plCreateCompoundShape(m_collisionSdkHandle,m_collisionWorldHandle);
for (int i=0;i<sNumSpheres;i++)
{
btVector3 childPos(i*1.5,0,0);
btQuaternion childOrn(0,0,0,1);
btVector3 scaling(radius,radius,radius);
plCollisionShapeHandle childShape = plCreateSphereShape(m_collisionSdkHandle, m_collisionWorldHandle,radius);
plAddChildShape(m_collisionSdkHandle,m_collisionWorldHandle,compoundShape, childShape,childPos,childOrn);
//m_guiHelper->createCollisionObjectGraphicsObject(colObj,color);
}
if (m_tutorialIndex==TUT_SPHERE_PLANE_BULLET2)
{
btCollisionShape* colShape = (btCollisionShape*) compoundShape;
m_guiHelper->createCollisionShapeGraphicsObject(colShape);
} else
{
}
{
btVector3 pos(j*sNumSpheres*1.5,-2.4,0);
btQuaternion orn(0,0,0,1);
plCollisionObjectHandle colObjHandle = plCreateCollisionObject(m_collisionSdkHandle,m_collisionWorldHandle,userPointer, -1,compoundShape,pos,orn);
if (m_tutorialIndex==TUT_SPHERE_PLANE_BULLET2)
{
btCollisionObject* colObj = (btCollisionObject*) colObjHandle;
btVector4 color=sColors[j&3];
m_guiHelper->createCollisionObjectGraphicsObject(colObj,color);
colliders.push_back(colObjHandle);
plAddCollisionObject(m_collisionSdkHandle, m_collisionWorldHandle,colObjHandle);
}
}
}
}
}
{
plCollisionShapeHandle colShape = plCreatePlaneShape(m_collisionSdkHandle, m_collisionWorldHandle,0,1,0,-3.5);
btVector3 pos(0,0,0);
btQuaternion orn(0,0,0,1);
void* userPointer = 0;
plCollisionObjectHandle colObj = plCreateCollisionObject(m_collisionSdkHandle,m_collisionWorldHandle,userPointer, 0,colShape,pos,orn);
colliders.push_back(colObj);
plAddCollisionObject(m_collisionSdkHandle, m_collisionWorldHandle,colObj);
}
int numContacts = plCollide(m_collisionSdkHandle,m_collisionWorldHandle,colliders[0],colliders[1],pointsOut,sPointCapacity);
printf("numContacts = %d\n", numContacts);
void* myUserPtr = 0;
plWorldCollide(m_collisionSdkHandle,m_collisionWorldHandle,myNearCallback, myUserPtr);
printf("total points=%d\n",gTotalPoints);
//plRemoveCollisionObject(m_collisionSdkHandle,m_collisionWorldHandle,colObj);
//plDeleteCollisionObject(m_collisionSdkHandle,colObj);
//plDeleteShape(m_collisionSdkHandle,colShape);
}
/*
m_timeSeriesCanvas0 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,512,256,"Constant Velocity");
m_timeSeriesCanvas0 ->setupTimeSeries(2,60, 0);
m_timeSeriesCanvas0->addDataSource("X position (m)", 255,0,0);
m_timeSeriesCanvas0->addDataSource("X velocity (m/s)", 0,0,255);
m_timeSeriesCanvas0->addDataSource("dX/dt (m/s)", 0,0,0);
*/
break;
}
default:
{
m_timeSeriesCanvas0 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,512,256,"Unknown");
m_timeSeriesCanvas0 ->setupTimeSeries(1,60, 0);
}
};
{
int boxId = m_app->registerCubeShape(100,0.01,100);
b3Vector3 pos = b3MakeVector3(0,-3.5,0);
b3Quaternion orn(0,0,0,1);
b3Vector4 color = b3MakeVector4(1,1,1,1);
b3Vector3 scaling = b3MakeVector3(1,1,1);
m_app->m_renderer->registerGraphicsInstance(boxId,pos,orn,color,scaling);
}
{
int textureIndex = -1;
if (1)
{
int width,height,n;
const char* filename = "data/cube.png";
const unsigned char* image=0;
const char* prefix[]={"./","../","../../","../../../","../../../../"};
int numprefix = sizeof(prefix)/sizeof(const char*);
for (int i=0;!image && i<numprefix;i++)
{
char relativeFileName[1024];
sprintf(relativeFileName,"%s%s",prefix[i],filename);
image = stbi_load(relativeFileName, &width, &height, &n, 3);
}
b3Assert(image);
if (image)
{
textureIndex = m_app->m_renderer->registerTexture(image,width,height);
}
}
}
m_app->m_renderer->writeTransforms();
}
collision_shape_t::pointer collisionShapeNode::createCompositeShape(const MPlug& plgInShape)
{
std::vector<collision_shape_t::pointer> childCollisionShapes;
std::vector< vec3f> childPositions;
std::vector< quatf> childOrientations;
MPlugArray plgaConnectedTo;
plgInShape.connectedTo(plgaConnectedTo, true, true);
int numSelectedShapes = plgaConnectedTo.length();
if(numSelectedShapes > 0) {
MObject node = plgaConnectedTo[0].node();
MDagPath dagPath;
MDagPath::getAPathTo(node, dagPath);
int numChildren = dagPath.childCount();
if (node.hasFn(MFn::kTransform))
{
MFnTransform trNode(dagPath);
MVector mtranslation = trNode.getTranslation(MSpace::kTransform);
MObject thisObject(thisMObject());
MFnDagNode fnDagNode(thisObject);
MStatus status;
MFnTransform fnParentTransform(fnDagNode.parent(0, &status));
mtranslation = trNode.getTranslation(MSpace::kPostTransform);
mtranslation = fnParentTransform.getTranslation(MSpace::kTransform);
const char* name = trNode.name().asChar();
printf("name = %s\n", name);
for (int i=0;i<numChildren;i++)
{
MObject child = dagPath.child(i);
if(child.hasFn(MFn::kMesh))
{
return false;
}
if(child.hasFn(MFn::kTransform))
{
MDagPath dagPath;
MDagPath::getAPathTo(child, dagPath);
MFnTransform trNode(dagPath);
MVector mtranslation = trNode.getTranslation(MSpace::kTransform);
mtranslation = trNode.getTranslation(MSpace::kPostTransform);
MQuaternion mrotation;
trNode.getRotation(mrotation, MSpace::kTransform);
double mscale[3];
trNode.getScale(mscale);
vec3f childPos((float)mtranslation.x, (float)mtranslation.y, (float)mtranslation.z);
quatf childOrn((float)mrotation.w, (float)mrotation.x, (float)mrotation.y, (float)mrotation.z);
const char* name = trNode.name().asChar();
printf("name = %s\n", name);
int numGrandChildren = dagPath.childCount();
{
for (int i=0;i<numGrandChildren;i++)
{
MObject grandchild = dagPath.child(i);
if(grandchild.hasFn(MFn::kMesh))
{
collision_shape_t::pointer childShape = createCollisionShape(grandchild);
if (childShape)
{
childCollisionShapes.push_back(childShape);
childPositions.push_back(childPos);
childOrientations.push_back(childOrn);
}
}
}
}
}
}
}
}
if (childCollisionShapes.size()>0)
{
composite_shape_t::pointer composite = solver_t::create_composite_shape(
&childCollisionShapes[0],
&childPositions[0],
&childOrientations[0],
childCollisionShapes.size()
);
return composite;
}
return 0;
}
