C++ Plane3D类代码示例

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

// line (or segment) in 3D vs plane
bool vsLine3D(const Line3D& line, const Plane3D& plane, Point3D *rpoint = NULL, bool check = true)
{
  // check if line is parallel with plane
  if ( abs( line.vector * plane.normal() ) < epsilon )
    return false;

  Point3D p  = line.point, q = line.point + line.vector;
  float normalLen = plane.normal().length();

  // get dist (and side) from plane for each point
  float distP = plane.Evaluate( p ) / normalLen;
  float distQ = plane.Evaluate( q ) / normalLen;

  // find t
  float t;

  if ( ( distP < 0.f && distQ < 0.f ) || ( distP > 0.f && distQ > 0.f ) )
  {
    if ( check ) return false;

    t = abs( distP ) < abs( distQ )
        ? -abs( distP / ( distP - distQ ) )
        : abs( ( distP + distQ ) / ( distP - distQ ) );
  }

  else
  {
    t = abs( distP ) / ( abs( distP ) + abs( distQ ) );
  }

  if ( rpoint ) *rpoint = p + t * line.vector;

  return true;
}

Eigen::VectorXd
Line3D::getLine(Plane3D &p1, Plane3D &p2) {
  Eigen::VectorXd v(6);

  this->m = p1.getPlane() * p2.getPlane().transpose() - p2.getPlane() * p1.getPlane().transpose();
  this->plucker_matrix2vector(this->m, v);
  this->plucker_vector_swap(v);
  return v;
}

// Compute angle between two geometric entities (in radians;  use acos)
float AngleBetween(const Line3D& line, const Plane3D& plane)
{
  Vector3D u = line.vector ^ plane.normal();
  Vector3D v = plane.normal() ^ u;

  float angle = acos( v * line.vector );

  return ( ( angle < 0.f ) ? ( angle + 2.f * M_PI ) : angle );
}

void PlaneExtents(const Triangle3D& tri,Plane3D& p,Real& dmin,Real& dmax)
{
  dmin=dmax=p.distance(tri.a);
  Real d=p.distance(tri.b);
  if(d<dmin) dmin=d;
  else if(d>dmax) dmax=d;
  d=p.distance(tri.c);
  if(d<dmin) dmin=d;
  else if(d>dmax) dmax=d;
}

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

  u[0] = SegmentZeroCrossing(da,db);
  u[1] = SegmentZeroCrossing(db,dc);
  u[2] = SegmentZeroCrossing(dc,da);
}

bool Triangle3D::intersects(const Plane3D& p) const
{
  Real minDist,maxDist,d;
  minDist = maxDist = p.distance(a);
  d = p.distance(b);
  if(d < minDist) minDist = d;
  else if(d > maxDist) maxDist = d;
  d = p.distance(c);
  if(d < minDist) minDist = d;
  else if(d > maxDist) maxDist = d;
  return (minDist <= Zero) && (maxDist >= Zero);
}

void TransformWidget::Drag(int dx,int dy,Camera::Viewport& viewport)
{
  dragX += dx;
  dragY += dy;
  Ray3D r;
  viewport.getClickSource(dragX,dragY,r.source);
  viewport.getClickVector(dragX,dragY,r.direction);
  if(hoverItem < 0) return;
  else if(hoverItem == 0) {
    Vector3 v;
    viewport.getMovementVectorAtDistance(dx,dy,clickDistance,v);
    T.t += v;
  }
  else if(hoverItem <= 3) { //translation
    Line3D axisLine;
    axisLine.source = clickPos;
    axisLine.direction = Vector3(T.R.col(hoverItem-1));
    Real t,u;
    axisLine.closestPoint(r,t,u);
    T.t = clickTransform.t + axisLine.direction*t;
  }
  else {
    Plane3D ringPlane;
    Vector3 axis;
    if(hoverItem <= 6) axis = Vector3(clickTransform.R.col(hoverItem-4));
    else axis = clickAxis;
    Vector3 x,y;
    GetCanonicalBasis(axis,x,y);
    //find rotation to minimize distance from clicked pos to drag ray
    Real cx = x.dot(clickPos - T.t);
    Real cy = y.dot(clickPos - T.t);
    ringPlane.setPointNormal(T.t,axis);
    Real t;
    bool res=ringPlane.intersectsRay(r,&t);
    //odd... no intersection
    if(res==false) return;
    Vector3 raypos = r.source + t*r.direction - T.t;
    Real rx = x.dot(raypos);
    Real ry = y.dot(raypos);
    if(Sqr(rx) + Sqr(ry) < 1e-5) return;
    Real theta = AngleDiff(Atan2(ry,rx),Atan2(cy,cx));
    AngleAxisRotation aa;
    aa.axis = axis;
    aa.angle = theta;
    QuaternionRotation qR,qT,qRes;
    qR.setAngleAxis(aa);
    qT.setMatrix(clickTransform.R);
    qRes.mul(qR,qT);
    qRes.getMatrix(T.R);
  }
  Refresh();
}

