//--------------------------------------------------------------------------------------------------------- public Bitmap DrawSketch(float n, Vector2 from, Vector2 to) { //我日,边排列的顺序是右手的,而顶点排列顺序是左手的,操! //LOS_RESULT ret = geo.LOS_Test( // polys[254].edges[2].from.ToVector2(), // polys[254].edges[1].from.ToVector2(), // out wp); LOS_RESULT ret = geo.LOS_Test(from, to, out wp); Bitmap bmp = new Bitmap((int)maxx + 5, (int)maxy + 5, System.Drawing.Imaging.PixelFormat.Format32bppArgb); using (Graphics g = Graphics.FromImage(bmp)) { Brush brush = new SolidBrush(Color.Black); g.FillRectangle(brush, new Rectangle(0, 0, (int)maxx + 5, (int)maxy + 5)); brush.Dispose(); int H = (int)maxy + 1; using (Pen pen = new Pen(Color.Red)) { for (int i = 0; i < num_polys; i++) { DrawPolygonEdges(g, pen, i); } } using (Pen pen = new Pen(Color.White)) { for (int i = 0; i < num_polys; i++) { if (!polys[i].enterable) { DrawPolygonEdges(g, pen, i); } } } Font font = new Font("Tahoma", 8); brush = new SolidBrush(Color.White); string msg = string.Format("Points:{0} Edges:{1} Polygons:{2} xPolygons:{3}", num_points, num_edges, num_polys, num_xpolys); g.DrawString(msg, font, brush, 2, 2); font.Dispose(); brush.Dispose(); DrawRayPath(g); //把路径画出来! //把测试多边形的AABB画出来! //for (int i = 0; i < num_polys; i++) DrawAABB(g, i); DrawAABB(g, 1); } return(bmp); }
//调用该函数时,在大多数情况下,p0在target的内部、边或点上 LOS_RESULT StartLOS_P(Vector2 n0, Vector2 p0, Polygon target, int dstidx) { LOS_RESULT los = LOS_RESULT.FAILED; if (target.idx == dstidx) { los = LOS_RESULT.CLEAR; } else //保证src与dst点不处于同一多边形 { int i; for (i = 0; i < target.num_edges; i++) { Edge edge = target.edges[i]; Vector2 from = edge.from.ToVector2() - p0; Vector2 to = edge.to.ToVector2() - p0; from.Normalize(); to.Normalize(); float ra = Vector2.Ccw(from, n0); float rb = Vector2.Ccw(to, n0); bool ta = FloatEqual(ra, 0); //V cross V = 零矢量 bool tb = FloatEqual(rb, 0); /* * //特殊情况1:穿出视线与多边形某边完全重合,计算精度导致 * if (ta && tb) //from/to/p0-n0三者共线 * { * if (edge.neighbor_poly == dstidx || edge.belong_poly==dstidx ) * { * los = LOS_RESULT.CLEAR; * break; * } * else * { * Vector2 pos = new Vector2(); * * float da = Vector2.Dot(from, n0); * float db = Vector2.Dot(to, n0); * * if (FloatEqual(da, 1)) //from与n0同向 * pos = edge.from.ToVector2(); * else if (FloatEqual(db, 1)) //to与n0同向 * pos = edge.to.ToVector2(); * else * Debug.Assert(false); //应该不可能 * * w.Add(new WayPoint(pos, edge.belong_poly)); //最好取e.belong_poly,取e.neighbor_poly有可能==-1 * PrintDebugPosition(edge.belong_poly, pos); * * los = StartLOS_V(n0, pos, dstidx); * break; * } * } * * //特殊情况2:穿出点正好是多边形的一个顶点 * //if (ta || tb) :这句话导致死循环,它没有排除射入边的情况 * if ((ta && rb < 0) || (ra > 0 && tb)) //ta || tb 且保证边为穿出边 * { * Vector2 pos = new Vector2(); * if (ta) pos = edge.from.ToVector2(); * if (tb) pos = edge.to.ToVector2(); * * w.Add(new WayPoint(pos, edge.belong_poly)); //最好取e.belong_poly,取e.neighbor_poly有可能==-1 * PrintDebugPosition(edge.belong_poly, pos); * * los = StartLOS_V(n0, pos, dstidx); * break; * } */ //普通情况:穿出点在多边形的边上 if (ra > 0 && rb < 0) //找到穿出边,即from在n0左边,to在n0右边 { //在确信射线与穿出边相交后,求其交点 Vector2 pos = CrossedRayLineSegIntersection(n0, p0, edge.from.ToVector2(), edge.to.ToVector2()); w.Add(new WayPoint(pos, edge.belong_poly)); PrintDebugPosition(edge.belong_poly, pos); if (!edge.wall) //穿出边是可以穿透的么? { Polygon newtarget = polys[edge.neighbor_poly]; if (newtarget.idx == dstidx) { los = LOS_RESULT.CLEAR; } else { los = StartLOS_P(n0, pos, newtarget, dstidx); //ok } } else { los = LOS_RESULT.BLOCKED; } break; //只有一根穿出边,找到了就over了 } } Debug.Assert(i != target.num_edges); //确信调用该函数时,p0始终在target的内部、边上或顶点上 } return(los); }
//--------------------------------------------------------------------------------------------------------- //from:起点;to:终点;waypoint:所有视线途经位点 public LOS_RESULT LOS_Test(Vector2 from, Vector2 to, out List <WayPoint> waypoint) { Belong_Status ret; Debug.WriteLine("----------------------------------------------------"); //////////////////////////////////////////////////////////////////////////////////////// Polygon dstpoly; Debug.Write("(目标) "); ret = RelaxPostion(ref to, out dstpoly); //松弛目标点 if (ret != Belong_Status.Polygon || !dstpoly.enterable) { waypoint = null; return(LOS_RESULT.FAILED); //目标点不合法 } int dstidx = dstpoly.idx; //关键!目标点所处的多边形 //////////////////////////////////////////////////////////////////////////////////////// Polygon srcpoly; Debug.Write("(源点) "); ret = RelaxPostion(ref from, out srcpoly); //松弛源点 if (ret != Belong_Status.Polygon || !srcpoly.enterable) { waypoint = null; return(LOS_RESULT.FAILED); //源点不合法 } //////////////////////////////////////////////////////////////////////////////////////// w = new List <WayPoint>(); w.Add(new WayPoint(from, -1)); //这是对的! PrintDebugPosition(-1, from); //////////////////////////////////////////////////////////////////////////////////////// Vector2 n0 = to - from; n0.Normalize(); //////////////////////////////////////////////////////////////////////////////////////// LOS_RESULT los = LOS_RESULT.FAILED; if (!FloatEqual(n0.LengthSq(), 0)) //from/to两点不容许重合 { Debug.Assert(ret == Belong_Status.Polygon); los = StartLOS_P(n0, from, srcpoly, dstidx); //此刻from在srcpoly内部 } if (los == LOS_RESULT.CLEAR) { w.Add(new WayPoint(to, dstidx)); PrintDebugPosition(dstidx, to); } Debug.WriteLine(string.Format("({0}) 共计 {1} 个位点", los.ToString(), w.Count)); Debug.WriteLine("----------------------------------------------------"); waypoint = w; return(los); }