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();
}
