1 // -------------------------------------------------------------------------
2 // 文件名 ： geometry.h
3 // 创建者 ： dingjie
4 // 创建时间 ： 2007-5-29
5 // 功能描述 ： 计算几何常用算法
6 //
7 // -------------------------------------------------------------------------
8 #ifndef __GEOMETRY_H__
9 #define __GEOMETRY_H__
10 11 #include <vector>
12 #include <iterator>
13 #include <math.h>
14 #include <vector>
15 #include <assert.h>
16 #include "Global.h"
17 18 #define MAGNETIC_DIST 0.003f
19 20 // -------------------------------------------------------------------------
21 // r=multiply(p0, p1, p2),得到(p2-p0)*(p1-p0)的叉积 ,根据叉积判断拐点走向
22 // r>0: p2在矢量p0p1的顺时针方向；
23 // r=0：p0p1p2三点共线；
24 // r<0: p2在矢量p0p1的逆时针方向
25 // -------------------------------------------------------------------------
26 inline
float Multiply(
const POINT2D& p0,
const POINT2D& p1,
const POINT2D& p2)
27 {
28 return((p2.x-p0.x)*(p1.y-p0.y)-(p1.x-p0.x)*(p2.y-p0.y));
29 }
30 // -------------------------------------------------------------------------
31 // 两点欧式距离
32 // -------------------------------------------------------------------------
33 inline
float Dist(
const POINT2D& p1,
const POINT2D& p2)
34 {
35 return(sqrt((p1.x-p2.x)*(p1.x-p2.x)+(p1.y-p2.y)*(p1.y-p2.y)));
36 }
37 38 // -------------------------------------------------------------------------
39 // 求p0到经p1p2确定的直线的距离
40 // -------------------------------------------------------------------------
41 inline
float PtToLineDist(
const POINT2D& p0,
const POINT2D& p1,
const POINT2D& p2)
42 {
43 return fabs(Multiply(p0, p1, p2))/Dist(p1, p2);
44 }
45 // -------------------------------------------------------------------------
46 // 判断两条线段是否有交点
47 // -------------------------------------------------------------------------
48 inline BOOL LineSegIntersect(
const LINESEG& ls1,
const LINESEG& ls2)
49 {
50 return((max(ls1.s.x, ls1.e.x)>=min(ls2.s.x, ls2.e.x))&&
//排斥测试
51 (max(ls2.s.x, ls2.e.x)>=min(ls1.s.x, ls1.e.x))&&
52 (max(ls1.s.y, ls1.e.y)>=min(ls2.s.y, ls2.e.y))&&
53 (max(ls2.s.y, ls2.e.y)>=min(ls1.s.y, ls1.e.y))&&
54 (Multiply(ls2.s, ls1.e, ls1.s)*Multiply(ls1.e, ls2.e, ls1.s)>=0)&&
//跨立测试
55 (Multiply(ls1.s, ls2.e, ls2.s)*Multiply(ls2.e, ls1.e, ls2.s)>=0));
56 }
57 58 inline BOOL LineSegIntersect(
const POINT2D& pt1,
const POINT2D& pt2,
59 const POINT2D& pt3,
const POINT2D& pt4)
60 {
61 return((max(pt1.x, pt2.x)>=min(pt3.x, pt4.x))&&
//排斥测试
62 (max(pt3.x, pt4.x)>=min(pt1.x, pt2.x))&&
63 (max(pt1.y, pt2.y)>=min(pt3.y, pt4.y))&&
64 (max(pt3.y, pt4.y)>=min(pt1.y, pt2.y))&&
65 (Multiply(pt3, pt2, pt1)*Multiply(pt2, pt4, pt1)>=0)&&
//跨立测试
66 (Multiply(pt1, pt4, pt3)*Multiply(pt4, pt2, pt3)>=0));
67 }
68 69 // -------------------------------------------------------------------------
70 // 如果返回值大于0，则p0在p1->p2左边(逆时针)，如果小于0在右边，否则共线
71 // -------------------------------------------------------------------------
72 inline
float LeftOrRight(
const POINT2D& p0,
const POINT2D& p1,
const POINT2D& p2)
73 {
74 return Multiply(p1, p0, p2);
75 }
76 77 // -------------------------------------------------------------------------
78 // 根据已知两点坐标，求过这两点的直线解析方程： a*x+b*y+c = 0 (a >= 0)
79 // -------------------------------------------------------------------------
80 inline LINE GetLine(
const POINT2D& p1,
const POINT2D& p2)
81 {
82 LINE tl;
83 int sign = 1;
84 tl.a = p2.y - p1.y;
85 if( tl.a < 0)
86 {
87 sign = -1;
88 tl.a = sign * tl.a;
89 }
90 tl.b = sign * (p1.x - p2.x);
91 tl.c = sign * (p1.y * p2.x - p1.x * p2.y);
92 return tl;
93 }
94 95 // -------------------------------------------------------------------------
96 // 如果两条直线 l1, l2相交，返回TRUE，且返回交点p
97 // -------------------------------------------------------------------------
98 inline BOOL GetLineIntersect(
const LINE& l1,
const LINE& l2, POINT2D& p)
99 {
100 float d = l1.a * l2.b - l2.a * l1.b;
101 if (fabs(d) < MIN_VALUE)
// 不相交
102 {
103 return FALSE;
104 }
105 p.x = (l2.c * l1.b - l1.c * l2.b) / d;
106 p.y = (l2.a * l1.c - l1.a * l2.c) / d;
107 return TRUE;
108 }
109 // -------------------------------------------------------------------------
110 // 计算距离线段sp->ep距离为distance的平行线，正值的时候
111 // 取线段右边的，也就是多边形内的，负值的时候相反。
112 // -------------------------------------------------------------------------
113 inline LINE GetParallelLine(
const POINT2D& sp,
const POINT2D& ep,
float distance, BOOL& bSucceeded)
114 {
115 bSucceeded = TRUE;
116 LINE line1 = GetLine(sp, ep);
117 LINE line(line1.a, line1.b, 0);
118 if ((fabs(line.a) <= MIN_VALUE) && (fabs(line.b) <= MIN_VALUE))
119 {
120 bSucceeded = FALSE;
121 return line;
122 }
123 124 //设line1方程为ax+by+c=0,则平行线方程为ax+by+m=0
125 //距离d=fabs(m-c)/sqrt(a*a+b*b)
126 float m1, m2;
127 float f = distance * sqrt(line1.a * line1.a + line1.b * line1.b);
128 m1 = line1.c + f;
129 m2 = line1.c - f;
130 131 line.c = (ep.y - sp.y < 0) ? m1 : m2;
132 133 return line;
134 }
135 // -------------------------------------------------------------------------
136 // r=dotmultiply(p1,p2,op0),得到矢量(p1-p0)和(p2-p0)的点积，如果两个矢量都
137 // 非零矢量r<0:两矢量夹角为锐角；r=0：两矢量夹角为直角；r>0:两矢量夹角为钝角
138 // -------------------------------------------------------------------------
139 inline
float DotMultiply(
const POINT2D& p1,
const POINT2D& p2,
const POINT2D& p0)
140 {
141 return ((p1.x - p0.x) * (p2.x - p0.x) + (p1.y - p0.y) * (p2.y - p0.y));
142 }
143 // -------------------------------------------------------------------------
144 // 点和线段的位置关系
145 // -------------------------------------------------------------------------
146 inline
float Relation(
const POINT2D& p,
const LINESEG& l)
147 {
148 LINESEG tl(l.s, p);
149 return DotMultiply(tl.e, l.e, l.s) / (Dist(l.s, l.e) * Dist(l.s, l.e));
150 }
151 // -------------------------------------------------------------------------
152 // 求点p到线段l所在直线的垂足 P
153 // -------------------------------------------------------------------------
154 inline POINT2D Perpendicular(
const POINT2D& p,
const LINESEG& l)
155 {
156 float r = Relation(p, l);
157 POINT2D tp;
158 tp.x = l.s.x + r * (l.e.x - l.s.x);
159 tp.y = l.s.y + r * (l.e.y - l.s.y);
160 return tp;
161 }
162 // -------------------------------------------------------------------------
163 // 求点p到线段l的最短距离,并返回线段上距该点最近的点np
164 // 注意：np是线段l上到点p最近的点，不一定是垂足
165 // -------------------------------------------------------------------------
166 inline
float PtToLinesegDist(
const POINT2D& p,
const LINESEG& l, POINT2D &np)
167 {
168 float r = Relation(p, l);
169 if(r < 0)
170 {
171 np = l.s;
172 return Dist(p, l.s);
173 }
174 else if(r > 1)
175 {
176 np = l.e;
177 return Dist(p, l.e);
178 }
179 np = Perpendicular(p, l);
180 return Dist(p, np);
181 }
182 // -------------------------------------------------------------------------
183 // sp在平行线上的垂足,距离sp为distance的点
184 // -------------------------------------------------------------------------
185 inline POINT2D GetRightPt(
const POINT2D& sp,
const POINT2D& ep,
float distance)
186 {
187 POINT2D pt;
188 BOOL b;
189 LINE line = GetParallelLine(sp, ep, distance, b);
190 assert(b);
191 LINE ppline(line.b, -line.a, line.a*sp.y-line.b*sp.x);
//过sp点的垂线
192 b = GetLineIntersect(line, ppline, pt);
193 assert(b);
194 return pt;
195 }
196 // -------------------------------------------------------------------------
197 // 返回顶角在o点，起始边为os，终止边为oe的夹角
198 // (非U角，单位：弧度)
199 // -------------------------------------------------------------------------
200 inline
float GetAngle(
const POINT2D& o,
const POINT2D& s,
const POINT2D& e)
201 {
202 float cosfi, fi, norm;
203 float dsx = s.x - o.x;
204 float dsy = s.y - o.y;
205 float dex = e.x - o.x;
206 float dey = e.y - o.y;
207 208 cosfi = dsx * dex + dsy * dey;
209 norm = (dsx * dsx + dsy * dsy) * (dex * dex + dey * dey);
210 cosfi /= sqrt(norm);
211 212 if (cosfi >= 1.0 )
return 0;
213 if (cosfi <= -1.0 )
return PI;
214 215 fi = acos(cosfi);
216 return fabs(fi);
217 }
218 // -------------------------------------------------------------------------
219 // 计算pt1绕pt2逆时针旋转alpha弧度以后的点
220 // -------------------------------------------------------------------------
221 inline POINT2D PtRotate(
const POINT2D& pt1,
const POINT2D& pt2,
float alpha)
222 {
223 float cosine = cos(alpha);
224 float sine = sin(alpha);
225 POINT2D pt = {(pt1.x-pt2.x)*cosine-(pt1.y-pt2.y)*sine+pt2.x,
226 (pt1.x-pt2.x)*sine+(pt1.y-pt2.y)*cosine+pt2.y};
227 return pt;
228 }
229 // -------------------------------------------------------------------------
230 // 计算多边形面积(带符号)；输入顶点按顺时针排列时，
231 // 返回正值；否则返回负值
232 // -------------------------------------------------------------------------
233 inline
float PolygonArea(
const POLYGON2D& polygon)
234 {
235 int nCount = polygon.data.size();
236 if (nCount < 3)
237 return 0.0f;
238 float s = polygon.data[0].y * (polygon.data[1].x - polygon.data[nCount-1].x);
239 for (
int i = 1; i < nCount; i++)
240 {
241 s += polygon.data[i].y * (polygon.data[(i+1)%nCount].x - polygon.data[(i-1)].x);
242 }
243 return s / 2;
244 }
245 // -------------------------------------------------------------------------
246 // 测试多边形顶点序列按时针方向
247 // -------------------------------------------------------------------------
248 inline BOOL AntiClockwise(
const POLYGON2D& polygon)
249 {
250 return (PolygonArea(polygon) < 0);
251 }
252 // -------------------------------------------------------------------------
253 // 多边形是否自交叉
254 // -------------------------------------------------------------------------
255 inline BOOL SelfIntersect(
const POLYGON2D& polygon, DRECT& rect,
int& a,
int& b)
256 {
257 rect.left = rect.bottom = 1E5f;
258 rect.right = rect.top = 1E-5f;
259 const std::vector<POINT2D>& pts = polygon.data;
260 int nCount = pts.size();
261 262 for (
int i = 0; i < nCount; i ++)
263 {
264 rect.left = min(rect.left, pts[i].x);
265 rect.bottom = min(rect.bottom, pts[i].y);
266 rect.right = max(rect.right, pts[i].x);
267 rect.top = max(rect.top, pts[i].y);
268 for (
int j = i+2; j < nCount; j ++)
269 {
270 if ((0==i)&&((j+1)%nCount==0))
271 continue;
272 if (LineSegIntersect(pts[i], pts[(i+1)%nCount], pts[j], pts[(j+1)%nCount]))
273 {
274 a = i;
275 b = j;
276 return TRUE;
277 }
278 }
279 }
280 return FALSE;
281 }
282 283 inline BOOL SelfIntersect(
const POLYGON2D& polygon)
284 {
285 const std::vector<POINT2D>& pts = polygon.data;
286 if (pts.size()<=3)
287 return FALSE;
288 int nCount = pts.size();
289 290 for (
int i = 0; i < nCount-1; i ++)
291 {
292 for (
int j = i+2; j < nCount; j ++)
293 {
294 if ((j+1)%nCount==i)
295 continue;
296 if (LineSegIntersect(pts[i], pts[(i+1)%nCount], pts[j], pts[(j+1)%nCount]))
297 return TRUE;
298 }
299 }
300 return FALSE;
301 }
302 303 // -------------------------------------------------------------------------
304 // 根据顶点序列计算边距为distance的缓冲顶点的坐标
305 // -------------------------------------------------------------------------
306 inline BOOL GetBufPoints(std::vector<POINT2D>& bufpts,
const std::vector<POINT2D>& pts,
float distance)
307 {
308 if (fabs(distance)<MIN_VALUE)
309 {
310 bufpts = pts;
311 return TRUE;
312 }
313 314 int nCount = pts.size();
315 bufpts.resize(nCount);
316 317 for (
int i = 0; i < nCount; i ++)
318 {
319 int nPrev = (i+nCount-1)%nCount;
320 int nNext = (i+1)%nCount;
321 BOOL succeed;
322 LINE line1 = GetParallelLine(pts[nPrev], pts[i], distance, succeed);
323 assert(succeed);
324 LINE line2 = GetParallelLine(pts[i], pts[nNext], distance, succeed);
325 assert(succeed);
326 if (!succeed)
327 return FALSE;
328 BOOL b = GetLineIntersect(line1, line2, bufpts[i]);
329 //assert(b);
330 if (!b)
331 {
332 bufpts[i] = GetRightPt(pts[i], pts[nNext], distance);
333 }
334 }
335 return TRUE;
336 }
337 // -------------------------------------------------------------------------
338 // 设置多边形凹凸性
339 // -------------------------------------------------------------------------
340 inline
void SetPolyProt(POLYGON2D& polygon)
341 {
342 const std::vector<POINT2D>& pts = polygon.data;
343 int nCount = pts.size();
344 if (nCount<4)
345 {
346 polygon.type = 1;
347 return;
348 }
349 350 float a,b;
351 a = LeftOrRight(pts[0], pts[1], pts[2]);
352 for(
int i = 0; i < nCount; i ++)
353 {
354 int nNext = (i+1)%nCount;
355 int nNextNext = (nNext+1)%nCount;
356 b = LeftOrRight(pts[i], pts[nNext], pts[nNextNext]);
357 if(a * b < 0)
358 {
359 polygon.type = 0;
360 return;
361 }
362 }
363 polygon.type = 1;
364 }
365 // -------------------------------------------------------------------------
366 // 计算当前多边形即pPoints，第一个有顶点合并的等距线的距离
367 // bInside为True，取多边形内方向，反之取多边形外方向
368 // -------------------------------------------------------------------------
369 inline
float GetMinJoinWidth(
const std::vector<POINT2D>& pPoints, BOOL bInside,
int& idx)
370 {
371 int nCount = pPoints.size();
372 float minwidth = 1E10;
373 std::vector<POINT2D> bufpts;
374 GetBufPoints(bufpts, pPoints, 1);
375 for (
int i = 0; i < nCount; i ++)
376 {
377 int nNext = (i+1)%nCount;
378 LINE line1 = GetLine(bufpts[i], pPoints[i]);
379 LINE line2 = GetLine(bufpts[nNext], pPoints[nNext]);
380 POINT2D ispt;
381 BOOL bIntersect = GetLineIntersect(line1, line2, ispt);
382 if(!bIntersect)
383 continue;
384 385 float f = LeftOrRight(ispt, pPoints[i], pPoints[nNext]);
386 387 if (bInside)
388 {
389 if (f > 0)
continue;
//多边形内方向
390 }
391 else392 {
393 if (f < 0)
continue;
//多边形外方向
394 }
395 396 //求此点到直线距离
397 float d = PtToLineDist(ispt, pPoints[i], pPoints[nNext]);
398 399 if (d < minwidth)
400 {
401 minwidth = d;
402 idx = i;
403 }
404 }
405 406 return minwidth;
407 }
408 // -------------------------------------------------------------------------
409 // 根据四个控制点计算一条bezier曲线,hits为点数
410 // -------------------------------------------------------------------------
411 inline
void GetBezier(std::vector<POINT2D>& bezier,
const POINT2D cp[4],
int hits)
412 {
413 assert(hits>1);
414 415 bezier.resize(hits);
416 int i = 0;
417 float X,Y;
418 float x[4];
419 float y[4];
420 421 for(i = 0; i < 4; i ++)
422 {
423 x[i] = cp[i].x;
424 y[i] = cp[i].y;
425 }
426 427 float a0,a1,a2,a3,b0,b1,b2,b3,t,t2,t3,dt;
428 a0 = x[0];
429 a1 = -3 * x[0] + 3 * x[1];
430 a2 = 3 * x[0] - 6 * x[1] + 3 * x[2];
431 a3 = -x[0] + 3 * x[1] - 3 * x[2] + x[3];
432 b0 = y[0];
433 b1 = -3 * y[0] + 3 * y[1];
434 b2 = 3 * y[0] - 6 * y[1] + 3 * y[2];
435 b3 = -y[0] + 3 * y[1] - 3 * y[2] + y[3];
436 dt = 1.0f / (hits-1);
437 438 for (i = 0; i < (hits-1); i ++)
439 {
440 t = i * dt;
441 t2 = t * t;
442 t3 = t * t2;
443 X = (a0 + a1 * t + a2 * t2 + a3 * t3);
444 Y = (b0 + b1 * t + b2 * t2 + b3 * t3);
445 bezier[i].x = X;
446 bezier[i].y = Y;
447 };
448 bezier[hits-1].x = cp[3].x;
449 bezier[hits-1].y = cp[3].y;
450 return;
451 }
452 // -------------------------------------------------------------------------
453 // 合并vector里距离超近的相邻点，只保留其中一个，另一个删除
454 // -------------------------------------------------------------------------
455 inline BOOL ClipPolygon(std::vector<POINT2D>& pts)
456 {
457 //删除距离近的相邻顶点
458 int nPtCnt = pts.size();
459 BOOL bCliped = TRUE;
460 while (bCliped)
461 {
462 bCliped = FALSE;
463 int nCount = pts.size();
464 int nCurr = nCount - 1;
465 for (
int i = nCount - 1; i >= 0 ; i --)
466 {
467 if (pts.size()<=2)
468 {
469 return (nPtCnt > pts.size());
470 }
471 int nPrev = (nCurr+pts.size()-1)%pts.size();
472 float d = Dist(pts[nCurr], pts[nPrev]);
473 if (d <= MAGNETIC_DIST)
474 {
475 pts.erase(pts.begin() + nCurr);
476 bCliped = TRUE;
477 }
478 else479 {
480 int nNext = (nCurr+1)%pts.size();
481 float d = PtToLineDist(pts[nNext], pts[nCurr], pts[nPrev]);
482 if(d <= MAGNETIC_DIST)
// 接近平行
483 {
484 pts.erase(pts.begin() + nCurr);
485 bCliped = TRUE;
486 }
487 }
488 nCurr --;
489 if (nCurr == -1)
490 nCurr = pts.size() - 1;
491 }
492 }
493 return (nPtCnt > pts.size());
494 }
495 496 //判断点p是否在线段l上
497 inline
int OnLineseg(
const LINESEG l,
const POINT2D p)
498 {
499 return( (Multiply(l.e,p,l.s)==0)&&
500 (((p.x-l.s.x)*(p.x-l.e.x)<0 )||( (p.y-l.s.y)*(p.y-l.e.y)<0)));
501 }
502 //射线法判断点q与多边形polygon的位置关系，要求polygon为简单多边形，(顶点按逆时针排列?)
503 //如果点在多边形内： 返回2
504 //如果点在多边形边上： 返回1
505 //如果点在多边形外： 返回0
506 inline
int PtInPolygon(
const POLYGON2D& polygon,
const POINT2D& point)
507 {
508 int nCount = polygon.data.size();
509 const std::vector<POINT2D>& data = polygon.data;
510 int i,c=0;
511 LINESEG line,edge;
512 line.s = line.e = point;
513 line.s.x = 1e10;
514 for(i = 0; i < nCount; i++)
515 {
516 edge.s = data[i];
517 edge.e = data[(i+1)%nCount];
518 if(OnLineseg(edge,point))
519 return 1;
//如果点在边上，返回1
520 if (edge.s.y==edge.e.y)
//平行线，跳过
521 continue;
522 523 if(min(edge.s.y, edge.e.y)!=point.y && LineSegIntersect(line,edge))
524 c++;
525 }
526 if(c%2 == 1)
527 return 2;
528 else 529 return 0;
530 }
531 //反转多边形
532 inline
void Reverse(POLYGON2D& polygons)
533 {
534 POLYGON2D tmp = polygons;
535 polygons.data.clear();
536 std::copy(tmp.data.rbegin(), tmp.data.rend(), std::back_inserter(polygons.data));
537 }
538 539 #endif //__GEOMETRY_H__