C++ AABB3D::setIntersection方法代码示例

Real VolumeGrid::Average(const AABB3D& range) const
{
  IntTriple imin,imax;
  GetIndexRange(range,imin,imax);
  //check range
  if(imax.a < 0 || imax.b < 0 || imax.c < 0) {
    return 0;
  }
  if(imin.a >= value.m || imin.b >= value.n || imax.c >= value.p) {
    return 0;
  }
  //limit range
  if(imin.a < 0) imin.a=0;
  if(imin.b < 0) imin.b=0;
  if(imin.c < 0) imin.c=0;
  if(imax.a >= value.m) imax.a=value.m-1;
  if(imax.b >= value.n) imax.b=value.n-1;
  if(imax.c >= value.p) imax.c=value.p-1;

  bool ignoreX=(range.bmin.x==range.bmax.x),ignoreY=(range.bmin.y==range.bmax.y),ignoreZ=(range.bmin.z==range.bmax.z);

  Vector3 cellcorner;
  Vector3 cellsize;
  cellsize.x = (bb.bmax.x-bb.bmin.x)/Real(value.m);
  cellsize.y = (bb.bmax.y-bb.bmin.y)/Real(value.n);
  cellsize.z = (bb.bmax.z-bb.bmin.z)/Real(value.p);
  Real sumValue=0;
  Real sumVolume=0;
  cellcorner.x = bb.bmin.x + imin.a*cellsize.x;
  for(int i=imin.a;i<=imax.a;i++,cellcorner.x += cellsize.x) {
    cellcorner.y = bb.bmin.y + imin.b*cellsize.y;
    for(int j=imin.b;j<=imax.b;j++,cellcorner.y += cellsize.y) {
      cellcorner.z = bb.bmin.z + imin.c*cellsize.z;
      for(int k=imin.c;k<=imax.c;k++,cellcorner.z += cellsize.z) {
	AABB3D intersect;
	intersect.bmin=cellcorner;
	intersect.bmax=cellcorner+cellsize;
	intersect.setIntersection(range);
	Vector3 isectsize=intersect.bmax-intersect.bmin;
	//due to rounding errors, may have negative sizes
	if(isectsize.x < 0 || isectsize.y < 0 || isectsize.z < 0) continue;
	Real volume=1;
	if(!ignoreX) volume*=isectsize.x;
	if(!ignoreY) volume*=isectsize.y;
	if(!ignoreZ) volume*=isectsize.z;
	sumValue += volume*value(i,j,k);
	sumVolume += volume;
      }
    }
  }
  Vector3 rangesize=range.bmax-range.bmin;
  Real rangeVolume = 1;
  if(!ignoreX) rangeVolume*=rangesize.x;
  if(!ignoreY) rangeVolume*=rangesize.y;
  if(!ignoreZ) rangeVolume*=rangesize.z;
  Assert(sumVolume < rangeVolume + Epsilon);
  return sumValue / rangeVolume;
}

void NearbyPoints(const CollisionPointCloud& pc,const GeometricPrimitive3D& g,Real tol,std::vector<int>& pointIds,size_t maxContacts)
{
  Box3D bb;
  GetBB(pc,bb);
  //quick reject test
  if(g.Distance(bb) > tol) return;

  GeometricPrimitive3D glocal = g;
  RigidTransform Tinv;
  Tinv.setInverse(pc.currentTransform);
  glocal.Transform(Tinv);

  AABB3D gbb = glocal.GetAABB();
  gbb.setIntersection(pc.bblocal);

  //octree overlap method
  vector<Vector3> points;
  vector<int> ids;
  pc.octree->BoxQuery(gbb.bmin-Vector3(tol),gbb.bmax+Vector3(tol),points,ids);
  for(size_t i=0;i<points.size();i++) 
    if(glocal.Distance(points[i]) <= tol) {
      pointIds.push_back(ids[i]);
      if(pointIds.size()>=maxContacts) return;
    }
  return;

  /*
  //grid enumeration method
  GridSubdivision::Index imin,imax;
  pc.grid.PointToIndex(Vector(3,gbb.bmin),imin);
  pc.grid.PointToIndex(Vector(3,gbb.bmax),imax);
  int numCells = (imax[0]-imin[0]+1)*(imax[1]-imin[1]+1)*(imax[2]-imin[2]+1);
  if(numCells > (int)pc.points.size()) {
    printf("Testing all points\n");
    //test all points, linearly
    for(size_t i=0;i<pc.points.size();i++)
      if(glocal.Distance(pc.points[i]) <= tol) {
	pointIds.push_back(int(i));
	if(pointIds.size()>=maxContacts) return;
      }
  }
  else {
    printf("Testing points in BoxQuery\n");
    gNearbyTestThreshold = tol;
    gNearbyTestResults.resize(0);
    gNearbyTestObject = &glocal;
    gNearbyTestBranch = maxContacts;
    pc.grid.BoxQuery(Vector(3,gbb.bmin),Vector(3,gbb.bmax),nearbyTest);
    pointIds.resize(gNearbyTestResults.size());
    for(size_t i=0;i<points.size();i++)
      pointIds[i] = gNearbyTestResults[i] - &pc.points[0];
  }
  */
}

bool WithinDistance(const CollisionPointCloud& pc,const GeometricPrimitive3D& g,Real tol)
{
  Box3D bb;
  GetBB(pc,bb);
  //quick reject test
  if(g.Distance(bb) > tol) return false;

  GeometricPrimitive3D glocal = g;
  RigidTransform Tinv;
  Tinv.setInverse(pc.currentTransform);
  glocal.Transform(Tinv);

  AABB3D gbb = glocal.GetAABB();  
  gbb.setIntersection(pc.bblocal);

  //octree overlap method
  vector<Vector3> points;
  vector<int> ids;
  pc.octree->BoxQuery(gbb.bmin-Vector3(tol),gbb.bmax+Vector3(tol),points,ids);
  for(size_t i=0;i<points.size();i++) 
    if(glocal.Distance(points[i]) <= tol) return true;
  return false;

  /*
  //grid enumeration method
  GridSubdivision::Index imin,imax;
  pc.grid.PointToIndex(Vector(3,gbb.bmin),imin);
  pc.grid.PointToIndex(Vector(3,gbb.bmax),imax);
  int numCells = (imax[0]-imin[0]+1)*(imax[1]-imin[1]+1)*(imax[2]-imin[2]+1);
  if(numCells > (int)pc.points.size()) {
    //test all points, linearly
    for(size_t i=0;i<pc.points.size();i++)
      if(glocal.Distance(pc.points[i]) <= tol) return true;
    return false;
  }
  else {
    gWithinDistanceTestThreshold = tol;
    gWithinDistanceTestObject = &glocal;
    bool collisionFree = pc.grid.IndexQuery(imin,imax,withinDistanceTest);
    return !collisionFree;
  }
  */
}