C++ Triangle3D类代码示例

//Returns a contact normal for the closest point to the triangle t.  p is the point on the triangle.
//The direction is the one in which triangle 1 can move to get away from closestpt
Vector3 ContactNormal(const CollisionMesh& m,const Vector3& p,int t,const Vector3& closestPt)
{
  Triangle3D tri;
  m.GetTriangle(t,tri);
  Vector3 b=tri.barycentricCoords(p);
  int type=FeatureType(b);
  switch(type) {
  case 1:  //pt
    //get the triangle normal
    {
      Vector3 n = VertexNormal(m,t,VertexIndex(b));
      n.inplaceNegative();
      return n;
    }
    break;
  case 2:  //edge
    {
      int e = EdgeIndex(b);
      Vector3 n = EdgeNormal(m,t,e);
      n.inplaceNegative();
      return n;
    }
    break;
  case 3:  //face
    return m.currentTransform.R*(-tri.normal());
  }
  static int warnedCount = 0;
  if(warnedCount % 10000 == 0) 
    printf("ODECustomMesh: Warning, degenerate triangle, types %d\n",type);
  warnedCount++;
  //AssertNotReached();
  return Vector3(Zero);
}

int ClosestPointDescent(const TriMeshWithTopology& m,const Vector3& p,int tri,Vector3& cp)
{
  Assert(m.tris.size() == m.triNeighbors.size());
  Assert(tri >= 0 && tri < (int)m.tris.size());
  Triangle3D t;
  Vector3 temp;
  Real closestd=Inf;
  FastFindHeap<int,Real> q;  //sorted from highest to lowest
  set<int> visited;
  q.push(tri,-Inf);
  while(!q.empty()) {
    int curtri = q.top(); q.pop();
    visited.insert(curtri);

    m.GetTriangle(curtri,t);
    temp = t.closestPoint(p);
    Real d=temp.distance(p);
    if(d < closestd) {
      tri = curtri;
      closestd = d;
      cp = temp;
      //traverse to neighbors
      for(int i=0;i<3;i++) {
	int neighbor = m.triNeighbors[curtri][i];
	if(neighbor >= 0) {
	  if(visited.find(neighbor)==visited.end())
	    q.adjust(neighbor,-d);
	}
      }
    }
  } 
  return tri;
}

Real Polygon3D::areaConvex() const
{
  Real sum=0;
  Triangle3D temp;
  for(size_t i=1;i+1<vertices.size();i++) {
    temp.set(vertices[0],vertices[i],vertices[i+1]);
    sum += temp.area();
  }
  return sum;
}

bool Box3D::intersects(const Triangle3D& t) const
{
  Triangle3D tloc;
  toLocal(t.a,tloc.a);
  toLocal(t.b,tloc.b);
  toLocal(t.c,tloc.c);
  AABB3D bbloc;
  bbloc.bmin.setZero();
  bbloc.bmax=dims;
  return tloc.intersects(bbloc);
}

