double PointLineDistance(const point &a, const point &b, const point &p, point &res) { double scale = dot(vec(a,b),vec(a,p)) / lengthSqr(vec(a,b)); res.real() = a.real() + scale * (b.real() - a.real()); res.imag() = a.imag() + scale * (b.imag() - a.imag()); return dist(p, res); }
line pointsToLine(point a, point b) { line result; result.a=b.imag()-a.imag(); result.b=a.real()-b.real(); result.c=a.imag()*b.real()-a.real()*b.imag(); return result; }
double get_angle(point a, point b, point c) { double ux = b.real() - a.real(); double uy = b.imag() - a.imag(); double vx = c.real() - a.real(); double vy = c.imag() - a.imag(); return acos( (ux * vx + uy * vy) / sqrt((ux * ux + uy * uy) * (vx * vx + vy * vy))); }
int turn(point O, point A, point B) { double res = (A.real() - O.real()) * (B.imag() - O.imag()) - (A.imag() - O.imag()) * (B.real() - O.real()); if (res < 0) return -1; if (res > 0) return 1; return 0; }
bool cmpAngle(point a, point b) { if (collinear(pivot, a, b)) return distsqr(pivot,a) < distsqr(pivot, b); // return ccw(pivot, a, b); double d1x = a.real() - pivot.real(); double d1y = a.imag() - pivot.imag(); double d2x = b.real() - pivot.real(); double d2y = b.imag() - pivot.imag(); return (atan2(d1y, d1x) - atan2(d2y, d2x) < 0); }
bool inPolygon(point P, vector<point> poly) { bool in = 0; int n = poly.size(); for (int i = 0, j = n - 1; i < n; j = i++) { if ((poly[i].imag() <= P.imag() + eps && P.imag() < poly[j].imag()) || (poly[j].imag() <= P.imag() + eps && P.imag() < poly[i].imag())) { if (P.real() - eps < (poly[j].real() - poly[i].real()) * (P.imag() - poly[i].imag()) / (poly[j].imag() - poly[i].imag()) + poly[i].real()) in ^= 1; } } return in; }
point closestPoint(line l, point p) { ld d=l.a*l.a+l.b*l.b; point result( (l.b*l.b*p.real()-l.a*l.c-l.a*l.b*p.imag())/d , (l.a*l.a*p.imag()-l.b*l.c-l.a*l.b*p.real())/d ); return result; }
double cross_p(point p, point q, point r) { return (r.real() - q.real()) * (p.imag() - q.imag()) - (r.imag() - q.imag()) * (p.real() - q.real()); }
int crossProd(point a, point b) { return a.real() * b.imag() - a.imag() * b.real(); }
bool cmpPoint(point a, point b) { if (a.real() == b.real()) return a.imag() < b.imag(); return a.real() < b.real(); }
void display1() { vector<double> color; double low = (vh[0]+0.9)/map_a/1.8 - map_b; double high = (vh[1]+0.9)/map_a/1.8 - map_b; // style glLineWidth(1); glPointSize(3); // ground const double gsize = 50.0; double gr_x = floor(robot.x); double gr_y = floor(robot.y); setColor(config["ground-panel"]); glBegin(GL_POLYGON); glVertex3d(gr_x+gsize, gr_y+gsize, 0.0); glVertex3d(gr_x-gsize, gr_y+gsize, 0.0); glVertex3d(gr_x-gsize, gr_y-gsize, 0.0); glVertex3d(gr_x+gsize, gr_y-gsize, 0.0); glEnd(); setColor(config["ground-line"]); glBegin(GL_LINES); for(int i=-gsize; i<=gsize; i++) { glVertex3d(gr_x+i*1.0, gr_y-gsize, 0.0); glVertex3d(gr_x+i*1.0, gr_y+gsize, 0.0); glVertex3d(gr_x-gsize, gr_y+i*1.0, 0.0); glVertex3d(gr_x+gsize, gr_y+i*1.0, 0.0); } glEnd(); // style glLineWidth(3); // robot glColor3d(1.0, 1.0, 1.0); drowCross(robot, glpos.data.theta, 1.0); // adjust if(disp_lajst) { glColor3d(0.0, 1.0, 1.0); drowCross(triple(lajst.data.x, lajst.data.y, 0.0), lajst.data.theta, 0.5); } if(disp_eajst) { glColor3d(0.0, 1.0, 0.0); drowCross(triple(eajst.data.x, eajst.data.y, 0.0), eajst.data.theta, 0.5); } #ifdef MERGEDEBUG glColor3d(1.0, 0.0, 1.0); drowCross(triple(mergecenter.real(), mergecenter.imag(), 0.0), eajst.data.theta, 0.5); #endif // gridmap if(disp_map) { color = config["map-points"]; glBegin(GL_POINTS); for(int i=0; i<gridmap.size(); i++) { if(low<=gridmap[i].z && gridmap[i].z<=high) { setColor(color, map_a*(map_b+gridmap[i].z)); if(view2d) glVertex2dv(gridmap[i].vec); else glVertex3dv(gridmap[i].vec); } } glEnd(); } // scan data for(int i=0; i<stream_num; i++) { // scan point if(!disp_sdata[i]) continue; glBegin(GL_POINTS); color = config[strprintf("urg-points-%d",i)]; for(int j=0; j<sdata[i].property.numPoints; j++) { if(sdata[i].data[j].isError()) continue; triple ref(sdata[i].data[j].reflect.vec); if(low<=ref.z && ref.z<=high) { ref.rotZ(glpos.data.theta); setColor(color, map_a*(map_b+ref.z)); glVertex3dv( (robot+ref).vec ); } } glEnd(); // scan laser if(!disp_laser) continue; glBegin(GL_LINES); for(int i=0; i<stream_num; i++) { color = config[strprintf("urg-beams-%d",i)]; for(int j=0; j<sdata[i].property.numPoints; j++) { if(sdata[i].data[j].isError()) continue; triple ref(sdata[i].data[j].reflect.vec); triple ori(sdata[i].data[j].origin.vec); if(low<=ref.z && ref.z<=high) { ref.rotZ(glpos.data.theta); ori.rotZ(glpos.data.theta); setColor(color, map_a*(map_b+ref.z)); glVertex3dv( (robot+ref).vec ); glVertex3dv( (robot+ori).vec ); } } } glEnd(); } // linemap if( disp_lmap ) { glColor3d(0.0, 0.4, 0.4); glBegin(GL_LINES); for(int i=0; i<linemap.size(); i++) { glVertex3dv( linemap[i].vec ); } glEnd(); } // edgemap if( disp_emap ) { glColor3d(0.0, 0.4, 0.0); glBegin(GL_LINES); for(int i=0; i<edgemap.size(); i++) { glVertex3dv( edgemap[i].vec ); } glEnd(); } // linedata if( ldata.isOpen() && disp_ldata ) { glColor3d(0.0, 1.0, 1.0); glBegin(GL_LINES); for(int i=0; i<ldata.data.numpoints; i++) { glVertex3d( ldata.data.start_x[i], ldata.data.start_y[i], 0.5 ); glVertex3d( ldata.data.finish_x[i], ldata.data.finish_y[i], 0.5 ); } glEnd(); } // edgedata if( edata.isOpen() && disp_edata ) { glColor3d(0.0, 1.0, 0.0); glBegin(GL_LINES); for(int i=0; i<edata.data.numpoints; i++) { glVertex3d( edata.data.start_x[i], edata.data.start_y[i], 0.5 ); glVertex3d( edata.data.finish_x[i], edata.data.finish_y[i], 0.5 ); } glEnd(); } #ifdef MERGEDEBUG glColor3d(1.0, 1.0, 1.0); glBegin(GL_LINES); glVertex3d(0,0,0); glVertex3d(10,0,0); glVertex3d(0,0,0); glVertex3d(0,10,0); glVertex3d(0,0,0); glVertex3d(0,0,10); glEnd(); //glColor3d(1.0, 1.0, 1.0); glBegin(GL_LINES); /* for(int i=0; i<mergedline.data.size(); i++) { point p1(0, 100), p2(0, -100); p1 *= polar(1.0, mergedline.data[i].rad); p1 += polar(mergedline.data[i].dist, mergedline.data[i].rad); p2 *= polar(1.0, mergedline.data[i].rad); p2 += polar(mergedline.data[i].dist, mergedline.data[i].rad); glColor3dv( testcolor[mergedline.data[i].group%7]); glVertex3d( p1.real(), p1.imag(), 1.5 ); glVertex3d( p2.real(), p2.imag(), 1.5 ); } */ for(int i=0; i<mergedline.data.size(); i++) { //glColor3dv( testcolor[mergedline.data[i].group%7]); glColor4d(1, 0, 0, 0.5); glVertex3d( mergedline.data[i][0].real(), mergedline.data[i][0].imag(), 0.5 ); glVertex3d( mergedline.data[i][1].real(), mergedline.data[i][1].imag(), 0.5 ); } glEnd(); #endif // style glPointSize(7); // route if(disp_way) { // route line glBegin(GL_LINES); for(int i=1; i<route.size(); i++) { //glColor3d(1.0, 1.0, 1.0); //if(pinfo.isOpen()) if(i==pinfo.data.waypoint) glColor3d(0.0, 1.0, 0.0); glColor3dv(amodecolor[routeinfo[i-1]]); glVertex3dv(route[i-1].vec); glVertex3dv(route[i ].vec); } glEnd(); // route point glColor3d(1.0, 1.0, 0.0); glBegin(GL_POINTS); for(int i=0; i<route.size(); i++) { glVertex3dv(route[i].vec); } glEnd(); // string glColor3d(1.0, 1.0, 1.0); for(int i=0; i<route.size(); i++) { //glColor3dv(amodecolor[routeinfo[i]]); myString(route[i]+triple(0,0,0.2), strprintf("%d",i).c_str()); } } }
bool operator()(const point &a, const point &b) const { if (!Equal(a.real(), b.real())) return a.real() < b.real(); return a.imag() < b.imag(); }
double cross(point a, point b) { return (a.real() * b.imag() - a.imag() * b.real()); }
double dot(point a, point b) { return (a.real() * b.real() + a.imag() * b.imag()); }
inline bool operator < (point a, point b){ if(a.real() != b.real())return a.real() < b.real(); return a.imag() < b.imag(); }
bool comp(const point& lhs, const point& rhs) { if(lhs.real()!=rhs.real()) return lhs.real()<rhs.real(); return lhs.imag()>rhs.imag(); }