void ConvexPolyhedron3D::planeExtents(const Plane3D& p,Real& dmin,Real& dmax) const
{
  if(numVertices == 0) {
    dmin=Inf;
    dmax=-Inf;
    return;
  }
  dmin=dmax = p.distance(vertices[0]);
  Real d;
  for(int i=1; i<numVertices; i++) {
      d = p.distance(vertices[i]);
      if(d < dmin) dmin=d;
      else if(d > dmax) dmax=d;
  }	
}

//------------------------------------------------------------------------------------------------------
//function that checks if sphere collides with a plane
//------------------------------------------------------------------------------------------------------
bool Sphere3D::IsColliding(const Plane3D& secondPlane) const
{

	//make use of Plane3D's plane-sphere collision function
	return (secondPlane.IsColliding(*this));

}

int Polygon::ClassifyPolygonToPlane(Plane3D& plane, bool predicate_flag)
{
    // Loop over all polygon vertices and count how many vertices
    // lie in front of and how many lie behind of the thickened plane
    unsigned long numInFront = 0, numBehind = 0;
    for (unsigned long i = 0; i < this->_nv; i++) {
        // Point *p = _verts[i];
		switch (plane.ClassifyPointToPlane(*_verts[i], predicate_flag)) {
        //switch (ClassifyPointToPlane(p, plane)) {
			case Plane3D::POINT_IN_FRONT_OF_PLANE:
				numInFront++;
				break;
			case Plane3D::POINT_BEHIND_PLANE:
				numBehind++;
				break;
        }
    }
	/*if (numInFront+numBehind != 3 && !(numInFront==0 && numBehind==0))
		return POLYGON_STRADDLING_PLANE;*/
    // If vertices on both sides of the plane, the polygon is straddling
    if (numBehind != 0 && numInFront != 0)
        return POLYGON_STRADDLING_PLANE;
    // If one or more vertices in front of the plane and no vertices behind
    // the plane, the polygon lies in front of the plane
    if (numInFront != 0)
        return POLYGON_IN_FRONT_OF_PLANE;
    // Ditto, the polygon lies behind the plane if no vertices in front of
    // the plane, and one or more vertices behind the plane
    if (numBehind != 0)
        return POLYGON_BEHIND_PLANE;
    // All vertices lie on the plane so the polygon is coplanar with the plane
    return POLYGON_COPLANAR_WITH_PLANE;
}

bool Circle3D::intersects(const Line3D& l,Real* _t) const
{
  Plane3D p;
  getPlane(p);
  Real t;
  if(p.intersectsLine(l,&t)) {
    if(t == Inf)  { //line lies in plane
      t = l.closestPointParameter(center);
    }
    if(_t) (*_t)=t;
    Point3D pt;
    l.eval(t,pt);
    return DistanceLEQ(pt,center,radius);
  }
  return false;
}

void Polygon3D::getPlane(int i,Plane3D& p) const
{
  Assert(vertices.size() >= 3);
  size_t j=next(i);
  size_t k=next(j);
  p.setPoints(vertices[i],vertices[j],vertices[k]);
}

void IKGoal::GetClosestGoalTransform(const RigidTransform& T0,RigidTransform& T) const
{
  //fill out rotation first
  if(rotConstraint == RotFixed) {
    GetFixedGoalRotation(T.R);
  }
  else if(rotConstraint == RotAxis) {
    //T.R*localAxis = endRotation
    GetMinimalRotation(localAxis,T0.R*endRotation,T.R);
    //make it so orthogonal directions perform a rotation similar to T0.R
    Vector3 lx,ly,rx,ry,refx;
    GetCanonicalBasis(localAxis,lx,ly);
    rx = T.R*rx;
    ry = T.R*ry;
    refx = T0.R*lx;
    Real x = dot(refx,rx);
    Real y = dot(refx,ry);
    //find the rotation about endRotation that gets closer to this
    Real theta = Atan2(y,x);
    AngleAxisRotation aa;
    aa.angle = theta;
    aa.axis = T0.R*endRotation;
    
    Matrix3 Rrot;
    aa.getMatrix(Rrot);
    T.R = Rrot*T.R;
  }
  else 
    T.R = T0.R;

  T.t = endPosition - T.R*localPosition;
  if(posConstraint == PosPlanar) {
    //find closest transform on plane to T0.t
    Plane3D p;
    p.setPointNormal(T.t,direction);
    p.project(T0.t,T.t);
  }
  else if(posConstraint == PosLinear) {
    //find closest transform on line to T0.t
    Line3D line;
    line.source = T.t;
    line.direction = direction;
    line.closestPoint(T0.t,T.t);
  }
  else if(posConstraint == PosNone)
    T.t = T0.t;
}

Point3D
Line3D::intersectPlane(Plane3D &p) {
  Eigen::Vector4d point;

  this->plucker_vector2matrix(this->m, this->v);
  point = this->m * p.getPlane();  
  return Point3D(point); 
}