int MeshPrimitiveCollide(CollisionMesh& m1,Real outerMargin1,GeometricPrimitive3D& g2,const RigidTransform& T2,Real outerMargin2,dContactGeom* contact,int maxcontacts)
{
  GeometricPrimitive3D gworld=g2;
  gworld.Transform(T2);
  Sphere3D s;
  if(gworld.type != GeometricPrimitive3D::Point && gworld.type != GeometricPrimitive3D::Sphere) {
    fprintf(stderr,"Distance computations between Triangles and %s not supported\n",gworld.TypeName());
    return 0;
  }
  if(gworld.type == GeometricPrimitive3D::Point) {
    s.center = *AnyCast<Point3D>(&gworld.data);
    s.radius = 0;
  }
  else {
    s = *AnyCast<Sphere3D>(&gworld.data);
  }
    
  Real tol = outerMargin1 + outerMargin2;
  Triangle3D tri;
  vector<int> tris;
  int k=0;
  NearbyTriangles(m1,gworld,tol,tris,maxcontacts);
  for(size_t j=0;j<tris.size();j++) {   
    m1.GetTriangle(tris[j],tri);
    tri.a = m1.currentTransform*tri.a;
    tri.b = m1.currentTransform*tri.b;
    tri.c = m1.currentTransform*tri.c;

    Vector3 cp = tri.closestPoint(s.center);
    Vector3 n = cp - s.center;
    Real nlen = n.length();
    Real d = nlen-s.radius;
    Vector3 pw = s.center;
    if(s.radius > 0)
      //adjust pw to the sphere surface
      pw += n*(s.radius/nlen);
    if(d < gNormalFromGeometryTolerance) {  //compute normal from the geometry
      Vector3 plocal;
      m1.currentTransform.mulInverse(cp,plocal);
      n = ContactNormal(m1,plocal,tris[j],pw);
    }
    else if(d > tol) {  //some penetration -- we can't trust the result of PQP
      continue;
    }
    else n /= nlen;
    //migrate the contact point to the center of the overlap region
    CopyVector(contact[k].pos,0.5*(cp+pw) + ((outerMargin2 - outerMargin1)*0.5)*n);
    CopyVector(contact[k].normal,n);
    contact[k].depth = tol - d;
    k++;
    if(k == maxcontacts) break;
  }
  return k;
}

bool ColDetect::coldetect(Triangle3D tri1, Triangle3D tri2, double *trans1, double *trans2) {//wrapper, so I would not have to deal with arrays
	int ntri1 = 1;	//number of rows in first array (one row for one Triangle, propably)
	int ntri2 = 1;	//number of rows in second array (one row for one Triangle, propably)
	int ntrans1 = 1;	//number of rows for transformation matrix
	int ntrans2 = 1;	//number of rows for transformation matrix
	std::vector<double> tri1vector = tri1.toVector();
	std::vector<double> tri2vector = tri2.toVector();
	double tri1array[9];
	double tri2array[9];
	copy(tri1vector.begin(), tri1vector.end(), tri1array);
	copy(tri2vector.begin(), tri2vector.end(), tri2array);
	return coldetect(ntri1, ntri2, ntrans1, ntrans2, tri1array, tri2array, trans1, trans2);
}

Vector3 Polygon3D::centroidConvex() const
{
  const static Real Third = 1.0/3.0;
  Vector3 c(Zero);
  Real sum=0;
  Triangle3D temp;
  for(size_t i=1;i+1<vertices.size();i++) {
    temp.set(vertices[0],vertices[i],vertices[i+1]);
    Real area = temp.area();
    c.madd((temp.a+temp.b+temp.c),area*Third);
    sum += area;
  }
  if(sum == 0) return c;
  return c/sum;
}

bool Triangle3D::intersects(const Triangle3D& T, Segment3D& S) const
{
  //intersect this plane with t
  Plane3D p;
  getPlane(p);
  if(!T.intersects(p,S)) return false;

  //now limit S to the boundaries of this triangle
  //find segment in plane coordinates (uA,vA)->(uB,vB)
  //clip against constraints u,v >= 0, u+v<=1
  Vector2 A,D;
  A = planeCoords(S.A);
  D = planeCoords(S.B)-A;
  Real tmin=Zero,tmax=One;
  //(u,v) = A+t*D
  //1) u >= 0 => -uA+t*(-uD) <= 0
  if(!ClipLine1D(-A.x,-D.x,tmin,tmax)) return false;
  //2) v >= 0 => -vA+t*(-vD) <= 0
  if(!ClipLine1D(-A.y,-D.y,tmin,tmax)) return false;
  //3) u+v <= 1 => (uA+vA-1)+t*(uD+vD) <= 0
  if(!ClipLine1D(A.x+A.y-One, D.x+D.y,tmin,tmax)) return false;
  Vector2 U=A; U.madd(D,tmin);
  S.A = planeCoordsToPoint(U);
  U=A; U.madd(D,tmax);
  S.B = planeCoordsToPoint(U);
  return true;
}

