void CollisionDetector::setupVClipModel(hrp::BodyPtr i_body)
{
    m_VclipLinks.resize(i_body->numLinks());
    std::cerr << i_body->numLinks() << std::endl;
    for (int i=0; i<i_body->numLinks(); i++) {
      assert(i_body->link(i)->index == i);
      setupVClipModel(i_body->link(i));
    }
}
void convertToAABB(hrp::BodyPtr i_body)
{
    for (int i=0; i<i_body->numLinks(); i++) convertToAABB(i_body->link(i));
}
void convertToConvexHull(hrp::BodyPtr i_body)
{
    for (int i=0; i<i_body->numLinks(); i++) {
        convertToConvexHull(i_body->link(i));
    }
}
RTC::ReturnCode_t setupCollisionModel(hrp::BodyPtr m_robot, const char *url, OpenHRP::BodyInfo_var binfo) {
    // do qhull
    OpenHRP::ShapeInfoSequence_var sis = binfo->shapes();
    OpenHRP::LinkInfoSequence_var lis = binfo->links();
    for(int i = 0; i < m_robot->numLinks(); i++ ) {
	const OpenHRP::LinkInfo& i_li = lis[i];
	const OpenHRP::TransformedShapeIndexSequence& tsis = i_li.shapeIndices;
	// setup
	int numTriangles = 0;
	for (unsigned int l=0; l<tsis.length(); l++) {
	    OpenHRP::ShapeInfo& si = sis[tsis[l].shapeIndex];
	    const OpenHRP::LongSequence& triangles = si.triangles;
	    numTriangles += triangles.length();
	}
	double points[numTriangles*3];
	int points_i = 0;
	hrp::Matrix44 Rs44; // inv
	hrp::Matrix33 Rs33 = m_robot->link(i)->Rs;

	Rs44 << Rs33(0,0),Rs33(0,1), Rs33(0,2), 0,
	    Rs33(1,0),Rs33(1,1), Rs33(1,2), 0,
	    Rs33(2,0),Rs33(2,1), Rs33(2,2), 0,
	    0.0,      0.0,       0.0,    1.0;
	for (unsigned int l=0; l<tsis.length(); l++) {
	    const OpenHRP::DblArray12& M = tsis[l].transformMatrix;
	    hrp::Matrix44 T0;
	    T0 << M[0], M[1], M[2],  M[3],
		M[4], M[5], M[6],  M[7],
		M[8], M[9], M[10], M[11],
		0.0,  0.0,  0.0,   1.0;
	    hrp::Matrix44 T(Rs44 * T0);
	    const OpenHRP::ShapeInfo& si = sis[tsis[l].shapeIndex];
	    const OpenHRP::LongSequence& triangles = si.triangles;
	    const float *vertices = si.vertices.get_buffer();

	    for(unsigned int j=0; j < triangles.length() / 3; ++j){
		for(int k=0; k < 3; ++k){
		    long orgVertexIndex = si.triangles[j * 3 + k];
		    int p = orgVertexIndex * 3;

		    hrp::Vector4 v(T * hrp::Vector4(vertices[p+0], vertices[p+1], vertices[p+2], 1.0));
		    points[points_i++] =  v[0];
		    points[points_i++] =  v[1];
		    points[points_i++] =  v[2];
		}
	    }
	}

	hrp::ColdetModelPtr coldetModel(new hrp::ColdetModel());
	coldetModel->setName(std::string(i_li.name));
	// qhull
	int vertexIndex = 0;
	int triangleIndex = 0;
	int num = 0;
	char flags[250];
	boolT ismalloc = False;
	sprintf(flags,"qhull Qt Tc");
	if (! qh_new_qhull (3,numTriangles,points,ismalloc,flags,NULL,stderr) ) {

	    qh_triangulate();
	    qh_vertexneighbors();

	    vertexT *vertex,**vertexp;
	    coldetModel->setNumVertices(qh num_vertices);
	    coldetModel->setNumTriangles(qh num_facets);
	    int index[qh num_vertices];
	    FORALLvertices {
		int p = qh_pointid(vertex->point);
		index[p] = vertexIndex;
		coldetModel->setVertex(vertexIndex++, points[p*3+0], points[p*3+1], points[p*3+2]);
	    }
	    facetT *facet;
	    num = qh num_facets;;
	    {
		FORALLfacets {
		    int j = 0, p[3];
		    setT *vertices = qh_facet3vertex (facet);
		    FOREACHvertexreverse12_ (vertices) {
			if (j<3) {
			    p[j] = index[qh_pointid(vertex->point)];
			} else {
			    fprintf(stderr, "extra vertex %d\n",j);
			}
			j++;
		    }
		    coldetModel->setTriangle(triangleIndex++, p[0], p[1], p[2]);
		}
	    }
	} // qh_new_qhull

        coldetModel->build();
	m_robot->link(i)->coldetModel =  coldetModel;

	qh_freeqhull(!qh_ALL);
	int curlong, totlong;
	qh_memfreeshort (&curlong, &totlong);
	if (curlong || totlong) {
	    fprintf(stderr, "convhulln: did not free %d bytes of long memory (%d pieces)\n", totlong, curlong);
	}
	//
	std::cerr << std::setw(16) << i_li.name << " reduce triangles from " << std::setw(5) << numTriangles << " to " << std::setw(4) << num << std::endl;
    }
Exemple #5
0
void convertToBoundingBox(hrp::BodyPtr i_body)
{
    for (int i=0; i<i_body->numLinks(); i++){
        convertToBoundingBox(i_body->link(i));
    }
}