void GetNearbyContacts(RobotWithGeometry& robot,int link,RobotWorld& world,Real tol,vector<ContactPoint>& contacts)
{
contacts.resize(0);
if(robot.IsGeometryEmpty(link)) {
return;
}
Geometry::AnyCollisionGeometry3D& g1 = *robot.geometry[link];
Real m1 = 0;
Real m2 = tol;
dContactGeom temp[1000];
int maxContacts = 1000;
vector<Geometry::AnyCollisionGeometry3D*> geomsToCheck;
for(size_t i=0;i<world.terrains.size();i++) {
if(world.terrains[i]->geometry.Empty()) continue;
geomsToCheck.push_back(&*world.terrains[i]->geometry);
}
for(size_t i=0;i<world.rigidObjects.size();i++) {
if(world.rigidObjects[i]->geometry.Empty()) {
continue;
}
geomsToCheck.push_back(&*world.rigidObjects[i]->geometry);
world.rigidObjects[i]->UpdateGeometry();
}
//now do the tolerance checks and add to the contacts list
for(size_t i=0;i<geomsToCheck.size();i++) {
int nc = GeometryGeometryCollide(g1,m1,*geomsToCheck[i],m2,temp,maxContacts);
if(nc > 0) {
size_t start = contacts.size();
contacts.resize(start+nc);
for(int j=0;j<nc;j++) {
//contacts[j].x.set(temp[j].pos);
contacts[j].x.set(temp[j].pos[0], temp[j].pos[1], temp[j].pos[2]);
contacts[j].n.set(temp[j].normal[0], temp[j].normal[1], temp[j].normal[2]);
//contacts[j].n.set(temp[j].normal);
contacts[j].kFriction = 0;
//convert to local coordinates
Vector3 localPos,localNormal;
robot.links[link].T_World.mulInverse(contacts[j].x,localPos);
robot.links[link].T_World.R.mulTranspose(contacts[j].n,localNormal);
contacts[j].x = localPos;
contacts[j].n = localNormal;
}
}
}
}
//Produces a list of contacts as though the robot were standing on a plane.
//tol is the tolerance with which minimum-distance points are generated.
//All contacts are given zero friction and in the local frame of the robot.
void GetFlatContacts(RobotWithGeometry& robot,Real tol,ContactFormation& contacts)
{
vector<AABB3D> bbs(robot.geometry.size());
vector<pair<Real,int> > order;
robot.UpdateGeometry();
for(size_t i=0;i<robot.geometry.size();i++)
if(robot.parents[i] >= 0 && !robot.IsGeometryEmpty(i)) {
AABB3D aabb = robot.geometry[i]->GetAABB();
bbs[i] = aabb;
order.push_back(pair<Real,int>(aabb.bmin.z,(int)i));
}
sort(order.begin(),order.end());
Real best = Inf;
for(size_t i=0;i<order.size();i++) {
if(order[i].first > best) break; //done
int k=order[i].second;
switch(robot.geometry[k]->type) {
case AnyGeometry3D::Primitive:
FatalError("Can't get flat contacts for primitives");
break;
case AnyGeometry3D::ImplicitSurface:
FatalError("Can't get flat contacts for implicit surfaces");
break;
case AnyGeometry3D::Group:
FatalError("Can't get flat contacts for geometry group");
break;
case AnyGeometry3D::TriangleMesh:
{
const TriMesh& m=robot.geometry[k]->AsTriangleMesh();
for(size_t v=0;v<m.verts.size();v++) {
Vector3 pw = robot.links[k].T_World*m.verts[v];
if(pw.z < best)
best = pw.z;
assert(pw.z >= order[i].first);
}
}
break;
case AnyGeometry3D::PointCloud:
{
const PointCloud3D& pc=robot.geometry[k]->AsPointCloud();
for(size_t v=0;v<pc.points.size();v++) {
Vector3 pw = robot.links[k].T_World*pc.points[v];
if(pw.z < best)
best = pw.z;
assert(pw.z >= order[i].first);
}
}
break;
}
}
//got the plane height, now output the vertices
ContactPoint cp;
cp.kFriction = 0.0;
contacts.links.resize(0);
contacts.contacts.resize(0);
for(size_t i=0;i<order.size();i++) {
if(order[i].first > best+tol) break; //done
int k=order[i].second;
contacts.links.resize(contacts.links.size()+1);
contacts.links.back()=k;
contacts.contacts.resize(contacts.contacts.size()+1);
vector<Vector3> pts;
switch(robot.geometry[k]->type) {
case AnyGeometry3D::TriangleMesh:
{
const TriMesh& m=robot.geometry[k]->AsTriangleMesh();
for(size_t v=0;v<m.verts.size();v++) {
Vector3 pw = robot.links[k].T_World*m.verts[v];
if(pw.z < best+tol) {
pts.push_back(pw);
}
}
}
break;
case AnyGeometry3D::PointCloud:
{
const PointCloud3D& pc=robot.geometry[k]->AsPointCloud();
for(size_t v=0;v<pc.points.size();v++) {
Vector3 pw = robot.links[k].T_World*pc.points[v];
if(pw.z < best+tol) {
pts.push_back(pw);
}
}
}
break;
default:
break;
}
//get the convex hull of those points
vector<Vector2> pts2(pts.size());
vector<Vector2> hull(pts.size());
vector<int> hindex(pts.size());
for(size_t v=0;v<pts.size();v++)
pts2[v].set(pts[v].x,pts[v].y);
int num = ConvexHull2D_Chain_Unsorted( &pts2[0], pts2.size(), &hull[0], &hindex[0]);
contacts.contacts.back().resize(num);
for(int v=0;v<num;v++) {
//local estimate
robot.links[k].T_World.mulInverse(pts[hindex[v]],cp.x);
robot.links[k].T_World.R.mulTranspose(Vector3(0,0,1),cp.n);
contacts.contacts.back()[v] = cp;
}
if(contacts.contacts.back().empty()) {
//bound was below threshold, but no contacts below threshold
contacts.links.resize(contacts.links.size()-1);
contacts.contacts.resize(contacts.contacts.size()-1);
}
}
}