int TriMesh::ClosestPoint(const Vector3& pt,Vector3& cp) const
{
  int tmin=-1;
  Real dmin=Inf;
  Vector3 temp;
  Triangle3D tri;
  for(size_t i=0;i<tris.size();i++) {
    GetTriangle(i,tri);
    temp = tri.closestPoint(pt);
    Real d2=temp.distanceSquared(pt);
    if(d2 < dmin) {
      tmin = (int)i;
      dmin = d2;
      cp = temp;
    }
  }
  return tmin;
}

int TriMesh::RayCast(const Ray3D& r,Vector3& pt) const
{
  int tmin=-1;
  Real dmin=Inf;
  Real d;
  Vector2 uv;
  Triangle3D tri;
  for(size_t i=0;i<tris.size();i++) {
    GetTriangle(i,tri);
    if(tri.rayIntersects(r,&d,&uv.x,&uv.y)) {
      if(d < dmin) {
	tmin = (int)i;
	dmin = d;
	pt = tri.planeCoordsToPoint(uv);
      }
    }
  }
  return tmin;
}

// Determines if 3D segment and triangle have a unique intersection.  
// If true and rpoint is not NULL, returns intersection point.
bool Intersects(const Segment3D& seg, const Triangle3D& tri, Point3D *rpoint)
{
  Vector3D n = ( tri[1] - tri[0] ) ^ ( tri[2] - tri[1] );

  float a = n[0], b = n[1], c = n[2];
  float d = -( a * tri[0][0] + b * tri[0][1] + c * tri[0][2] );

  if ( !Intersects( seg, Plane3D( a, b, c, d ), rpoint ) )
    return false;

  return tri.contains( *rpoint );
}

void Triangle3D::edgeIntersections(const Triangle3D& T, Real u[3]) const
{
    Plane3D PT;
    T.getPlane(PT);
    Real da,db,dc;
    da=PT.distance(a);
    db=PT.distance(b);
    dc=PT.distance(c);

    u[0]=u[1]=u[2]=-One;

    //check to see if these points are within T's boundaries
    Vector3 x;
    Vector2 U;
    Real ui;
    //edge a,b
    ui = SegmentZeroCrossing(da,db);
    if(ui >= Zero && ui <= One) {
        interpolate(a,b,ui,x);
        U = T.planeCoords(x);
        if(containsPlaneCoords(U)) u[0] = ui;
    }

    //edge b,c
    ui = SegmentZeroCrossing(db,dc);
    if(ui >= Zero && ui <= One) {
        interpolate(b,c,ui,x);
        U = T.planeCoords(x);
        if(containsPlaneCoords(U)) u[1] = ui;
    }

    //edge c,a
    ui = SegmentZeroCrossing(dc,da);
    if(ui >= Zero && ui <= One) {
        interpolate(c,a,ui,x);
        U = T.planeCoords(x);
        if(containsPlaneCoords(U)) u[2] = ui;
    }
}

