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;
}
*/
}
Real Distance(const CollisionPointCloud& pc,const GeometricPrimitive3D& g)
{
GeometricPrimitive3D glocal = g;
RigidTransform Tinv;
Tinv.setInverse(pc.currentTransform);
glocal.Transform(Tinv);
/*
AABB3D gbb = glocal.GetAABB();
gDistanceTestValue = Inf;
gDistanceTestObject = &glocal;
pc.grid.BoxQuery(Vector(3,gbb.bmin),Vector(3,gbb.bmax),distanceTest);
return gDistanceTestValue;
*/
//test all points, linearly
Real dmax = Inf;
for(size_t i=0;i<pc.points.size();i++)
dmax = Min(dmax,glocal.Distance(pc.points[i]));
return dmax;
}
void OrientedSupportPolygon::GetXYSlice(Real height,UnboundedPolytope2D& poly) const
{
//transform SP space to original space
RigidTransform Tinv;
Tinv.setInverse(T);
Vector3 x,y,z;
Tinv.R.get(x,y,z);
Matrix2 R2;
R2(0,0) = T.R(0,0);
R2(0,1) = T.R(0,1);
R2(1,0) = T.R(1,0);
R2(1,1) = T.R(1,1);
Real det=R2.determinant();
Assert(Abs(det) > Epsilon); //otherwise the slice is infinite...
Assert(Abs(z.z) > Epsilon); //otherwise the slice is infinite...
bool flip = (det < 0.0); //the plane is flipped
poly.vertices.resize(sp.vertices.size());
for(size_t i=0;i<sp.vertices.size();i++) {
const PointRay2D& v=sp.vertices[i];
Real u;
//let p = [v.x,v.y,u] (u unknown), q be the transformed point
if(v.isRay) {
//pick u s.t. p = T*[q.x,q.y,0] //parallel to xy plane
u = (-x.z*v.x - y.z*v.y)/z.z;
poly.vertices[i].isRay = true;
poly.vertices[i].x = x.x*v.x + y.x*v.y + u*z.x;
poly.vertices[i].y = x.y*v.x + y.y*v.y + u*z.y;
}
else {
//pick u s.t. p = T*[q.x,q.y,height]
u = (height - x.z*v.x - y.z*v.y - Tinv.t.z)/z.z;
poly.vertices[i].isRay = false;
poly.vertices[i].x = x.x*v.x + y.x*v.y + u*z.x + Tinv.t.x;
poly.vertices[i].y = x.y*v.x + y.y*v.y + u*z.y + Tinv.t.y;
}
}
if(flip) //maintain CCW ordering
reverse(poly.vertices.begin(),poly.vertices.end());
//laziness
poly.CalcPlanes();
}
