int numberOfIntersections(std::vector<std::shared_ptr<line_segment>>& polygon, std::shared_ptr<line_segment> ray)
{
int numberOfinters = 0;
std::vector<int> cross;
for (int i = 0; i < polygon.size(); ++i)
{
cross.push_back(cross_product(polygon[i], ray));
}
for (int i = 0; i < polygon.size(); ++i)
{
Intersection temp = doesIntersect(polygon[i], ray);
if (temp == ONE)
{
numberOfinters++;
}
else
if (temp == INFINITE)
{
int lastCross = cross[(i - 1) % cross.size()];
int z = (i + 1) % cross.size();
while (cross[z] == 0)
{
z = (z + 1) % cross.size();
}
i = z;
if ((lastCross > 0 && cross[z] > 0) || (lastCross < 0 && cross[z] < 0))
{
numberOfinters++;
}
}
}
return numberOfinters;
}
void newSheep::identityRequest_callBack(se306_example::IdentityRequest request)
{
//ROS_INFO("Request received by %s,%s",robot_name.c_str(),robot_number.c_str());
//ROS_INFO("%s",request.abs_cmd_vel_angular_z);
if(request.sender.compare(robot_name) != 0) {
se306_example::IdentityReply reply;
bool result = doesIntersect(request.px, request.py);
//ROS_INFO ("x is %f and y is %f and angle is %f", request.px, request.py, theta);
if(result) {
// ROS_INFO("YAY RESULT!");
// ROS_INFO ("x is %f and y is %f", request.px, request.py);
reply.sender = robot_name;
reply.destination = request.sender;
reply.type = "sheep";
/*this needs to be changed a bit to reflect actual velocity*/
reply.abs_cmd_vel_angular_z = angular_z;
reply.abs_cmd_vel_linear_x = linear_x;
reply.theta = theta;
Reply_pub.publish(reply);
ROS_INFO("reply sent");
}
}
}