//data loading
bool ModelData::LoadIn(QString filepath)
{	
    bool loaderReady;
    bool abort;

    STLTri* pLoadedTri = NULL;
    Triangle3D newtri;

	this->filepath = filepath;
	
	if(filepath.isEmpty())
		return false;
	
	//extract filename from path!
    filename = QFileInfo(filepath).fileName();

    B9ModelLoader mLoader(filepath,loaderReady,NULL);

    if(loaderReady == false)//error opening model data
	{
        //display Loader Error
		QMessageBox msgBox;
        msgBox.setText(mLoader.GetError());
        msgBox.exec();
		return false;
	}

    //make a progress bar and connect it to
    LoadingBar loadbar(0,100);
    loadbar.useCancelButton(false);
    loadbar.setDescription("Importing: " + filename);
    QObject::connect(&mLoader,SIGNAL(PercentCompletedUpdate(qint64,qint64)),
                     &loadbar,SLOT(setProgress(qint64,qint64)));



    //now we are ready to walk the loader through reading each triangle
    //and copying it into the this model data.
    while(mLoader.LoadNextTri(pLoadedTri,abort))
    {
        if(abort)
        {
            //display Loader abort error
            QMessageBox msgBox;
            msgBox.setText(mLoader.GetError());
            msgBox.exec();
            return false;
        }
        else
        {
            //newtri.normal.setX(pLoadedTri->nx);
            //newtri.normal.setY(pLoadedTri->ny);
            //newtri.normal.setZ(pLoadedTri->nz);

            newtri.vertex[0].setX(pLoadedTri->x0);
            newtri.vertex[0].setY(pLoadedTri->y0);
            newtri.vertex[0].setZ(pLoadedTri->z0);
            newtri.vertex[1].setX(pLoadedTri->x1);
            newtri.vertex[1].setY(pLoadedTri->y1);
            newtri.vertex[1].setZ(pLoadedTri->z1);
            newtri.vertex[2].setX(pLoadedTri->x2);
            newtri.vertex[2].setY(pLoadedTri->y2);
            newtri.vertex[2].setZ(pLoadedTri->z2);

            //use right hand rule for importing normals - ignore file normals..
            newtri.UpdateNormalFromGeom();

            delete pLoadedTri;
            newtri.UpdateBounds();

            triList.push_back(newtri);
        }
    }



    qDebug() << "Loaded triangles: " << triList.size();
	//now center it!
	CenterModel();

    //generate a normal display lists.

    int displaySuccess = FormNormalDisplayLists();

    if(displaySuccess)
        return true;
    else
        return false;
}

int Slice::GenerateSegments(B9ModelInstance* inputInstance)
{
    unsigned int t;
	int v1;
	int v2;
    int cmpcount = 0;//0 or 1 来指示你所寻找的点;

    QVector3D* thisvert = NULL;//局部指针进行快速访问顶点
	QVector3D* thatvert = NULL;

    std::vector<Triangle3D*>* rangedTriangles;

	double xdisp;
	double ydisp;
	double zdisp;
	double planefraction;

	int intersections = 0;


	//Triangle Splitting here:
    //Get the right container of triangles and use that as the list
    //(we used to use the triList which was O(n^2))
    rangedTriangles = inputInstance->GetTrianglesAroundZ(realAltitude);


    for(t = 0; t < rangedTriangles->size(); t++)//for each triangle in the model
	{
		//we want to create a temporary pointer to the currenct triangle
        Triangle3D* pTransTri = rangedTriangles->at(t);

		//test if the triangle intersects the XY plane of this slice!
		if(!pTransTri->IntersectsXYPlane(realAltitude))
		{
            continue;
		}
			
		intersections++;
		cmpcount = 0;
		
		//create 1 new segment object for the end result 
		Segment* seg1 = new Segment;
		QVector2D points[2];
		for(v1=0;v1<3;v1++)//for 1 or 2 triangle verts ABOVE the plane:
		{
			thisvert = &pTransTri->vertex[v1];
			if(thisvert->z() <= realAltitude)//we only want to compare FROM above the plane by convention (yes this means flush triangles below the plane)
			{
				continue;
			}
			for(v2=0; v2<3; v2++)//to every other triangle vert
			{
				if(v2 == v1)
				{continue;}

				thatvert = &pTransTri->vertex[v2];

				//are both points on the same side of plane?
				//if so we dont want to compare
				if((thatvert->z() > realAltitude))
				{
					continue;
				}
					
				cmpcount++;
				//common
				//displacments (final - initial)
				xdisp = thatvert->x() - thisvert->x();
				ydisp = thatvert->y() - thisvert->y();
				zdisp = thatvert->z() - thisvert->z();

                planefraction = (thisvert->z() - realAltitude)/fabs(zdisp);//0-1 fraction of where the plane is in relation to the z distance between the 2 verts.
				//(0 would be the plane is at the hieght of thisvert)

				points[cmpcount-1].setX(thisvert->x() + xdisp*planefraction);
				points[cmpcount-1].setY(thisvert->y() + ydisp*planefraction);
			}
		}
	
		//initiallize the segment.
		seg1->normal.setX(pTransTri->normal.x());
		seg1->normal.setY(pTransTri->normal.y());

		seg1->p1.setX(points[0].x());
		seg1->p1.setY(points[0].y());

		seg1->p2.setX(points[1].x());
		seg1->p2.setY(points[1].y());

		seg1->normal.normalize();

		seg1->CorrectPointOrder();//to match the normal convention!
			
		AddSegment(seg1);
	}
	return segmentList.size();
}

