diff --git a/data/plane_transparent.urdf b/data/plane_transparent.urdf index 92279ca1e..1634fd4b7 100644 --- a/data/plane_transparent.urdf +++ b/data/plane_transparent.urdf @@ -15,7 +15,7 @@ - + diff --git a/data/spider.bullet b/data/spider.bullet index 05d17c886..b7925c544 100644 Binary files a/data/spider.bullet and b/data/spider.bullet differ diff --git a/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp b/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp index f53cba232..868d25529 100644 --- a/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp +++ b/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp @@ -139,7 +139,7 @@ void CollisionShape2TriangleMesh(btCollisionShape* collisionShape, const btTrans btConvexShape* convex = (btConvexShape*)collisionShape; { btShapeHull* hull = new btShapeHull(convex); - hull->buildHull(0.0); + hull->buildHull(0.0, 1); { //int strideInBytes = 9*sizeof(float); diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp index 2559d3d3c..97d618aa7 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp +++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp @@ -301,6 +301,15 @@ void OpenGLGuiHelper::changeTexture(int textureUniqueId, const unsigned char* rg int OpenGLGuiHelper::registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId) { + if (textureId == -2) + { + if (m_data->m_checkedTextureGrey<0) + { + m_data->m_checkedTextureGrey = createCheckeredTexture(192, 192, 192); + } + textureId = m_data->m_checkedTextureGrey; + } + int shapeId = m_data->m_glApp->m_renderer->registerShape(vertices, numvertices,indices,numIndices,primitiveType, textureId); return shapeId; } diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index 07d4fd26b..daa3a0c59 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp @@ -2,7 +2,10 @@ #include "../../ThirdPartyLibs/tinyxml/tinyxml.h" #include "Bullet3Common/b3FileUtils.h" #include "Bullet3Common/b3HashMap.h" - +#include "LinearMath/btQuickprof.h" +#include "BulletCollision/CollisionShapes/btShapeHull.h" +#include "../../CommonInterfaces/CommonRenderInterface.h" +#include "../../CommonInterfaces/CommonGUIHelperInterface.h" #include #include "../../Utils/b3ResourcePath.h" #include @@ -13,6 +16,9 @@ #include "../ImportURDFDemo/urdfLexicalCast.h" #include "../ImportObjDemo/LoadMeshFromObj.h" #include "../ImportSTLDemo/LoadMeshFromSTL.h" +#include "../ImportColladaDemo/LoadMeshFromCollada.h" +#include "../OpenGLWindow/ShapeData.h" + #include"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h" #include "../ImportMeshUtility/b3ImportMeshUtility.h" @@ -31,6 +37,20 @@ +#define mjcf_sphere_indiced textured_detailed_sphere_indices +#define mjcf_sphere_vertices textured_detailed_sphere_vertices + + + +static btVector4 sGoogleColors[4] = +{ + btVector4(60. / 256., 186. / 256., 84. / 256., 1), + btVector4(244. / 256., 194. / 256., 13. / 256., 1), + btVector4(219. / 256., 50. / 256., 54. / 256., 1), + btVector4(72. / 256., 133. / 256., 237. / 256., 1), +}; + + #include enum ePARENT_LINK_ENUMS @@ -192,12 +212,14 @@ struct BulletMJCFImporterInternalData mutable btAlignedObjectArray m_allocatedMeshInterfaces; int m_flags; + int m_textureId; BulletMJCFImporterInternalData() :m_inertiaFromGeom(true), m_activeModel(-1), m_activeBodyUniqueId(-1), - m_flags(0) + m_flags(0), + m_textureId(-1) { m_pathPrefix[0] = 0; } @@ -705,6 +727,12 @@ struct BulletMJCFImporterInternalData linkPtr->m_contactInfo.m_flags |= URDF_CONTACT_HAS_ROLLING_FRICTION; } + { + geom.m_localMaterial.m_matColor.m_rgbaColor = sGoogleColors[linkIndex & 3]; + geom.m_localMaterial.m_matColor.m_specularColor.setValue(1, 1, 1); + geom.m_hasLocalMaterial = true; + } + std::string rgba = defaults.m_defaultGeomRgba; if (const char* rgbattr = link_xml->Attribute("rgba")) { @@ -713,11 +741,16 @@ struct BulletMJCFImporterInternalData if (!rgba.empty()) { // "0 0.7 0.7 1" - parseVector4(geom.m_localMaterial.m_matColor.m_rgbaColor, rgba); - geom.m_hasLocalMaterial = true; - geom.m_localMaterial.m_name = rgba; + if ((m_flags&CUF_MJCF_COLORS_FROM_FILE)) + { + parseVector4(geom.m_localMaterial.m_matColor.m_rgbaColor, rgba); + geom.m_hasLocalMaterial = true; + geom.m_localMaterial.m_name = rgba; + } } + + const char* posS = link_xml->Attribute("pos"); if (posS) { @@ -880,31 +913,43 @@ struct BulletMJCFImporterInternalData if (handledGeomType) { - UrdfCollision col; - col.m_flags |= URDF_HAS_COLLISION_GROUP; - col.m_collisionGroup = defaults.m_defaultCollisionGroup; - - col.m_flags |= URDF_HAS_COLLISION_MASK; - col.m_collisionMask = defaults.m_defaultCollisionMask; - - //contype, conaffinity - const char* conTypeStr = link_xml->Attribute("contype"); - if (conTypeStr) { + UrdfCollision col; col.m_flags |= URDF_HAS_COLLISION_GROUP; - col.m_collisionGroup = urdfLexicalCast(conTypeStr); - } - const char* conAffinityStr = link_xml->Attribute("conaffinity"); - if (conAffinityStr) - { + col.m_collisionGroup = defaults.m_defaultCollisionGroup; + col.m_flags |= URDF_HAS_COLLISION_MASK; - col.m_collisionMask = urdfLexicalCast(conAffinityStr); + col.m_collisionMask = defaults.m_defaultCollisionMask; + + //contype, conaffinity + const char* conTypeStr = link_xml->Attribute("contype"); + if (conTypeStr) + { + col.m_flags |= URDF_HAS_COLLISION_GROUP; + col.m_collisionGroup = urdfLexicalCast(conTypeStr); + } + const char* conAffinityStr = link_xml->Attribute("conaffinity"); + if (conAffinityStr) + { + col.m_flags |= URDF_HAS_COLLISION_MASK; + col.m_collisionMask = urdfLexicalCast(conAffinityStr); + } + + col.m_geometry = geom; + col.m_linkLocalFrame = linkLocalFrame; + col.m_sourceFileLocation = sourceFileLocation(link_xml); + linkPtr->m_collisionArray.push_back(col); + } + { + UrdfVisual vis; + vis.m_geometry = geom; + vis.m_linkLocalFrame = linkLocalFrame; + vis.m_sourceFileLocation = sourceFileLocation(link_xml); + linkPtr->m_visualArray.push_back(vis); + } - col.m_geometry = geom; - col.m_linkLocalFrame = linkLocalFrame; - col.m_sourceFileLocation = sourceFileLocation(link_xml); - linkPtr->m_collisionArray.push_back(col); + } else { @@ -1380,6 +1425,7 @@ BulletMJCFImporter::BulletMJCFImporter(struct GUIHelperInterface* helper, LinkVi m_data->m_guiHelper = helper; m_data->m_customVisualShapesConverter = customConverter; m_data->m_flags = flags; + m_data->m_textureId = -1; } BulletMJCFImporter::~BulletMJCFImporter() @@ -1525,6 +1571,49 @@ std::string BulletMJCFImporter::getBodyName() const return m_data->m_fileModelName; } + + +bool BulletMJCFImporter::getLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const +{ + bool hasLinkColor = false; + { + const UrdfLink* link = m_data->getLink(m_data->m_activeModel, linkIndex); + if (link) + { + for (int i = 0; i < link->m_visualArray.size(); i++) + { + if (link->m_visualArray[i].m_geometry.m_hasLocalMaterial) + { + matCol = link->m_visualArray[i].m_geometry.m_localMaterial.m_matColor; + hasLinkColor = true; + break; + } + } + + if (!hasLinkColor) + { + for (int i = 0; i < link->m_collisionArray.size(); i++) + { + if (link->m_collisionArray[i].m_geometry.m_hasLocalMaterial) + { + matCol = link->m_collisionArray[0].m_geometry.m_localMaterial.m_matColor; + hasLinkColor = true; + } + break; + } + } + } + } + + if (!hasLinkColor) + { + matCol.m_rgbaColor = sGoogleColors[linkIndex & 3]; + matCol.m_specularColor.setValue(1, 1, 1); + hasLinkColor = true; + } + return hasLinkColor; +} + bool BulletMJCFImporter::getLinkColor(int linkIndex, btVector4& colorRGBA) const { // UrdfLink* link = m_data->getLink(linkIndex); @@ -1664,9 +1753,512 @@ bool BulletMJCFImporter::getRootTransformInWorld(btTransform& rootTransformInWor return true; } +void BulletMJCFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut) const +{ + + + GLInstanceGraphicsShape* glmesh = 0; + int strideInBytes = 9 * sizeof(float); + + btConvexShape* convexColShape = 0; + + switch (visual->m_geometry.m_type) + { + case URDF_GEOM_CAPSULE: + { + +#if 1 + + btScalar height = visual->m_geometry.m_capsuleHeight; + + btTransform capsuleTrans; + capsuleTrans.setIdentity(); + if (visual->m_geometry.m_hasFromTo) + { + btVector3 f = visual->m_geometry.m_capsuleFrom; + btVector3 t = visual->m_geometry.m_capsuleTo; + + //compute the local 'fromto' transform + btVector3 localPosition = btScalar(0.5)*(t + f); + btQuaternion localOrn; + localOrn = btQuaternion::getIdentity(); + + btVector3 diff = t - f; + btScalar lenSqr = diff.length2(); + height = 0.f; + + if (lenSqr > SIMD_EPSILON) + { + height = btSqrt(lenSqr); + btVector3 ax = diff / height; + + btVector3 zAxis(0, 0, 1); + localOrn = shortestArcQuat(zAxis, ax); + } + capsuleTrans.setOrigin(localPosition); + capsuleTrans.setRotation(localOrn); + } + + btScalar diam = 2.*visual->m_geometry.m_capsuleRadius; + b3AlignedObjectArray transformedVertices; + int numVertices = sizeof(mjcf_sphere_vertices) / strideInBytes; + transformedVertices.resize(numVertices); + for (int i = 0; i0) + trVer[up] += halfHeight; + else + trVer[up] -= halfHeight; + + trVer = capsuleTrans*trVer; + + transformedVertices[i].xyzw[0] = trVer[0]; + transformedVertices[i].xyzw[1] = trVer[1]; + transformedVertices[i].xyzw[2] = trVer[2]; + transformedVertices[i].xyzw[3] = 0; + transformedVertices[i].normal[0] = mjcf_sphere_vertices[i * 9 + 4]; + transformedVertices[i].normal[1] = mjcf_sphere_vertices[i * 9 + 5]; + transformedVertices[i].normal[2] = mjcf_sphere_vertices[i * 9 + 6]; + //transformedVertices[i].uv[0] = mjcf_sphere_vertices[i * 9 + 7]; + //transformedVertices[i].uv[1] = mjcf_sphere_vertices[i * 9 + 8]; + + btScalar u = btAtan2(transformedVertices[i].normal[0], transformedVertices[i].normal[2]) / (2 * SIMD_PI) + 0.5; + btScalar v = transformedVertices[i].normal[1] * 0.5 + 0.5; + transformedVertices[i].uv[0] = u; + transformedVertices[i].uv[1] = v; + } + + glmesh = new GLInstanceGraphicsShape; + // int index = 0; + glmesh->m_indices = new b3AlignedObjectArray(); + glmesh->m_vertices = new b3AlignedObjectArray(); + + int numIndices = sizeof(mjcf_sphere_indiced) / sizeof(int); + for (int i = 0; i < numIndices; i++) + { + glmesh->m_indices->push_back(mjcf_sphere_indiced[i]); + } + for (int i = 0; i < transformedVertices.size(); i++) + { + glmesh->m_vertices->push_back(transformedVertices[i]); + } + glmesh->m_numIndices = numIndices; + glmesh->m_numvertices = transformedVertices.size(); + glmesh->m_scaling[0] = 1; + glmesh->m_scaling[1] = 1; + glmesh->m_scaling[2] = 1; + glmesh->m_scaling[3] = 1; +#else + if (visual->m_geometry.m_hasFromTo) + { + btVector3 f = visual->m_geometry.m_capsuleFrom; + btVector3 t = visual->m_geometry.m_capsuleTo; + btVector3 fromto[2] = { f, t }; + btScalar radii[2] = { btScalar(visual->m_geometry.m_capsuleRadius) + , btScalar(visual->m_geometry.m_capsuleRadius) }; + + btMultiSphereShape* ms = new btMultiSphereShape(fromto, radii, 2); + convexColShape = ms; + } + else + { + btCapsuleShapeZ* cap = new btCapsuleShapeZ(visual->m_geometry.m_capsuleRadius, + visual->m_geometry.m_capsuleHeight); + convexColShape = cap; + } +#endif + + break; + } + + case URDF_GEOM_CYLINDER: + { + btAlignedObjectArray vertices; + + //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); + int numSteps = 32; + for (int i = 0; im_geometry.m_capsuleRadius; + btScalar cylLength = visual->m_geometry.m_capsuleHeight; + + 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(m_data->m_globalDefaults.m_defaultCollisionMargin); + cylZShape->recalcLocalAabb(); + 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(m_data->m_globalDefaults.m_defaultCollisionMargin); + break; + } + + case URDF_GEOM_SPHERE: + { +#if 1 + btScalar sphereSize = 2.*visual->m_geometry.m_sphereRadius; + b3AlignedObjectArray transformedVertices; + int numVertices = sizeof(mjcf_sphere_vertices) / strideInBytes; + transformedVertices.resize(numVertices); + + + glmesh = new GLInstanceGraphicsShape; + // int index = 0; + glmesh->m_indices = new b3AlignedObjectArray(); + glmesh->m_vertices = new b3AlignedObjectArray(); + printf("vertices:\n"); + for (int i = 0; im_indices->push_back(mjcf_sphere_indiced[i]); + } + for (int i = 0; i < transformedVertices.size(); i++) + { + glmesh->m_vertices->push_back(transformedVertices[i]); + } + glmesh->m_numIndices = numIndices; + glmesh->m_numvertices = transformedVertices.size(); + glmesh->m_scaling[0] = 1; + glmesh->m_scaling[1] = 1; + glmesh->m_scaling[2] = 1; + glmesh->m_scaling[3] = 1; + +#else + + btScalar radius = visual->m_geometry.m_sphereRadius; + btSphereShape* sphereShape = new btSphereShape(radius); + convexColShape = sphereShape; + convexColShape->setMargin(m_data->m_globalDefaults.m_defaultCollisionMargin); +#endif + break; + } + + case URDF_GEOM_MESH: + { + switch (visual->m_geometry.m_meshFileType) + { + case UrdfGeometry::FILE_OBJ: + { + b3ImportMeshData meshData; + if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData)) + { + + if (meshData.m_textureImage1) + { + MJCFURDFTexture texData; + texData.m_width = meshData.m_textureWidth; + texData.m_height = meshData.m_textureHeight; + texData.textureData1 = meshData.m_textureImage1; + texData.m_isCached = meshData.m_isCached; + texturesOut.push_back(texData); + } + glmesh = meshData.m_gfxShape; + } + break; + } + + case UrdfGeometry::FILE_STL: + { + glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str()); + break; + } + + case UrdfGeometry::FILE_COLLADA: + { + btAlignedObjectArray visualShapes; + btAlignedObjectArray visualShapeInstances; + btTransform upAxisTrans; upAxisTrans.setIdentity(); + float unitMeterScaling = 1; + int upAxis = 2; + + LoadMeshFromCollada(visual->m_geometry.m_meshFileName.c_str(), + visualShapes, + visualShapeInstances, + upAxisTrans, + unitMeterScaling, + upAxis); + + glmesh = new GLInstanceGraphicsShape; + // int index = 0; + glmesh->m_indices = new b3AlignedObjectArray(); + glmesh->m_vertices = new b3AlignedObjectArray(); + + for (int i = 0; im_shapeIndex]; + + b3AlignedObjectArray verts; + verts.resize(gfxShape->m_vertices->size()); + + int baseIndex = glmesh->m_vertices->size(); + + for (int i = 0; im_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; km_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; vm_vertices->push_back(verts[v]); + } + } + glmesh->m_numIndices = glmesh->m_indices->size(); + glmesh->m_numvertices = glmesh->m_vertices->size(); + //glmesh = LoadMeshFromCollada(visual->m_geometry.m_meshFileName); + + break; + } + } // switch file type + + if (!glmesh || !glmesh->m_vertices || glmesh->m_numvertices <= 0) + { + b3Warning("%s: cannot extract anything useful from mesh '%s'\n", urdfPathPrefix, visual->m_geometry.m_meshFileName.c_str()); + break; + } + + //apply the geometry scaling + for (int i = 0; im_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]; + } + break; + } + case URDF_GEOM_PLANE: + { + b3Warning("No default visual for URDF_GEOM_PLANE"); + break; + } + default: + { + b3Warning("Error: unknown visual geometry type %i\n", visual->m_geometry.m_type); + } + } + + //if we have a convex, tesselate into localVertices/localIndices + if ((glmesh == 0) && convexColShape) + { + BT_PROFILE("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(); + glmesh->m_vertices = new b3AlignedObjectArray(); + + + + + 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; + btVector3 normal = pos.normalized(); + vtx.normal[0] = normal.x(); + vtx.normal[1] = normal.y(); + vtx.normal[2] = normal.z(); + btScalar u = btAtan2(normal[0], normal[2]) / (2 * SIMD_PI) + 0.5; + btScalar v = normal[1] * 0.5 + 0.5; + vtx.uv[0] = u; + vtx.uv[1] = v; + glmesh->m_vertices->push_back(vtx); + } + + btAlignedObjectArray 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) + { + BT_PROFILE("glmesh"); + 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; +} + int BulletMJCFImporter::convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const { - return -1; + int graphicsIndex = -1; + if (m_data->m_flags&CUF_MJCF_COLORS_FROM_FILE) + { + btAlignedObjectArray vertices; + btAlignedObjectArray indices; + btTransform startTrans; startTrans.setIdentity(); + btAlignedObjectArray textures; + + const UrdfModel& model = *m_data->m_models[m_data->m_activeModel]; + 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]; + + convertURDFToVisualShapeInternal(&vis, pathPrefix, inertialFrame.inverse()*childTrans, vertices, indices, textures); + + } + } + if (vertices.size() && indices.size()) + { + if (1) + { + int textureIndex = -2; + if (textures.size()) + { + textureIndex = m_data->m_guiHelper->registerTexture(textures[0].textureData1, textures[0].m_width, textures[0].m_height); + } + { + B3_PROFILE("registerGraphicsShape"); + graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, textureIndex); + } + + } + } + + //delete textures + for (int i = 0; i < textures.size(); i++) + { + B3_PROFILE("free textureData"); + if (!textures[i].m_isCached) + { + free(textures[i].textureData1); + } + } + } + return graphicsIndex; } bool BulletMJCFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h index 39be50898..fa3422bd6 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h @@ -12,12 +12,20 @@ struct MJCFErrorLogger virtual void printMessage(const char* msg)=0; }; - +struct MJCFURDFTexture +{ + int m_width; + int m_height; + unsigned char* textureData1; + bool m_isCached; +}; class BulletMJCFImporter : public URDFImporterInterface { struct BulletMJCFImporterInternalData* m_data; + void convertURDFToVisualShapeInternal(const struct UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut) const; + public: BulletMJCFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter, int flags); virtual ~BulletMJCFImporter(); @@ -46,6 +54,7 @@ public: /// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const; + bool getLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const; //optional method to get collision group (type) and mask (affinity) virtual int getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const ; diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.h b/examples/Importers/ImportURDFDemo/URDF2Bullet.h index abc4f3573..a3a79be1e 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.h +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.h @@ -25,6 +25,7 @@ enum ConvertURDFFlags { CUF_RESERVED=64, CUF_USE_IMPLICIT_CYLINDER=128, CUF_GLOBAL_VELOCITIES_MB=256, + CUF_MJCF_COLORS_FROM_FILE=512, }; void ConvertURDF2Bullet(const URDFImporterInterface& u2b, diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 05a81c8c1..dd7d31db8 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -674,6 +674,7 @@ enum eURDF_Flags URDF_RESERVED=64, URDF_USE_IMPLICIT_CYLINDER =128, URDF_GLOBAL_VELOCITIES_MB =256, + MJCF_COLORS_FROM_FILE=512, }; enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index d0a9c321a..aa7756416 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -470,12 +470,14 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi 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; + btVector3 normal = pos.safeNormalize(); + vtx.normal[0] = normal.x(); + vtx.normal[1] = normal.y(); + vtx.normal[2] = normal.z(); + btScalar u = btAtan2(normal[0], normal[2]) / (2 * SIMD_PI) + 0.5; + btScalar v = normal[1] * 0.5 + 0.5; + vtx.uv[0] = u; + vtx.uv[1] = v; glmesh->m_vertices->push_back(vtx); } @@ -586,18 +588,33 @@ void TinyRendererVisualShapeConverter::convertVisualShapes( { color.setValue(1,1,1,1); } - if (model && useVisual) + if (model) { - btHashString matName(linkPtr->m_visualArray[v1].m_materialName.c_str()); - UrdfMaterial*const* matPtr = model->m_materials[matName]; - if (matPtr) + if (useVisual) { - for (int i=0; i<4; i++) + btHashString matName(linkPtr->m_visualArray[v1].m_materialName.c_str()); + UrdfMaterial*const* matPtr = model->m_materials[matName]; + if (matPtr) { - rgbaColor[i] = (*matPtr)->m_matColor.m_rgbaColor[i]; + for (int i = 0; i < 4; i++) + { + rgbaColor[i] = (*matPtr)->m_matColor.m_rgbaColor[i]; + } + //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); + } + } + + } + else + { + + if (vis && vis->m_geometry.m_hasLocalMaterial) + { + for (int i = 0; i < 4; i++) + { + rgbaColor[i] = vis->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[i]; } - //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); } } diff --git a/examples/pybullet/examples/quadruped.py b/examples/pybullet/examples/quadruped.py index dfdd65892..c325ff22b 100644 --- a/examples/pybullet/examples/quadruped.py +++ b/examples/pybullet/examples/quadruped.py @@ -4,7 +4,10 @@ import math def drawInertiaBox(parentUid, parentLinkIndex, color): - mass,frictionCoeff, inertia =p.getDynamicsInfo(bodyUniqueId=parentUid,linkIndex=parentLinkIndex, flags = p.DYNAMICS_INFO_REPORT_INERTIA) + dyn = p.getDynamicsInfo(parentUid, parentLinkIndex) + mass=dyn[0] + frictionCoeff=dyn[1] + inertia = dyn[2] if (mass>0): Ixx = inertia[0] Iyy = inertia[1] @@ -79,7 +82,7 @@ p.setTimeStep(fixedTimeStep) orn = p.getQuaternionFromEuler([0,0,0.4]) p.setRealTimeSimulation(0) -quadruped = p.loadURDF("quadruped/minitaur_v1.urdf",[1,-1,.3],orn,useFixedBase=False, useMaximalCoordinates=useMaximalCoordinates) +quadruped = p.loadURDF("quadruped/minitaur_v1.urdf",[1,-1,.3],orn,useFixedBase=False, useMaximalCoordinates=useMaximalCoordinates, flags=p.URDF_USE_IMPLICIT_CYLINDER) nJoints = p.getNumJoints(quadruped) jointNameToId = {} @@ -123,7 +126,11 @@ halfpi = 1.57079632679 twopi = 4*halfpi kneeangle = -2.1834 -mass, friction, localInertiaDiagonal = p.getDynamicsInfo(quadruped,-1, flags=p.DYNAMICS_INFO_REPORT_INERTIA ) +dyn = p.getDynamicsInfo(quadruped,-1) +mass=dyn[0] +friction=dyn[1] +localInertiaDiagonal = dyn[2] + print("localInertiaDiagonal",localInertiaDiagonal) #this is a no-op, just to show the API diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index c719bda5c..6ca7252ae 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -526,6 +526,33 @@ void b3pybulletExitFunc(void) } } +static PyObject* pybullet_isConnected(PyObject* self, PyObject* args, PyObject* keywds) +{ + int physicsClientId = 0; + int isConnected = 0; + int method = 0; + PyObject* pylist = 0; + PyObject* val = 0; + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = { "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm != 0) + { + if (b3CanSubmitCommand(sm)) + { + isConnected = 1; + method = sPhysicsClientsGUI[physicsClientId]; + } + } + + return PyLong_FromLong(isConnected); +} + + static PyObject* pybullet_getConnectionInfo(PyObject* self, PyObject* args, PyObject* keywds) { @@ -8249,6 +8276,10 @@ static PyMethodDef SpamMethods[] = { "getConnectionInfo(physicsClientId=0)\n" "Return if a given client id is connected, and using what method."}, + { "isConnected", (PyCFunction)pybullet_isConnected, METH_VARARGS | METH_KEYWORDS, + "isConnected(physicsClientId=0)\n" + "Return if a given client id is connected." }, + {"resetSimulation", (PyCFunction)pybullet_resetSimulation, METH_VARARGS | METH_KEYWORDS, "resetSimulation(physicsClientId=0)\n" "Reset the simulation: remove all objects and start from an empty world."}, @@ -8818,7 +8849,8 @@ initpybullet(void) PyModule_AddIntConstant(m, "URDF_USE_INERTIA_FROM_FILE", URDF_USE_INERTIA_FROM_FILE); PyModule_AddIntConstant(m, "URDF_USE_IMPLICIT_CYLINDER", URDF_USE_IMPLICIT_CYLINDER); PyModule_AddIntConstant(m, "URDF_GLOBAL_VELOCITIES_MB", URDF_GLOBAL_VELOCITIES_MB); - + PyModule_AddIntConstant(m, "MJCF_COLORS_FROM_FILE", MJCF_COLORS_FROM_FILE); + PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION", URDF_USE_SELF_COLLISION); PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_PARENT", URDF_USE_SELF_COLLISION_EXCLUDE_PARENT); PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS", URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS); diff --git a/src/BulletCollision/CollisionShapes/btShapeHull.cpp b/src/BulletCollision/CollisionShapes/btShapeHull.cpp index 3beaf8658..9f2686903 100644 --- a/src/BulletCollision/CollisionShapes/btShapeHull.cpp +++ b/src/BulletCollision/CollisionShapes/btShapeHull.cpp @@ -20,6 +20,8 @@ subject to the following restrictions: #include "LinearMath/btConvexHull.h" #define NUM_UNITSPHERE_POINTS 42 +#define NUM_UNITSPHERE_POINTS_HIGHRES 256 + btShapeHull::btShapeHull (const btConvexShape* shape) { @@ -36,9 +38,9 @@ btShapeHull::~btShapeHull () } bool -btShapeHull::buildHull (btScalar /*margin*/) +btShapeHull::buildHull (btScalar /*margin*/, int highres) { - int numSampleDirections = NUM_UNITSPHERE_POINTS; + int numSampleDirections = highres? NUM_UNITSPHERE_POINTS_HIGHRES:NUM_UNITSPHERE_POINTS; { int numPDA = m_shape->getNumPreferredPenetrationDirections(); if (numPDA) @@ -53,11 +55,11 @@ btShapeHull::buildHull (btScalar /*margin*/) } } - btVector3 supportPoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; + btVector3 supportPoints[NUM_UNITSPHERE_POINTS_HIGHRES+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; int i; for (i = 0; i < numSampleDirections; i++) { - supportPoints[i] = m_shape->localGetSupportingVertex(getUnitSpherePoints()[i]); + supportPoints[i] = m_shape->localGetSupportingVertex(getUnitSpherePoints(highres)[i]); } HullDesc hd; @@ -118,9 +120,268 @@ btShapeHull::numIndices () const } -btVector3* btShapeHull::getUnitSpherePoints() +btVector3* btShapeHull::getUnitSpherePoints(int highres) { - static btVector3 sUnitSpherePoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = + static btVector3 sUnitSpherePointsHighres[NUM_UNITSPHERE_POINTS_HIGHRES + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2] = + { + btVector3(btScalar(0.997604), btScalar(0.067004), btScalar(0.017144)), + btVector3(btScalar(0.984139), btScalar(-0.086784), btScalar(-0.154427)), + btVector3(btScalar(0.971065), btScalar(0.124164), btScalar(-0.203224)), + btVector3(btScalar(0.955844), btScalar(0.291173), btScalar(-0.037704)), + btVector3(btScalar(0.957405), btScalar(0.212238), btScalar(0.195157)), + btVector3(btScalar(0.971650), btScalar(-0.012709), btScalar(0.235561)), + btVector3(btScalar(0.984920), btScalar(-0.161831), btScalar(0.059695)), + btVector3(btScalar(0.946673), btScalar(-0.299288), btScalar(-0.117536)), + btVector3(btScalar(0.922670), btScalar(-0.219186), btScalar(-0.317019)), + btVector3(btScalar(0.928134), btScalar(-0.007265), btScalar(-0.371867)), + btVector3(btScalar(0.875642), btScalar(0.198434), btScalar(-0.439988)), + btVector3(btScalar(0.908035), btScalar(0.325975), btScalar(-0.262562)), + btVector3(btScalar(0.864519), btScalar(0.488706), btScalar(-0.116755)), + btVector3(btScalar(0.893009), btScalar(0.428046), btScalar(0.137185)), + btVector3(btScalar(0.857494), btScalar(0.362137), btScalar(0.364776)), + btVector3(btScalar(0.900815), btScalar(0.132524), btScalar(0.412987)), + btVector3(btScalar(0.934964), btScalar(-0.241739), btScalar(0.259179)), + btVector3(btScalar(0.894570), btScalar(-0.103504), btScalar(0.434263)), + btVector3(btScalar(0.922085), btScalar(-0.376668), btScalar(0.086241)), + btVector3(btScalar(0.862177), btScalar(-0.499154), btScalar(-0.085330)), + btVector3(btScalar(0.861982), btScalar(-0.420218), btScalar(-0.282861)), + btVector3(btScalar(0.818076), btScalar(-0.328256), btScalar(-0.471804)), + btVector3(btScalar(0.762657), btScalar(-0.179329), btScalar(-0.621124)), + btVector3(btScalar(0.826857), btScalar(0.019760), btScalar(-0.561786)), + btVector3(btScalar(0.731434), btScalar(0.206599), btScalar(-0.649817)), + btVector3(btScalar(0.769486), btScalar(0.379052), btScalar(-0.513770)), + btVector3(btScalar(0.796806), btScalar(0.507176), btScalar(-0.328145)), + btVector3(btScalar(0.679722), btScalar(0.684101), btScalar(-0.264123)), + btVector3(btScalar(0.786854), btScalar(0.614886), btScalar(0.050912)), + btVector3(btScalar(0.769486), btScalar(0.571141), btScalar(0.285139)), + btVector3(btScalar(0.707432), btScalar(0.492789), btScalar(0.506288)), + btVector3(btScalar(0.774560), btScalar(0.268037), btScalar(0.572652)), + btVector3(btScalar(0.796220), btScalar(0.031230), btScalar(0.604077)), + btVector3(btScalar(0.837395), btScalar(-0.320285), btScalar(0.442461)), + btVector3(btScalar(0.848127), btScalar(-0.450548), btScalar(0.278307)), + btVector3(btScalar(0.775536), btScalar(-0.206354), btScalar(0.596465)), + btVector3(btScalar(0.816320), btScalar(-0.567007), btScalar(0.109469)), + btVector3(btScalar(0.741191), btScalar(-0.668690), btScalar(-0.056832)), + btVector3(btScalar(0.755632), btScalar(-0.602975), btScalar(-0.254949)), + btVector3(btScalar(0.720311), btScalar(-0.521318), btScalar(-0.457165)), + btVector3(btScalar(0.670746), btScalar(-0.386583), btScalar(-0.632835)), + btVector3(btScalar(0.587031), btScalar(-0.219769), btScalar(-0.778836)), + btVector3(btScalar(0.676015), btScalar(-0.003182), btScalar(-0.736676)), + btVector3(btScalar(0.566932), btScalar(0.186963), btScalar(-0.802064)), + btVector3(btScalar(0.618254), btScalar(0.398105), btScalar(-0.677533)), + btVector3(btScalar(0.653964), btScalar(0.575224), btScalar(-0.490933)), + btVector3(btScalar(0.525367), btScalar(0.743205), btScalar(-0.414028)), + btVector3(btScalar(0.506439), btScalar(0.836528), btScalar(-0.208885)), + btVector3(btScalar(0.651427), btScalar(0.756426), btScalar(-0.056247)), + btVector3(btScalar(0.641670), btScalar(0.745149), btScalar(0.180908)), + btVector3(btScalar(0.602643), btScalar(0.687211), btScalar(0.405180)), + btVector3(btScalar(0.516586), btScalar(0.596999), btScalar(0.613447)), + btVector3(btScalar(0.602252), btScalar(0.387801), btScalar(0.697573)), + btVector3(btScalar(0.646549), btScalar(0.153911), btScalar(0.746956)), + btVector3(btScalar(0.650842), btScalar(-0.087756), btScalar(0.753983)), + btVector3(btScalar(0.740411), btScalar(-0.497404), btScalar(0.451830)), + btVector3(btScalar(0.726946), btScalar(-0.619890), btScalar(0.295093)), + btVector3(btScalar(0.637768), btScalar(-0.313092), btScalar(0.703624)), + btVector3(btScalar(0.678942), btScalar(-0.722934), btScalar(0.126645)), + btVector3(btScalar(0.489072), btScalar(-0.867195), btScalar(-0.092942)), + btVector3(btScalar(0.622742), btScalar(-0.757541), btScalar(-0.194636)), + btVector3(btScalar(0.596788), btScalar(-0.693576), btScalar(-0.403098)), + btVector3(btScalar(0.550150), btScalar(-0.582172), btScalar(-0.598287)), + btVector3(btScalar(0.474436), btScalar(-0.429745), btScalar(-0.768101)), + btVector3(btScalar(0.372574), btScalar(-0.246016), btScalar(-0.894583)), + btVector3(btScalar(0.480095), btScalar(-0.026513), btScalar(-0.876626)), + btVector3(btScalar(0.352474), btScalar(0.177242), btScalar(-0.918787)), + btVector3(btScalar(0.441848), btScalar(0.374386), btScalar(-0.814946)), + btVector3(btScalar(0.492389), btScalar(0.582223), btScalar(-0.646693)), + btVector3(btScalar(0.343498), btScalar(0.866080), btScalar(-0.362693)), + btVector3(btScalar(0.362036), btScalar(0.745149), btScalar(-0.559639)), + btVector3(btScalar(0.334131), btScalar(0.937044), btScalar(-0.099774)), + btVector3(btScalar(0.486925), btScalar(0.871718), btScalar(0.052473)), + btVector3(btScalar(0.452776), btScalar(0.845665), btScalar(0.281820)), + btVector3(btScalar(0.399503), btScalar(0.771785), btScalar(0.494576)), + btVector3(btScalar(0.296469), btScalar(0.673018), btScalar(0.677469)), + btVector3(btScalar(0.392088), btScalar(0.479179), btScalar(0.785213)), + btVector3(btScalar(0.452190), btScalar(0.252094), btScalar(0.855286)), + btVector3(btScalar(0.478339), btScalar(0.013149), btScalar(0.877928)), + btVector3(btScalar(0.481656), btScalar(-0.219380), btScalar(0.848259)), + btVector3(btScalar(0.615327), btScalar(-0.494293), btScalar(0.613837)), + btVector3(btScalar(0.594642), btScalar(-0.650414), btScalar(0.472325)), + btVector3(btScalar(0.562249), btScalar(-0.771345), btScalar(0.297631)), + btVector3(btScalar(0.467411), btScalar(-0.437133), btScalar(0.768231)), + btVector3(btScalar(0.519513), btScalar(-0.847947), btScalar(0.103808)), + btVector3(btScalar(0.297640), btScalar(-0.938159), btScalar(-0.176288)), + btVector3(btScalar(0.446727), btScalar(-0.838615), btScalar(-0.311359)), + btVector3(btScalar(0.331790), btScalar(-0.942437), btScalar(0.040762)), + btVector3(btScalar(0.413358), btScalar(-0.748403), btScalar(-0.518259)), + btVector3(btScalar(0.347596), btScalar(-0.621640), btScalar(-0.701737)), + btVector3(btScalar(0.249831), btScalar(-0.456186), btScalar(-0.853984)), + btVector3(btScalar(0.131772), btScalar(-0.262931), btScalar(-0.955678)), + btVector3(btScalar(0.247099), btScalar(-0.042261), btScalar(-0.967975)), + btVector3(btScalar(0.113624), btScalar(0.165965), btScalar(-0.979491)), + btVector3(btScalar(0.217438), btScalar(0.374580), btScalar(-0.901220)), + btVector3(btScalar(0.307983), btScalar(0.554615), btScalar(-0.772786)), + btVector3(btScalar(0.166702), btScalar(0.953181), btScalar(-0.252021)), + btVector3(btScalar(0.172751), btScalar(0.844499), btScalar(-0.506743)), + btVector3(btScalar(0.177630), btScalar(0.711125), btScalar(-0.679876)), + btVector3(btScalar(0.120064), btScalar(0.992260), btScalar(-0.030482)), + btVector3(btScalar(0.289640), btScalar(0.949098), btScalar(0.122546)), + btVector3(btScalar(0.239879), btScalar(0.909047), btScalar(0.340377)), + btVector3(btScalar(0.181142), btScalar(0.821363), btScalar(0.540641)), + btVector3(btScalar(0.066986), btScalar(0.719097), btScalar(0.691327)), + btVector3(btScalar(0.156750), btScalar(0.545478), btScalar(0.823079)), + btVector3(btScalar(0.236172), btScalar(0.342306), btScalar(0.909353)), + btVector3(btScalar(0.277541), btScalar(0.112693), btScalar(0.953856)), + btVector3(btScalar(0.295299), btScalar(-0.121974), btScalar(0.947415)), + btVector3(btScalar(0.287883), btScalar(-0.349254), btScalar(0.891591)), + btVector3(btScalar(0.437165), btScalar(-0.634666), btScalar(0.636869)), + btVector3(btScalar(0.407113), btScalar(-0.784954), btScalar(0.466664)), + btVector3(btScalar(0.375111), btScalar(-0.888193), btScalar(0.264839)), + btVector3(btScalar(0.275394), btScalar(-0.560591), btScalar(0.780723)), + btVector3(btScalar(0.122015), btScalar(-0.992209), btScalar(-0.024821)), + btVector3(btScalar(0.087866), btScalar(-0.966156), btScalar(-0.241676)), + btVector3(btScalar(0.239489), btScalar(-0.885665), btScalar(-0.397437)), + btVector3(btScalar(0.167287), btScalar(-0.965184), btScalar(0.200817)), + btVector3(btScalar(0.201632), btScalar(-0.776789), btScalar(-0.596335)), + btVector3(btScalar(0.122015), btScalar(-0.637971), btScalar(-0.760098)), + btVector3(btScalar(0.008054), btScalar(-0.464741), btScalar(-0.885214)), + btVector3(btScalar(-0.116054), btScalar(-0.271096), btScalar(-0.955482)), + btVector3(btScalar(-0.000727), btScalar(-0.056065), btScalar(-0.998424)), + btVector3(btScalar(-0.134007), btScalar(0.152939), btScalar(-0.978905)), + btVector3(btScalar(-0.025900), btScalar(0.366026), btScalar(-0.930108)), + btVector3(btScalar(0.081231), btScalar(0.557337), btScalar(-0.826072)), + btVector3(btScalar(-0.002874), btScalar(0.917213), btScalar(-0.398023)), + btVector3(btScalar(-0.050683), btScalar(0.981761), btScalar(-0.182534)), + btVector3(btScalar(-0.040536), btScalar(0.710153), btScalar(-0.702713)), + btVector3(btScalar(-0.139081), btScalar(0.827973), btScalar(-0.543048)), + btVector3(btScalar(-0.101029), btScalar(0.994010), btScalar(0.041152)), + btVector3(btScalar(0.069328), btScalar(0.978067), btScalar(0.196133)), + btVector3(btScalar(0.023860), btScalar(0.911380), btScalar(0.410645)), + btVector3(btScalar(-0.153521), btScalar(0.736789), btScalar(0.658145)), + btVector3(btScalar(-0.070002), btScalar(0.591750), btScalar(0.802780)), + btVector3(btScalar(0.002590), btScalar(0.312948), btScalar(0.949562)), + btVector3(btScalar(0.090988), btScalar(-0.020680), btScalar(0.995627)), + btVector3(btScalar(0.088842), btScalar(-0.250099), btScalar(0.964006)), + btVector3(btScalar(0.083378), btScalar(-0.470185), btScalar(0.878318)), + btVector3(btScalar(0.240074), btScalar(-0.749764), btScalar(0.616374)), + btVector3(btScalar(0.210803), btScalar(-0.885860), btScalar(0.412987)), + btVector3(btScalar(0.077524), btScalar(-0.660524), btScalar(0.746565)), + btVector3(btScalar(-0.096736), btScalar(-0.990070), btScalar(-0.100945)), + btVector3(btScalar(-0.052634), btScalar(-0.990264), btScalar(0.127426)), + btVector3(btScalar(-0.106102), btScalar(-0.938354), btScalar(-0.328340)), + btVector3(btScalar(0.013323), btScalar(-0.863112), btScalar(-0.504596)), + btVector3(btScalar(-0.002093), btScalar(-0.936993), btScalar(0.349161)), + btVector3(btScalar(-0.106297), btScalar(-0.636610), btScalar(-0.763612)), + btVector3(btScalar(-0.229430), btScalar(-0.463769), btScalar(-0.855546)), + btVector3(btScalar(-0.245236), btScalar(-0.066175), btScalar(-0.966999)), + btVector3(btScalar(-0.351587), btScalar(-0.270513), btScalar(-0.896145)), + btVector3(btScalar(-0.370906), btScalar(0.133108), btScalar(-0.918982)), + btVector3(btScalar(-0.264360), btScalar(0.346000), btScalar(-0.900049)), + btVector3(btScalar(-0.151375), btScalar(0.543728), btScalar(-0.825291)), + btVector3(btScalar(-0.218697), btScalar(0.912741), btScalar(-0.344346)), + btVector3(btScalar(-0.274507), btScalar(0.953764), btScalar(-0.121635)), + btVector3(btScalar(-0.259677), btScalar(0.692266), btScalar(-0.673044)), + btVector3(btScalar(-0.350416), btScalar(0.798810), btScalar(-0.488786)), + btVector3(btScalar(-0.320170), btScalar(0.941127), btScalar(0.108297)), + btVector3(btScalar(-0.147667), btScalar(0.952792), btScalar(0.265034)), + btVector3(btScalar(-0.188061), btScalar(0.860636), btScalar(0.472910)), + btVector3(btScalar(-0.370906), btScalar(0.739900), btScalar(0.560941)), + btVector3(btScalar(-0.297143), btScalar(0.585334), btScalar(0.754178)), + btVector3(btScalar(-0.189622), btScalar(0.428241), btScalar(0.883393)), + btVector3(btScalar(-0.091272), btScalar(0.098695), btScalar(0.990747)), + btVector3(btScalar(-0.256945), btScalar(0.228375), btScalar(0.938827)), + btVector3(btScalar(-0.111761), btScalar(-0.133251), btScalar(0.984696)), + btVector3(btScalar(-0.118006), btScalar(-0.356253), btScalar(0.926725)), + btVector3(btScalar(-0.119372), btScalar(-0.563896), btScalar(0.817029)), + btVector3(btScalar(0.041228), btScalar(-0.833949), btScalar(0.550010)), + btVector3(btScalar(-0.121909), btScalar(-0.736543), btScalar(0.665172)), + btVector3(btScalar(-0.307681), btScalar(-0.931160), btScalar(-0.195026)), + btVector3(btScalar(-0.283679), btScalar(-0.957990), btScalar(0.041348)), + btVector3(btScalar(-0.227284), btScalar(-0.935243), btScalar(0.270890)), + btVector3(btScalar(-0.293436), btScalar(-0.858252), btScalar(-0.420860)), + btVector3(btScalar(-0.175767), btScalar(-0.780677), btScalar(-0.599262)), + btVector3(btScalar(-0.170108), btScalar(-0.858835), btScalar(0.482865)), + btVector3(btScalar(-0.332854), btScalar(-0.635055), btScalar(-0.696857)), + btVector3(btScalar(-0.447791), btScalar(-0.445299), btScalar(-0.775128)), + btVector3(btScalar(-0.470622), btScalar(-0.074146), btScalar(-0.879164)), + btVector3(btScalar(-0.639417), btScalar(-0.340505), btScalar(-0.689049)), + btVector3(btScalar(-0.598438), btScalar(0.104722), btScalar(-0.794256)), + btVector3(btScalar(-0.488575), btScalar(0.307699), btScalar(-0.816313)), + btVector3(btScalar(-0.379882), btScalar(0.513592), btScalar(-0.769077)), + btVector3(btScalar(-0.425740), btScalar(0.862775), btScalar(-0.272516)), + btVector3(btScalar(-0.480769), btScalar(0.875412), btScalar(-0.048439)), + btVector3(btScalar(-0.467890), btScalar(0.648716), btScalar(-0.600043)), + btVector3(btScalar(-0.543799), btScalar(0.730956), btScalar(-0.411881)), + btVector3(btScalar(-0.516284), btScalar(0.838277), btScalar(0.174076)), + btVector3(btScalar(-0.353343), btScalar(0.876384), btScalar(0.326519)), + btVector3(btScalar(-0.572875), btScalar(0.614497), btScalar(0.542007)), + btVector3(btScalar(-0.503600), btScalar(0.497261), btScalar(0.706161)), + btVector3(btScalar(-0.530920), btScalar(0.754870), btScalar(0.384685)), + btVector3(btScalar(-0.395884), btScalar(0.366414), btScalar(0.841818)), + btVector3(btScalar(-0.300656), btScalar(0.001678), btScalar(0.953661)), + btVector3(btScalar(-0.461060), btScalar(0.146912), btScalar(0.875000)), + btVector3(btScalar(-0.315486), btScalar(-0.232212), btScalar(0.919893)), + btVector3(btScalar(-0.323682), btScalar(-0.449187), btScalar(0.832644)), + btVector3(btScalar(-0.318999), btScalar(-0.639527), btScalar(0.699134)), + btVector3(btScalar(-0.496771), btScalar(-0.866029), btScalar(-0.055271)), + btVector3(btScalar(-0.496771), btScalar(-0.816257), btScalar(-0.294377)), + btVector3(btScalar(-0.456377), btScalar(-0.869528), btScalar(0.188130)), + btVector3(btScalar(-0.380858), btScalar(-0.827144), btScalar(0.412792)), + btVector3(btScalar(-0.449352), btScalar(-0.727405), btScalar(-0.518259)), + btVector3(btScalar(-0.570533), btScalar(-0.551064), btScalar(-0.608632)), + btVector3(btScalar(-0.656394), btScalar(-0.118280), btScalar(-0.744874)), + btVector3(btScalar(-0.756696), btScalar(-0.438105), btScalar(-0.484882)), + btVector3(btScalar(-0.801773), btScalar(-0.204798), btScalar(-0.561005)), + btVector3(btScalar(-0.785186), btScalar(0.038618), btScalar(-0.617805)), + btVector3(btScalar(-0.709082), btScalar(0.262399), btScalar(-0.654306)), + btVector3(btScalar(-0.583412), btScalar(0.462265), btScalar(-0.667383)), + btVector3(btScalar(-0.616001), btScalar(0.761286), btScalar(-0.201272)), + btVector3(btScalar(-0.660687), btScalar(0.750204), btScalar(0.020072)), + btVector3(btScalar(-0.744987), btScalar(0.435823), btScalar(-0.504791)), + btVector3(btScalar(-0.713765), btScalar(0.605554), btScalar(-0.351373)), + btVector3(btScalar(-0.686251), btScalar(0.687600), btScalar(0.236927)), + btVector3(btScalar(-0.680201), btScalar(0.429407), btScalar(0.593732)), + btVector3(btScalar(-0.733474), btScalar(0.546450), btScalar(0.403814)), + btVector3(btScalar(-0.591023), btScalar(0.292923), btScalar(0.751445)), + btVector3(btScalar(-0.500283), btScalar(-0.080757), btScalar(0.861922)), + btVector3(btScalar(-0.643710), btScalar(0.070115), btScalar(0.761985)), + btVector3(btScalar(-0.506332), btScalar(-0.308425), btScalar(0.805122)), + btVector3(btScalar(-0.503015), btScalar(-0.509847), btScalar(0.697573)), + btVector3(btScalar(-0.482525), btScalar(-0.682105), btScalar(0.549229)), + btVector3(btScalar(-0.680396), btScalar(-0.716323), btScalar(-0.153451)), + btVector3(btScalar(-0.658346), btScalar(-0.746264), btScalar(0.097562)), + btVector3(btScalar(-0.653272), btScalar(-0.646915), btScalar(-0.392948)), + btVector3(btScalar(-0.590828), btScalar(-0.732655), btScalar(0.337645)), + btVector3(btScalar(-0.819140), btScalar(-0.518013), btScalar(-0.246166)), + btVector3(btScalar(-0.900513), btScalar(-0.282178), btScalar(-0.330487)), + btVector3(btScalar(-0.914953), btScalar(-0.028652), btScalar(-0.402122)), + btVector3(btScalar(-0.859924), btScalar(0.220209), btScalar(-0.459898)), + btVector3(btScalar(-0.777185), btScalar(0.613720), btScalar(-0.137836)), + btVector3(btScalar(-0.805285), btScalar(0.586889), btScalar(0.082728)), + btVector3(btScalar(-0.872413), btScalar(0.406077), btScalar(-0.271735)), + btVector3(btScalar(-0.859339), btScalar(0.448072), btScalar(0.246101)), + btVector3(btScalar(-0.757671), btScalar(0.216320), btScalar(0.615594)), + btVector3(btScalar(-0.826165), btScalar(0.348139), btScalar(0.442851)), + btVector3(btScalar(-0.671810), btScalar(-0.162803), btScalar(0.722557)), + btVector3(btScalar(-0.796504), btScalar(-0.004543), btScalar(0.604468)), + btVector3(btScalar(-0.676298), btScalar(-0.378223), btScalar(0.631794)), + btVector3(btScalar(-0.668883), btScalar(-0.558258), btScalar(0.490673)), + btVector3(btScalar(-0.821287), btScalar(-0.570118), btScalar(0.006994)), + btVector3(btScalar(-0.767428), btScalar(-0.587810), btScalar(0.255470)), + btVector3(btScalar(-0.933296), btScalar(-0.349837), btScalar(-0.079865)), + btVector3(btScalar(-0.982667), btScalar(-0.100393), btScalar(-0.155208)), + btVector3(btScalar(-0.961396), btScalar(0.160910), btScalar(-0.222938)), + btVector3(btScalar(-0.934858), btScalar(0.354555), btScalar(-0.006864)), + btVector3(btScalar(-0.941687), btScalar(0.229736), btScalar(0.245711)), + btVector3(btScalar(-0.884317), btScalar(0.131552), btScalar(0.447536)), + btVector3(btScalar(-0.810359), btScalar(-0.219769), btScalar(0.542788)), + btVector3(btScalar(-0.915929), btScalar(-0.210048), btScalar(0.341743)), + btVector3(btScalar(-0.816799), btScalar(-0.407192), btScalar(0.408303)), + btVector3(btScalar(-0.903050), btScalar(-0.392416), btScalar(0.174076)), + btVector3(btScalar(-0.980325), btScalar(-0.170969), btScalar(0.096586)), + btVector3(btScalar(-0.995936), btScalar(0.084891), btScalar(0.029441)), + btVector3(btScalar(-0.960031), btScalar(0.002650), btScalar(0.279283)), + }; + static btVector3 sUnitSpherePoints[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2] = { btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)), btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)), @@ -165,6 +426,8 @@ btVector3* btShapeHull::getUnitSpherePoints() btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)), btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654)) }; + if (highres) + return sUnitSpherePointsHighres; return sUnitSpherePoints; } diff --git a/src/BulletCollision/CollisionShapes/btShapeHull.h b/src/BulletCollision/CollisionShapes/btShapeHull.h index e959f198b..78ea4b650 100644 --- a/src/BulletCollision/CollisionShapes/btShapeHull.h +++ b/src/BulletCollision/CollisionShapes/btShapeHull.h @@ -34,7 +34,7 @@ protected: unsigned int m_numIndices; const btConvexShape* m_shape; - static btVector3* getUnitSpherePoints(); + static btVector3* getUnitSpherePoints(int highres=0); public: BT_DECLARE_ALIGNED_ALLOCATOR(); @@ -42,7 +42,7 @@ public: btShapeHull (const btConvexShape* shape); ~btShapeHull (); - bool buildHull (btScalar margin); + bool buildHull (btScalar margin, int highres=0); int numTriangles () const; int numVertices () const;