static bool parseVector4(btVector4& vec4, const std::string& vector_str) { vec4.setZero(); btArray<std::string> pieces; btArray<float> rgba; urdfStringSplit(pieces, vector_str, urdfIsAnyOf(" ")); for (int i = 0; i < pieces.size(); ++i) { if (!pieces[i].empty()) { rgba.push_back(urdfLexicalCast<double>(pieces[i].c_str())); } } if (rgba.size() != 4) { return false; } vec4.setValue(rgba[0],rgba[1],rgba[2],rgba[3]); return true; }
osg::Vec4 osgbCollision::asOsgVec4( const btVector4& v ) { return osg::Vec4( v.x(), v.y(), v.z(), v.w() ); }
void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, const btVector3& color,int debugMode,const btVector3& worldBoundsMin,const btVector3& worldBoundsMax, btVector4 bodyColor) { if (shape->getShapeType() == CUSTOM_CONVEX_SHAPE_TYPE) { btVector3 org(m[12], m[13], m[14]); btVector3 dx(m[0], m[1], m[2]); btVector3 dy(m[4], m[5], m[6]); // btVector3 dz(m[8], m[9], m[10]); const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape); btVector3 halfExtent = boxShape->getHalfExtentsWithMargin(); dx *= halfExtent[0]; dy *= halfExtent[1]; // dz *= halfExtent[2]; glColor3f(1,1,1); glDisable(GL_LIGHTING); glLineWidth(2); glBegin(GL_LINE_LOOP); glDrawVector(org - dx - dy); glDrawVector(org - dx + dy); glDrawVector(org + dx + dy); glDrawVector(org + dx - dy); glEnd(); return; } else if((shape->getShapeType() == BOX_SHAPE_PROXYTYPE) && (debugMode & btIDebugDraw::DBG_FastWireframe)) { btVector3 org(m[12], m[13], m[14]); btVector3 dx(m[0], m[1], m[2]); btVector3 dy(m[4], m[5], m[6]); btVector3 dz(m[8], m[9], m[10]); const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape); btVector3 halfExtent = boxShape->getHalfExtentsWithMargin(); dx *= halfExtent[0]; dy *= halfExtent[1]; dz *= halfExtent[2]; glBegin(GL_LINE_LOOP); glDrawVector(org - dx - dy - dz); glDrawVector(org + dx - dy - dz); glDrawVector(org + dx + dy - dz); glDrawVector(org - dx + dy - dz); glDrawVector(org - dx + dy + dz); glDrawVector(org + dx + dy + dz); glDrawVector(org + dx - dy + dz); glDrawVector(org - dx - dy + dz); glEnd(); glBegin(GL_LINES); glDrawVector(org + dx - dy - dz); glDrawVector(org + dx - dy + dz); glDrawVector(org + dx + dy - dz); glDrawVector(org + dx + dy + dz); glDrawVector(org - dx - dy - dz); glDrawVector(org - dx + dy - dz); glDrawVector(org - dx - dy + dz); glDrawVector(org - dx + dy + dz); glEnd(); return; } glPushMatrix(); btglMultMatrix(m); if (shape->getShapeType() == UNIFORM_SCALING_SHAPE_PROXYTYPE) { const btUniformScalingShape* scalingShape = static_cast<const btUniformScalingShape*>(shape); const btConvexShape* convexShape = scalingShape->getChildShape(); float scalingFactor = (float)scalingShape->getUniformScalingFactor(); { btScalar tmpScaling[4][4]={{scalingFactor,0,0,0}, {0,scalingFactor,0,0}, {0,0,scalingFactor,0}, {0,0,0,1}}; drawOpenGL( (btScalar*)tmpScaling,convexShape,color,debugMode,worldBoundsMin,worldBoundsMax); } glPopMatrix(); return; } if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE) { const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape); for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--) { btTransform childTrans = compoundShape->getChildTransform(i); const btCollisionShape* colShape = compoundShape->getChildShape(i); btScalar childMat[16]; childTrans.getOpenGLMatrix(childMat); drawOpenGL(childMat,colShape,color,debugMode,worldBoundsMin,worldBoundsMax); } } else { if(m_textureenabled&&(!m_textureinitialized)) { GLubyte* image=new GLubyte[256*256*3]; for(int y=0;y<256;++y) { const int t=y>>4; GLubyte* pi=image+y*256*3; for(int x=0;x<256;++x) { const int s=x>>4; const GLubyte b=180; GLubyte c=b+((s+t&1)&1)*(255-b); pi[0]=pi[1]=pi[2]=c;pi+=3; } } glGenTextures(1,(GLuint*)&m_texturehandle); glBindTexture(GL_TEXTURE_2D,m_texturehandle); glTexEnvf(GL_TEXTURE_ENV,GL_TEXTURE_ENV_MODE,GL_MODULATE); glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR_MIPMAP_LINEAR); glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_LINEAR_MIPMAP_LINEAR); glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_WRAP_S,GL_REPEAT); glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_WRAP_T,GL_REPEAT); gluBuild2DMipmaps(GL_TEXTURE_2D,3,256,256,GL_RGB,GL_UNSIGNED_BYTE,image); delete[] image; glMatrixMode(GL_TEXTURE); glLoadIdentity(); glScalef(0.025,0.025,0.025); } static const GLfloat planex[]={1,0,0,0}; static const GLfloat planey[]={0,1,0,0}; static const GLfloat planez[]={0,0,1,0}; glTexGenfv(GL_S,GL_OBJECT_PLANE,planex); glTexGenfv(GL_T,GL_OBJECT_PLANE,planez); glTexGeni(GL_S,GL_TEXTURE_GEN_MODE,GL_OBJECT_LINEAR); glTexGeni(GL_T,GL_TEXTURE_GEN_MODE,GL_OBJECT_LINEAR); glEnable(GL_TEXTURE_GEN_S); // glEnable(GL_TEXTURE_GEN_T); // ??? glEnable(GL_TEXTURE_GEN_R); m_textureinitialized=true; //drawCoordSystem(); //glPushMatrix(); glEnable(GL_COLOR_MATERIAL); if(m_textureenabled) { glEnable(GL_TEXTURE_2D); glBindTexture(GL_TEXTURE_2D,m_texturehandle); } else { glDisable(GL_TEXTURE_2D); } //glColor3f(color.x(),color.y(), color.z()); glColor4f(bodyColor.x(), bodyColor.y(), bodyColor.z(), bodyColor.w()); //glColor4f(0.1, 1, 0.1, 0.3); bool useWireframeFallback = true; if (!(debugMode & btIDebugDraw::DBG_DrawWireframe)) { ///you can comment out any of the specific cases, and use the default ///the benefit of 'default' is that it approximates the actual collision shape including collision margin //int shapetype=m_textureenabled?MAX_BROADPHASE_COLLISION_TYPES:shape->getShapeType(); int shapetype=shape->getShapeType(); switch (shapetype) { case SPHERE_SHAPE_PROXYTYPE: { const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape); float radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin drawSphere(radius,10,10); useWireframeFallback = false; break; } case BOX_SHAPE_PROXYTYPE: { const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape); btVector3 halfExtent = boxShape->getHalfExtentsWithMargin(); static int indices[36] = { 0,1,2, 3,2,1, 4,0,6, 6,0,2, 5,1,4, 4,1,0, 7,3,1, 7,1,5, 5,4,7, 7,4,6, 7,2,3, 7,6,2}; static btVector3 vertices[8]={ btVector3(1,1,1),btVector3(-1,1,1), btVector3(1,-1,1), btVector3(-1,-1,1), btVector3(1,1,-1), btVector3(-1,1,-1), btVector3(1,-1,-1), btVector3(-1,-1,-1)}; glBegin (GL_TRIANGLES); int si=36; for (int i=0;i<si;i+=3) { btVector3 v1 = vertices[indices[i]]*halfExtent; btVector3 v2 = vertices[indices[i+1]]*halfExtent; btVector3 v3 = vertices[indices[i+2]]*halfExtent; btVector3 normal = (v3-v1).cross(v2-v1); normal.normalize (); glNormal3f(normal.getX(),normal.getY(),normal.getZ()); glVertex3f (v1.x(), v1.y(), v1.z()); glVertex3f (v2.x(), v2.y(), v2.z()); glVertex3f (v3.x(), v3.y(), v3.z()); } glEnd(); useWireframeFallback = false; break; } #if 0 case CONE_SHAPE_PROXYTYPE: { const btConeShape* coneShape = static_cast<const btConeShape*>(shape); int upIndex = coneShape->getConeUpIndex(); float radius = coneShape->getRadius();//+coneShape->getMargin(); float height = coneShape->getHeight();//+coneShape->getMargin(); switch (upIndex) { case 0: glRotatef(90.0, 0.0, 1.0, 0.0); break; case 1: glRotatef(-90.0, 1.0, 0.0, 0.0); break; case 2: break; default: { } }; glTranslatef(0.0, 0.0, -0.5*height); glutSolidCone(radius,height,10,10); useWireframeFallback = false; break; } #endif case STATIC_PLANE_PROXYTYPE: { const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape); btScalar planeConst = staticPlaneShape->getPlaneConstant(); const btVector3& planeNormal = staticPlaneShape->getPlaneNormal(); btVector3 planeOrigin = planeNormal * planeConst; btVector3 vec0,vec1; btPlaneSpace1(planeNormal,vec0,vec1); btScalar vecLen = 100.f; btVector3 pt0 = planeOrigin + vec0*vecLen; btVector3 pt1 = planeOrigin - vec0*vecLen; btVector3 pt2 = planeOrigin + vec1*vecLen; btVector3 pt3 = planeOrigin - vec1*vecLen; glBegin(GL_LINES); glVertex3f(pt0.getX(),pt0.getY(),pt0.getZ()); glVertex3f(pt1.getX(),pt1.getY(),pt1.getZ()); glVertex3f(pt2.getX(),pt2.getY(),pt2.getZ()); glVertex3f(pt3.getX(),pt3.getY(),pt3.getZ()); glEnd(); break; } /* case CYLINDER_SHAPE_PROXYTYPE: { const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape); int upAxis = cylinder->getUpAxis(); float radius = cylinder->getRadius(); float halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis]; drawCylinder(radius,halfHeight,upAxis); break; } */ case MULTI_SPHERE_SHAPE_PROXYTYPE: { const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape); btTransform childTransform; childTransform.setIdentity(); for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--) { btSphereShape sc(multiSphereShape->getSphereRadius(i)); childTransform.setOrigin(multiSphereShape->getSpherePosition(i)); btScalar childMat[16]; childTransform.getOpenGLMatrix(childMat); drawOpenGL(childMat,&sc,color,debugMode,worldBoundsMin,worldBoundsMax); } break; } default: { if (shape->isConvex()) { ShapeCache* sc=cache((btConvexShape*)shape); #if 0 btConvexShape* convexShape = (btConvexShape*)shape; if (!shape->getUserPointer()) { //create a hull approximation void* mem = btAlignedAlloc(sizeof(btShapeHull),16); btShapeHull* hull = new(mem) btShapeHull(convexShape); ///cleanup memory m_shapeHulls.push_back(hull); btScalar margin = shape->getMargin(); hull->buildHull(margin); convexShape->setUserPointer(hull); // printf("numTriangles = %d\n", hull->numTriangles ()); // printf("numIndices = %d\n", hull->numIndices ()); // printf("numVertices = %d\n", hull->numVertices ()); } #endif //if (shape->getUserPointer()) { //glutSolidCube(1.0); btShapeHull* hull = &sc->m_shapehull/*(btShapeHull*)shape->getUserPointer()*/; if (hull->numTriangles () > 0) { int index = 0; const unsigned int* idx = hull->getIndexPointer(); const btVector3* vtx = hull->getVertexPointer(); glBegin (GL_TRIANGLES); for (int i = 0; i < hull->numTriangles (); i++) { int i1 = index++; int i2 = index++; int i3 = index++; btAssert(i1 < hull->numIndices () && i2 < hull->numIndices () && i3 < hull->numIndices ()); int index1 = idx[i1]; int index2 = idx[i2]; int index3 = idx[i3]; btAssert(index1 < hull->numVertices () && index2 < hull->numVertices () && index3 < hull->numVertices ()); btVector3 v1 = vtx[index1]; btVector3 v2 = vtx[index2]; btVector3 v3 = vtx[index3]; btVector3 normal = (v3-v1).cross(v2-v1); normal.normalize (); glNormal3f(normal.getX(),normal.getY(),normal.getZ()); glVertex3f (v1.x(), v1.y(), v1.z()); glVertex3f (v2.x(), v2.y(), v2.z()); glVertex3f (v3.x(), v3.y(), v3.z()); } glEnd (); } } } } } } /// for polyhedral shapes if (debugMode==btIDebugDraw::DBG_DrawFeaturesText && (shape->isPolyhedral())) { btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape; { glRasterPos3f(0.0, 0.0, 0.0); //btDrawString(BMF_GetFont(BMF_kHelvetica10),polyshape->getExtraDebugInfo()); glColor3f(1.f, 1.f, 1.f); int i; for (i=0;i<polyshape->getNumVertices();i++) { btVector3 vtx; polyshape->getVertex(i,vtx); glRasterPos3f(vtx.x(), vtx.y(), vtx.z()); char buf[12]; sprintf(buf," %d",i); //btDrawString(BMF_GetFont(BMF_kHelvetica10),buf); } for (i=0;i<polyshape->getNumPlanes();i++) { btVector3 normal; btVector3 vtx; polyshape->getPlane(normal,vtx,i); btScalar d = vtx.dot(normal); glRasterPos3f(normal.x()*d, normal.y()*d, normal.z()*d); char buf[12]; sprintf(buf," plane %d",i); //btDrawString(BMF_GetFont(BMF_kHelvetica10),buf); } } } #ifdef USE_DISPLAY_LISTS if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE||shape->getShapeType() == GIMPACT_SHAPE_PROXYTYPE) { GLuint dlist = OGL_get_displaylist_for_shape((btCollisionShape * )shape); if (dlist) { glCallList(dlist); } else { #else if (shape->isConcave() && !shape->isInfinite()) { btConcaveShape* concaveMesh = (btConcaveShape*) shape; GlDrawcallback drawCallback; drawCallback.m_wireframe = (debugMode & btIDebugDraw::DBG_DrawWireframe)!=0; concaveMesh->processAllTriangles(&drawCallback,worldBoundsMin,worldBoundsMax); } #endif #ifdef USE_DISPLAY_LISTS } } #endif glDisable(GL_DEPTH_TEST); glRasterPos3f(0,0,0);//mvtx.x(), vtx.y(), vtx.z()); if (debugMode&btIDebugDraw::DBG_DrawText) { GLDebugDrawString(0,0,shape->getName()); } if (debugMode& btIDebugDraw::DBG_DrawFeaturesText) { //btDrawString(BMF_GetFont(BMF_kHelvetica10),shape->getExtraDebugInfo()); } glEnable(GL_DEPTH_TEST); // glPopMatrix(); if(m_textureenabled) glDisable(GL_TEXTURE_2D); }
/*! \pre tv_plane and tu_plane must be set \post distances[2] is set with the distance closest_point_u, closest_point_v, edge_edge_dir are set too \return - 0: faces are paralele - 1: face U casts face V - 2: face V casts face U - 3: nearest edges */ SIMD_FORCE_INLINE GUINT cross_line_intersection_test() { // Compute direction of intersection line edge_edge_dir = tu_plane.cross(tv_plane); GREAL Dlen; VEC_LENGTH(edge_edge_dir,Dlen); if(Dlen<0.0001) { return 0; //faces near paralele } edge_edge_dir*= 1/Dlen;//normalize // Compute interval for triangle 1 GUINT tu_e0,tu_e1;//edge indices GREAL tu_scale_e0,tu_scale_e1;//edge scale if(!compute_intervals(du[0],du[1],du[2], du0du1,du0du2,tu_scale_e0,tu_scale_e1,tu_e0,tu_e1)) return 0; // Compute interval for triangle 2 GUINT tv_e0,tv_e1;//edge indices GREAL tv_scale_e0,tv_scale_e1;//edge scale if(!compute_intervals(dv[0],dv[1],dv[2], dv0dv1,dv0dv2,tv_scale_e0,tv_scale_e1,tv_e0,tv_e1)) return 0; //proyected vertices btVector3 up_e0 = tu_vertices[tu_e0].lerp(tu_vertices[(tu_e0+1)%3],tu_scale_e0); btVector3 up_e1 = tu_vertices[tu_e1].lerp(tu_vertices[(tu_e1+1)%3],tu_scale_e1); btVector3 vp_e0 = tv_vertices[tv_e0].lerp(tv_vertices[(tv_e0+1)%3],tv_scale_e0); btVector3 vp_e1 = tv_vertices[tv_e1].lerp(tv_vertices[(tv_e1+1)%3],tv_scale_e1); //proyected intervals GREAL isect_u[] = {up_e0.dot(edge_edge_dir),up_e1.dot(edge_edge_dir)}; GREAL isect_v[] = {vp_e0.dot(edge_edge_dir),vp_e1.dot(edge_edge_dir)}; sort_isect(isect_u[0],isect_u[1],tu_e0,tu_e1,up_e0,up_e1); sort_isect(isect_v[0],isect_v[1],tv_e0,tv_e1,vp_e0,vp_e1); const GREAL midpoint_u = 0.5f*(isect_u[0]+isect_u[1]); // midpoint const GREAL midpoint_v = 0.5f*(isect_v[0]+isect_v[1]); // midpoint if(midpoint_u<midpoint_v) { if(isect_u[1]>=isect_v[1]) // face U casts face V { return 1; } else if(isect_v[0]<=isect_u[0]) // face V casts face U { return 2; } // closest points closest_point_u = up_e1; closest_point_v = vp_e0; // calc edges and separation if(isect_u[1]+ MIN_EDGE_EDGE_DIS<isect_v[0]) //calc distance between two lines instead { SEGMENT_COLLISION( tu_vertices[tu_e1],tu_vertices[(tu_e1+1)%3], tv_vertices[tv_e0],tv_vertices[(tv_e0+1)%3], closest_point_u, closest_point_v); edge_edge_dir = closest_point_u-closest_point_v; VEC_LENGTH(edge_edge_dir,distances[2]); edge_edge_dir *= 1.0f/distances[2];// normalize } else { distances[2] = isect_v[0]-isect_u[1];//distance negative //edge_edge_dir *= -1.0f; //normal pointing from V to U } } else { if(isect_v[1]>=isect_u[1]) // face V casts face U { return 2; } else if(isect_u[0]<=isect_v[0]) // face U casts face V { return 1; } // closest points closest_point_u = up_e0; closest_point_v = vp_e1; // calc edges and separation if(isect_v[1]+MIN_EDGE_EDGE_DIS<isect_u[0]) //calc distance between two lines instead { SEGMENT_COLLISION( tu_vertices[tu_e0],tu_vertices[(tu_e0+1)%3], tv_vertices[tv_e1],tv_vertices[(tv_e1+1)%3], closest_point_u, closest_point_v); edge_edge_dir = closest_point_u-closest_point_v; VEC_LENGTH(edge_edge_dir,distances[2]); edge_edge_dir *= 1.0f/distances[2];// normalize } else { distances[2] = isect_u[0]-isect_v[1];//distance negative //edge_edge_dir *= -1.0f; //normal pointing from V to U } } return 3; }
glm::vec4 toVec4(const btVector4& v) { return glm::vec4(v.getX(), v.getY(), v.getZ(), v.getW()); }