//data loading
bool ModelData::LoadIn(QString filepath)
{	
    unsigned int m;
    unsigned int t;
    unsigned int i;

    Triangle3D newtri;
    const struct aiFace* face;

	
	this->filepath = filepath;
	
	if(filepath.isEmpty())
		return false;
	
	//extract filename from path!
	filename = QFileInfo(filepath).baseName();

	//AI_CONFIG_PP_FD_REMOVE = aiPrimitiveType_POINTS | aiPrimitiveType_LINES;
    pScene = aiImportFile(filepath.toAscii(), aiProcess_Triangulate);// | aiProcess_JoinIdenticalVertices); //trian
	
    if(pScene == NULL)//assimp cant handle the file - lets try our own reader.
	{
		//display Assimp Error
		QMessageBox msgBox;
		msgBox.setText("Assimp Error:  " + QString().fromAscii(aiGetErrorString()));
		msgBox.exec();

        aiReleaseImport(pScene);

		return false;
	}

    qDebug() << "Model imported with " << pScene->mMeshes[0]->mNumFaces << " faces.";
	

	for (m = 0; m < pScene->mNumMeshes; m++) 
	{
		const aiMesh* mesh = pScene->mMeshes[m];
		
	    for (t = 0; t < mesh->mNumFaces; t++)
		{
            face = &mesh->mFaces[t];
			
			if(face->mNumIndices == 3)
			{
				for(i = 0; i < face->mNumIndices; i++) 
				{
					int index = face->mIndices[i];
				
                    newtri.normal.setX(mesh->mNormals[index].x);
                    newtri.normal.setY(mesh->mNormals[index].y);
                    newtri.normal.setZ(mesh->mNormals[index].z);
			
                    newtri.vertex[i].setX(mesh->mVertices[index].x);
                    newtri.vertex[i].setY(mesh->mVertices[index].y);
                    newtri.vertex[i].setZ(mesh->mVertices[index].z);
				}
                newtri.UpdateBounds();
                triList.push_back(newtri);

			}
		}
	}

    aiReleaseImport(pScene);

    qDebug() << "Loaded triangles: " << triList.size();
	//now center it!
	CenterModel();

	//generate a displaylist
    int displayerror = FormDisplayList();

    //check for errors in display list creation (if its a large model the card may run out of memory.

    if(displayerror){
    while(displayerror)//loop and see if there are additional errors as well.
    {
        //display Assimp Error
        qDebug() << "Display List Error: " << displayerror; //write to log as well.
        QMessageBox msgBox;

        switch(displayerror)
        {
        case GL_OUT_OF_MEMORY:
            msgBox.setText("OpenGL Error:  GL_OUT_OF_MEMORY\nModel is too large to render on your graphics card.");
            break;
        case GL_INVALID_ENUM:
            msgBox.setText("OpenGL Error:  GL_INVALID_ENUM");
            break;
        case GL_INVALID_VALUE:
            msgBox.setText("OpenGL Error:  GL_INVALID_VALUE");
            break;
        case GL_INVALID_FRAMEBUFFER_OPERATION:
            msgBox.setText("OpenGL Error:  GL_INVALID_FRAMEBUFFER_OPERATION");
            break;
        case GL_STACK_UNDERFLOW:
            msgBox.setText("OpenGL Error:  GL_STACK_UNDERFLOW");
//.........这里部分代码省略.........