double PointGrid::footprintCost(const geometry_msgs::Point& position, const vector<geometry_msgs::Point>& footprint,
double inscribed_radius, double circumscribed_radius){
//the half-width of the circumscribed sqaure of the robot is equal to the circumscribed radius
double outer_square_radius = circumscribed_radius;
//get all the points inside the circumscribed square of the robot footprint
geometry_msgs::Point c_lower_left, c_upper_right;
c_lower_left.x = position.x - outer_square_radius;
c_lower_left.y = position.y - outer_square_radius;
c_upper_right.x = position.x + outer_square_radius;
c_upper_right.y = position.y + outer_square_radius;
//This may return points that are still outside of the cirumscribed square because it returns the cells
//contained by the range
getPointsInRange(c_lower_left, c_upper_right, points_);
//if there are no points in the circumscribed square... we don't have to check against the footprint
if(points_.empty())
return 1.0;
//compute the half-width of the inner square from the inscribed radius of the robot
double inner_square_radius = sqrt((inscribed_radius * inscribed_radius) / 2.0);
//we'll also check against the inscribed square
geometry_msgs::Point i_lower_left, i_upper_right;
i_lower_left.x = position.x - inner_square_radius;
i_lower_left.y = position.y - inner_square_radius;
i_upper_right.x = position.x + inner_square_radius;
i_upper_right.y = position.y + inner_square_radius;
//if there are points, we have to do a more expensive check
for(unsigned int i = 0; i < points_.size(); ++i){
list<geometry_msgs::Point32>* cell_points = points_[i];
if(cell_points != NULL){
for(list<geometry_msgs::Point32>::iterator it = cell_points->begin(); it != cell_points->end(); ++it){
const geometry_msgs::Point32& pt = *it;
//first, we'll check to make sure we're in the outer square
//printf("(%.2f, %.2f) ... l(%.2f, %.2f) ... u(%.2f, %.2f)\n", pt.x, pt.y, c_lower_left.x, c_lower_left.y, c_upper_right.x, c_upper_right.y);
if(pt.x > c_lower_left.x && pt.x < c_upper_right.x && pt.y > c_lower_left.y && pt.y < c_upper_right.y){
//do a quick check to see if the point lies in the inner square of the robot
if(pt.x > i_lower_left.x && pt.x < i_upper_right.x && pt.y > i_lower_left.y && pt.y < i_upper_right.y)
return -1.0;
//now we really have to do a full footprint check on the point
if(ptInPolygon(pt, footprint))
return -1.0;
}
}
}
}
//if we get through all the points and none of them are in the footprint it's legal
return 1.0;
}
void PointGrid::removePointsInPolygon(const vector<geometry_msgs::Point> poly){
if(poly.size() == 0)
return;
geometry_msgs::Point lower_left, upper_right;
lower_left.x = poly[0].x;
lower_left.y = poly[0].y;
upper_right.x = poly[0].x;
upper_right.y = poly[0].y;
//compute the containing square of the polygon
for(unsigned int i = 1; i < poly.size(); ++i){
lower_left.x = min(lower_left.x, poly[i].x);
lower_left.y = min(lower_left.y, poly[i].y);
upper_right.x = max(upper_right.x, poly[i].x);
upper_right.y = max(upper_right.y, poly[i].y);
}
ROS_DEBUG("Lower: (%.2f, %.2f), Upper: (%.2f, %.2f)\n", lower_left.x, lower_left.y, upper_right.x, upper_right.y);
getPointsInRange(lower_left, upper_right, points_);
//if there are no points in the containing square... we don't have to do anything
if(points_.empty())
return;
//if there are points, we have to check them against the polygon explicitly to remove them
for(unsigned int i = 0; i < points_.size(); ++i){
list<geometry_msgs::Point32>* cell_points = points_[i];
if(cell_points != NULL){
list<geometry_msgs::Point32>::iterator it = cell_points->begin();
while(it != cell_points->end()){
const geometry_msgs::Point32& pt = *it;
//check if the point is in the polygon and if it is, erase it from the grid
if(ptInPolygon(pt, poly)){
it = cell_points->erase(it);
}
else
it++;
}
}
}
}
bool BaseRegion::pointInRegion(int x, int y) {
if (_points.size() < 3) {
return false;
}
Point32 pt;
pt.x = x;
pt.y = y;
Rect32 rect;
rect.left = x - 1;
rect.right = x + 2;
rect.top = y - 1;
rect.bottom = y + 2;
if (BasePlatform::ptInRect(&_rect, pt)) {
return ptInPolygon(x, y);
} else {
return false;
}
}
//collision move elipsoid with polygon
bool kgmCollision::collision(vec3& start, vec3& end, float rx, float ry, float rz,
vec3* poly, u32 points)
{
kgmIntersection intersection;
plane3 plane(poly[0], poly[1], poly[2]);
line3 line(start, end);
vec3 pt_insect;
vec3 pr_start, pr_end, pr_delta;
bool b_plinsect = false;
float s_dist = plane.distance(start),
e_dist = plane.distance(end);
// if(s_dist < 0.0) return false;;
pr_start = plane.projection(start);
pr_end = plane.projection(end);
pr_delta = pr_end - end;
if(plane.intersect(line, pt_insect))
{
if(ptInPolygon(pt_insect, poly, points) ||
crossEllipticPolygon(pt_insect, rx, ry, rz, poly, points))
{
m_normal = plane.normal();
m_point = pt_insect = pr_end;
m_collision = true;
return true;
}
}
if((SQR(pr_end.x - end.x) < SQR(rx)) &&
(SQR(pr_end.y - end.y) < SQR(ry)) &&
(SQR(pr_end.z - end.z) < SQR(rz)))
{
if(ptInPolygon(pr_end, poly, points) ||
crossEllipticPolygon(pr_end, rx, ry, rz, poly, points))
{
m_normal = plane.normal();
m_point = pr_end;
m_collision = true;
return true;
}
/* if((SQR(pr_start.x - start.x) < SQR(rx)) &&
(SQR(pr_start.y - start.y) < SQR(ry)) &&
(SQR(pr_start.z - start.z) < SQR(rz))){
if(ptInPolygon(pr_start, poly, points) ||
crossEllipticPolygon(pr_start, rx, ry, rz, poly, points)){
m_normal = plane.normal();
m_point = pr_end;
m_collision = true;
return true;
}
}//*/
}
m_normal = plane.normal();
m_point = pt_insect;
return m_collision;
}
//collision move sphere with polygon
bool kgmCollision::collision(vec3& start, vec3& end, float radius, vec3* poly, u32 points)
{
plane3 plane(poly[0], poly[1], poly[2]);
line3 line(start, end);
vec3 pt_insect;
float s_dist = plane.distance(start),
e_dist = plane.distance(end);
bool b_plinsect = false;
bool b_plnear = false;
if(s_dist < 0.0) return false;
if(e_dist > radius) return false;
if(plane.intersect(line, pt_insect))
{
b_plinsect = true;
}
if((e_dist > 0.0f) && (e_dist < radius))
{
pt_insect = plane.projection(end);
b_plnear = true;
}
if(!b_plinsect && !b_plnear)
{
return false;
}
///*
if(!ptInPolygon(pt_insect, poly, points))
{
sphere3 sphere(pt_insect, radius);
for(int i = 0; i < points ; i++)
{
line3 line;
if(i == (points - 1))
line.set(poly[0], poly[i]);
else
line.set(poly[i], poly[i + 1]);
if(crossLineSphere(line, sphere))
{
m_normal = plane.normal();
m_point = pt_insect;
return true;;
}
}
return false;
}
//*/
m_normal = plane.normal();
m_point = pt_insect;
return true;
}
