void Life::draw(const Camera &camera) { if(anim != NULL) anim->draw(Object::getPosition() - point_to_vector(camera.getPosition()), etc::ALIGN_BY_CENTER, IntRect(0, 0, -1, -1)); }
bool PixelMap::isCollision(const RectMap &rectmap, const IntPoint &pos1, const IntPoint &pos2, const IntRect &rect1, const IntRect &rect2, const Alignment &alignment1, const Alignment &alignment2) const { IntPoint corner1(pos1), corner2(pos2); IntRect framerect1, framerect2; IntRect mrect1, mrect2; IntRect *cutrect; /* Break */ if(map == NULL) return false; /* Translate negative axises */ mrect1 = rect_translate_negative_axises(rect1, size); mrect2 = rect_translate_negative_axises(rect2, rectmap.getSize()); /* Align */ if(alignment1.getX() != 0) corner1.decX((int)(mrect1.getWidth() * alignment1.getX())); if(alignment1.getY() != 0) corner1.decY((int)(mrect1.getHeight() * alignment1.getY())); if(alignment2.getX() != 0) corner2.decX((int)(mrect2.getWidth() * alignment2.getX())); if(alignment2.getY() != 0) corner2.decY((int)(mrect2.getHeight() * alignment2.getY())); /* Framerects */ framerect1.load(corner1.getX(), corner1.getY(), mrect1.getWidth(), mrect1.getHeight()); framerect2.load(corner2.getX(), corner2.getY(), mrect2.getWidth(), mrect2.getHeight()); /* Break */ if(!size_to_rect(size).isCovering(mrect1)) throw Exception() << "pixelmap1 doesn't fully cover rect"; /* Cutrect */ if((cutrect = framerect1.getCutrect(framerect2)) == NULL) return false; /* Iterate rects, check pixels */ for(int r = 0; r < rectmap.getRectCount(); r++) { int fpx, fpy; /* First pixel to check */ int lpx, lpy; /* Last pixel to check */ IntRect srect = rectmap.getRect(r) + point_to_vector(corner2) - IntVector(mrect2.getX(), mrect2.getY()); IntRect *scutrect; /* Clip with cutrect*/ if((scutrect = srect.getCutrect(*cutrect)) == NULL) continue; /* Pixels to check */ fpx = scutrect->getX() - (corner1.getX() - mrect1.getX()); fpy = scutrect->getY() - (corner1.getY() - mrect1.getY()); lpx = fpx + scutrect->getWidth() - 1; lpy = fpy + scutrect->getHeight() - 1; /* Check pixels */ for(int py = fpy; py <= lpy; py++) { for(int px = fpx; px <= lpx; px++) { if(is_pixel(px, py)) { /* Free mem */ delete cutrect; delete scutrect; /* Collision detected */ return true; } } } delete scutrect; } delete cutrect; return false; }
void costmap_cb(const nav_msgs::GridCells::ConstPtr& msg) { ODOM_FRAME = msg->header.frame_id; geometry_msgs::PoseArray ff_msg; ff_msg.header.stamp = msg->header.stamp; ff_msg.header.frame_id = ODOM_FRAME; last_fv = msg->header.stamp; force_vector[0] = force_vector[1] = 0; std::vector<geometry_msgs::Point> points = msg->cells; std::vector<geometry_msgs::Pose> poses; int count = 0; float total_weight = 0.0; for (std::vector<std::string>::size_type i = 0; i < points.size(); i++) { geometry_msgs::PointStamped point, point_trans; point.header.stamp = msg->header.stamp; point.header.frame_id = ODOM_FRAME; point.point.x = points[i].x; point.point.y = points[i].y; tf_listener->waitForTransform(BASE_FRAME, ODOM_FRAME, point.header.stamp, ros::Duration(0.1)); try { tf_listener->transformPoint(BASE_FRAME, point, point_trans); } catch (tf::ExtrapolationException e) { ROS_ERROR("Unable to get tf transform: %s", e.what()); return; } float x = point_trans.point.x; float y = point_trans.point.y; float yaw = atan(y / x); if (x > 0) yaw = modulus(yaw + PI, 2*PI); std::vector<float> point_vector = point_to_vector(x, y); if (point_vector.size() > 0) { count++; //float weight = 0.75*(MAX_RANGE - BASE_RADIUS - CLEARING_DIST)/(sqrt(pow(x, 2) + pow(y, 2)) - BASE_RADIUS - CLEARING_DIST); //if ( weight < 0 ) // std::cout << "evil" << std::endl; //length += weight; //float weight = (PI - abs(yaw)) / PI; force_vector[0] += point_vector[0]; force_vector[1] += point_vector[1]; geometry_msgs::PoseStamped pose, pose_trans; pose.header.stamp = msg->header.stamp; pose.header.frame_id = BASE_FRAME; pose.pose.position.x = x; pose.pose.position.y = y; pose.pose.orientation = tf::createQuaternionMsgFromYaw(yaw); tf_listener->waitForTransform(ODOM_FRAME, BASE_FRAME, pose.header.stamp, ros::Duration(0.1)); try { tf_listener->transformPose(ODOM_FRAME, pose, pose_trans); } catch (tf::ExtrapolationException e) { ROS_ERROR("Unable to get tf transform: %s", e.what()); return; } poses.push_back(pose_trans.pose); } } //force_vector[0] /= length + 1; //force_vector[1] /= length + 1; std::cout << force_vector[0] << "," << force_vector[1] << std::endl; // publish resulting force vector as Pose geometry_msgs::PoseStamped force, force_trans; force.header.stamp = msg->header.stamp; force.header.frame_id = BASE_FRAME; float force_yaw = atan(force_vector[1] / force_vector[0]); if (force_vector[0] > 0) force_yaw = modulus(force_yaw + PI, 2*PI); force.pose.orientation = tf::createQuaternionMsgFromYaw(force_yaw); tf_listener->waitForTransform(BASE_FRAME, ODOM_FRAME, msg->header.stamp, ros::Duration(0.1)); try { tf_listener->transformPose(ODOM_FRAME, force, force_trans); } catch (tf::ExtrapolationException e) { ROS_ERROR("Unable to get tf transform: %s", e.what()); return; } force_obst_pub.publish(force_trans); // publish force field as PoseArray ff_msg.poses = poses; force_field_pub.publish(ff_msg); }