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