std::map<float, std::vector<Body*> > Body::findAlignments(
std::vector<Body*> bodies,
const float time,
const float tolerance = M_PI/180.0) {
//std::vector<float> angles;
std::map<float, std::vector<Body*> > aligned_bodies;
//std::cout << std::endl << name_ << " angles:" << std::endl;
for (size_t i = 0; i < bodies.size(); i++) {
if (bodies[i] == this) {
continue;
}
const float angle = getRelativeAngle(this, bodies[i], time);
// search through existing angles and see if current one matches
bool found_match = false;
for (std::map<float, std::vector<Body*> >::iterator it = aligned_bodies.begin();
it != aligned_bodies.end(); it++) {
const double a2 = it->first;
for (size_t j = 0; j < 2; j++) {
const double a3 = a2 + (float(j) - 1.0) * M_PI;
//std::cout << angle << " " << a2 << " " << a3 << std::endl;
if (
(std::abs(a3 - angle) < tolerance)
) {
it->second.push_back(bodies[i]);
//if (it->second.size() > 2)
if (false)
std::cout << " " << time << " " << name_ << ": "
<< " match " << bodies[i]->name_ << " "
<< a2 << " " << a3 << " " << angle << " "
<< std::abs(a3 - angle) << " " << it->second.size()
<< std::endl;
found_match = true;
//continue;
}
}
} // search for similar angles
if (!found_match) {
//std::cout << bodies[i]->name_ << " " << angle << std::endl;
aligned_bodies[angle].push_back(this);
aligned_bodies[angle].push_back(bodies[i]);
}
} // find all relative angles
return aligned_bodies;
}
static bool verifyFollowing()
{
double slope = 0;
double intercept = 0;
getLinearEquation(_current_waypoints.getWaypointPosition(1),_current_waypoints.getWaypointPosition(2),&slope,&intercept);
double displacement = getDistanceBetweenLineAndPoint(_current_pose.pose.position,slope,intercept);
double relative_angle = getRelativeAngle(_current_waypoints.getWaypointPose(1),_current_pose.pose);
//ROS_INFO("side diff : %lf , angle diff : %lf",displacement,relative_angle);
if(displacement < g_displacement_threshold || relative_angle < g_relative_angle_threshold){
//ROS_INFO("Following : True");
return true;
}
else{
//ROS_INFO("Following : False");
return false;
}
}
int main(int nargn, char** argv) {
std::string name;
float distance;
float period;
const double cur_time = atof(argv[1]); // std::stof(argv[1]);
std::vector<Body*> bodies;
while (std::cin >> name >> distance >> period) {
//std::cout << name << " " << distance << " " << period << std::endl;
bodies.push_back(new Body(name, distance, period));
}
//std::cout << bodies.size() << std::endl;
const int wd = 1024;
const cv::Point2f offset = cv::Point2f(wd/2, wd/2);
double t = cur_time;
float sc = 64.0;
bool time_changed = true;
std::vector< std::map<float, std::vector<Body*> > > all_aligned_bodies;
std::vector<std::vector<Body*> > best_alignments;
while (true) { //(t < cur_time) {
cv::Mat im = cv::Mat(wd, wd, CV_8UC3, cv::Scalar::all(0));
if (time_changed) all_aligned_bodies.clear();
for (size_t i = 0; i < bodies.size(); i++) {
// draw body
cv::Point2f pt = bodies[i]->getPosition(t);
cv::Point2f pix = pt * (wd / sc) + offset;
cv::Scalar col = cv::Scalar(255, i * 20, 255 - i * 20);
cv::circle(im, bodies[0]->getPosition(0) + offset,
bodies[i]->distance_ * wd / sc,
cv::Scalar(0,0,55), 1);
cv::circle(im, pix, 3, col, -1);
std::stringstream label;
label << bodies[i]->name_[0]; // i;
cv::putText(im, label.str(), pix + cv::Point2f(10,10), 1, 1, col, 1);
// find alignments
if (time_changed) {
all_aligned_bodies.push_back( bodies[i]->findAlignments(bodies, t) );
}
}
int wait_val = 5;
if (time_changed) {
best_alignments = getBestAlignments(all_aligned_bodies);
for (size_t k = 0; k < best_alignments.size(); k++) {
for (size_t j = 1; j < best_alignments[k].size(); j++) {
const float angle = getRelativeAngle(
best_alignments[k][0],
best_alignments[k][j],
t
);
std::cout << best_alignments[k][0]->name_ << "-"
<< best_alignments[k][j]->name_ << " angle " << angle
//<< " " << best_alignments[k].size()
<< std::endl;
}
std::cout << t << " ";
for (size_t j = 0; j < best_alignments[k].size(); j++) {
std::cout << best_alignments[k][j]->name_;
if (j < best_alignments[k].size() - 1)
std::cout << "-";
}
std::cout << std::endl;
}
}
for (size_t k = 0; k < best_alignments.size(); k++) {
cv::Point2f pt = best_alignments[k][0]->getPosition(t);
cv::Point2f pix = pt * (wd / sc) + offset;
// draw alignments
for (size_t j = 0; j < best_alignments[k].size(); j++) {
cv::Point2f pt2 = best_alignments[k][j]->getPosition(t);
cv::Point2f pix2 = pt2 * (wd / sc) + offset;
//std::cout << i << " " << j << " " << it->first
// << " " << (it->second[j])->name_ << std::endl;
//std::cout << pix << pix2 << std::endl;
cv::line( im, pix, pix2, cv::Scalar(255, k * 50, 255 - k * 20), 1 );
} // draw alignments
//wait_val = 0;
}
cv::imshow("im", im);
//int c = cv::waitKey(0);
int c = cv::waitKey(wait_val);
time_changed = false;
if (c == 'w') sc *= 1.2;
else if (c == 's') sc *= 0.9;
else if (c == 'q') break;
/*else {
t += 0.01;
time_changed = true;
}*/
}
for (size_t i = 0; i < bodies.size(); i++) {
delete bodies[i];
}
return 0;
}
