C++ Plane3D::distance方法代码示例

int ClosestContact(const ContactPoint& p,const Meshing::TriMesh& mesh,ContactPoint& pclose,Real normalScale)
{
  int closest = -1;
  Real closestDist2 = Inf;
  Triangle3D tri;
  Plane3D plane;
  for(size_t i=0;i<mesh.tris.size();i++) {
    mesh.GetTriangle(i,tri);
    //first check distance to supporting plane, since it's a lower bound
    tri.getPlane(plane);
    Real dxmin = plane.distance(p.x);
    Real dn = normalScale*plane.normal.distanceSquared(p.n);
    if(dn + Sqr(dxmin) < closestDist2) {
      //has potential to be closer than previous
      Vector3 cp = tri.closestPoint(p.x);
      Real d = cp.distanceSquared(p.x) + dn;
      if(d < closestDist2) {
	closest = (int)i;
	closestDist2 = d;
	pclose.x = cp;
	pclose.n = plane.normal;
	pclose.kFriction = p.kFriction;
      }
    }
  }
  return closest;
}

Real Polygon3D::maxDistance(const Plane3D& p) const
{
  Real dmax=0;
  for(size_t i=0;i<vertices.size();i++) {
    Real d = p.distance(vertices[i]);
    if(d > dmax) dmax=d;
  }
  return dmax;
}

bool Circle3D::setIntersection(const Sphere3D& s,const Plane3D& p)
{
  Real d = p.distance(s.center);
  Real absd=Abs(d);
  if(absd > s.radius) return false;
  axis = p.normal;
  //projection of s.center on plane
  center = s.center - d*p.normal;
  radius = pythag_leg(absd,s.radius);
  return true;
}

bool ConvexPolyhedron3D::planeNeg(const Plane3D& p) const
{
	Real vpos;
	for(int i=0; i<numVertices; i++)
	{
		vpos = p.distance(vertices[i]);
		if(vpos > Zero)
			return false;
	}

	return true;
}

bool TriMesh::PlaneSplits(const Plane3D& p,Real& dmin,Real& dmax) const
{
  dmin=Inf;
  dmax=-Inf;
  Real d;
  for(size_t i=0;i<verts.size();i++) {
    d = p.distance(verts[i]);
    if(d < dmin) dmin=d;
    if(d > dmax) dmax=d;
  }
  return (dmin <= Zero && dmax >= Zero);
}

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;
    }
}

bool Triangle3D::intersects(const Plane3D& P, Segment3D& S) const
{
    Real d[3];
    const Point3D* p[3] = {&a,&b,&c};
    for(int i=0; i<3; i++) d[i]=P.distance(*p[i]);
    //insertion sort
    for(int i=1; i<3; i++) {
        Real di=d[i];
        const Point3D* pi=p[i];
        int j=i;
        for(; j>0; j--) {
            if(d[j-1] <= di) break;
            d[j] = d[j-1];
            p[j] = p[j-1];
        }
        d[j] = di;
        p[j] = pi;
    }
    if(!(d[0] <= d[1] && d[1] <= d[2])) {
        printf ("AAAACK: %f %f %f\n",d[0],d[1],d[2]);
    }
    assert(d[0] <= d[1] && d[1] <= d[2]);

    if(d[0] > Zero) return false;
    if(d[2] < Zero) return false;
    Real u;
    if(d[1] <= Zero) { //both 0 and 1 are inside p
        if(d[0] == d[2]) u = 0;
        else u = d[0]/(d[0]-d[2]);
        S.a = (One-u)*(*p[0]) + u*(*p[2]);
        if(d[1] == d[2]) u = 0;
        else u = d[1]/(d[1]-d[2]);
        S.b = (One-u)*(*p[1]) + u*(*p[2]);
    }
    else { //only 0 is inside p
        u = d[0]/(d[0]-d[1]);
        S.a = (One-u)*(*p[0]) + u*(*p[1]);
        u = d[0]/(d[0]-d[2]);
        S.b = (One-u)*(*p[0]) + u*(*p[2]);
    }
    return true;
}

bool TriMesh::Intersects(const Plane3D& p) const
{
  vector<Real> d(verts.size());
  for(size_t i=0;i<verts.size();i++)
    d[i] = p.distance(verts[i]);
  Real da,db,dc;
  for(size_t i=0;i<tris.size();i++) {
    da=d[tris[i].a];
    db=d[tris[i].b];
    dc=d[tris[i].c];
    if(da > Zero) {
      if(db <= Zero || dc <= Zero) return true;
    }
    else if(da < Zero) {
      if(db >= Zero || dc >= Zero) return true;
    }
    else //da==Zero
      return true;
  }
  return false;
}

bool Sphere3D::intersects(const Plane3D& p) const
{
  return Abs(p.distance(center)) <= radius;
}

bool ClipLine(const Vector3& x, const Vector3& v, const Plane3D& b, Real& u1, Real& u2)
{
	//p is dot(v, normal), q is signed dist to plane
	return ClipLine1D(b.distance(x), dot(v,b.normal), u1,u2);
}