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