exmath.zip_exmath_几个几何运算的小工具_线段关系

#include "StdAfx.h" #include "AlgUtil.h" #include <math.h> int AUtil_IsPointInPathBlocks(LPPOINTD blocks, int blocks_used_cnt, double safe_space, POINTD point, double point_radius) { for (int i = 0; i < blocks_used_cnt - 1; i++) { POINTD p1 = blocks[i]; POINTD p2 = blocks[i + 1]; VRECT vr1 = {0}; AUtil_VRECTFrom2Points(p1, p2, safe_space, safe_space, &vr1); VRECT vr2 = {0}; vr2.p1.x = point.x - point_radius;vr2.p1.y = point.y - point_radius; vr2.p2.x = point.x + point_radius;vr2.p2.y = point.y - point_radius; vr2.p3.x = point.x + point_radius;vr2.p3.y = point.y + point_radius; vr2.p4.x = point.x - point_radius;vr2.p4.y = point.y + point_radius; if (AUtil_IsVRECTOverlap(vr1, vr2)) { return i; } } return -1; } void AUtil_VRECTFrom2Points(POINTD p1, POINTD p2, double offsetRight, double offsetLeft, VRECT* rect) { memset(rect, 0, sizeof(VRECT)); VRECT vr = {0}; double d = (p1.x - p2.x) * (p1.x- p2.x) + (p1.y - p2.y) * (p1.y - p2.y); d = sqrt(d); double sina = (p2.y - p1.y) / d; double cosa = (p2.x - p1.x) / d; vr.p1.x = p2.x - (double)(offsetLeft * sina); vr.p1.y = p2.y + (double)(offsetLeft * cosa); vr.p2.x = p2.x + (double)(offsetRight * sina); vr.p2.y = p2.y - (double)(offsetRight * cosa); vr.p3.x = p1.x + (double)(offsetRight * sina); vr.p3.y = p1.y - (double)(offsetRight * cosa); vr.p4.x = p1.x - (double)(offsetLeft * sina); vr.p4.y = p1.y + (double)(offsetLeft * cosa); memcpy(rect, &vr, sizeof(VRECT)); } double AUtil_GetTriangleSquar(POINTD pt0, POINTD pt1, POINTD pt2) { double a = (pt0.x - pt1.x) * (pt0.x - pt1.x) + (pt0.y - pt1.y) * (pt0.y - pt1.y); double b = (pt0.x - pt2.x) * (pt0.x - pt2.x) + (pt0.y - pt2.y) * (pt0.y - pt2.y); double c = (pt1.x - pt2.x) * (pt1.x - pt2.x) + (pt1.y - pt2.y) * (pt1.y - pt2.y); a = sqrt(a); b = sqrt(b); c = sqrt(c); double p = (a + b + c ) / 2.0; double s = sqrt(p * (p - a) * (p - b) * (p - c)); return (double)s; } BOOL AUtil_IsInTriangle(POINTD A, POINTD B, POINTD C, POINTD D) { double SABC, SADB, SBDC, SADC; SABC = AUtil_GetTriangleSquar(A, B, C); SADB = AUtil_GetTriangleSquar(A, D, B); SBDC = AUtil_GetTriangleSquar(B, D, C); SADC = AUtil_GetTriangleSquar(A, D, C); double sumSquar = SADB + SBDC + SADC; double d = fabs(sumSquar - SABC); return d < 32.0f; } BOOL AUtil_FindLineIntersection(POINTD start1, POINTD end1, POINTD start2, POINTD end2, POINTD* inters) { memset(inters, 0, sizeof(POINTD)); double denom = ((end1.x - start1.x) * (end2.y - start2.y)) - ((end1.y - start1.y) * (end2.x - start2.x)); // AB & CD are parallel if (denom == 0) { return FALSE; } double numer = ((start1.y - start2.y) * (end2.x - start2.x)) - ((start1.x - start2.x) * (end2.y - start2.y)); double r = numer / denom; double numer2 = ((start1.y - start2.y) * (end1.x - start1.x)) - ((start1.x - start2.x) * (end1.y - start1.y)); double s = numer2 / denom; if ((r < 0 || r > 1) || (s < 0 || s > 1)) { return FALSE; } // Find intersection point inters->x = start1.x + (r * (end1.x - start1.x)); inters->y = start1.y + (r * (end1.y - start1.y)); return TRUE; } //BOOL AUtil_VRECT_In_VRECT(VRECT vr1, VRECT vr2) BOOL AUtil_IsVRECTOverlap(VRECT vr1, VRECT vr2) { POINTD pts1[5] = {0}; pts1[0] = vr1.p1; pts1[1] = vr1.p2; pts1[2] = vr1.p3; pts1[3] = vr1.p4; pts1[4] = vr1.p1; POINTD pts2[5] = {0}; pts2[0] = vr2.p1; pts2[1] = vr2.p2; pts2[2] = vr2.p3; pts2[3] = vr2.p4; pts2[4] = vr2.p1; for(int i = 0; i < 4; i++) { for(int k = 0; k < 4; k++) { POINTD intp={0}; BOOL intt = AUtil_FindLineIntersection(pts1[i], pts1[i + 1], pts2[k], pts2[k+1], &intp); if(intt) return TRUE; } } for (int i = 0; i < 4; i++) { BOOL overlap1 = AUtil_IsInTriangle(vr2.p1, vr2.p2, vr2.p3, pts1[i]); BOOL overlap2 = AUtil_IsInTriangle(vr2.p1, vr2.p4, vr2.p3, pts1[i]); BOOL overlap3 = AUtil_IsInTriangle(vr1.p2, vr1.p1, vr1.p4, pts2[i]); BOOL overlap4 = AUtil_IsInTriangle(vr1.p2, vr1.p3, vr1.p4, pts2[i]); if(overlap1 || overlap2 || overlap3 || overlap4) { return TRUE; } } double xx1 = (vr1.p1.x + vr1.p2.x + vr1.p3.x + vr1.p4.x) / 4.0f; double yy1 = (vr1.p1.y + vr1.p2.y + vr1.p3.y + vr1.p4.y) / 4.0f; double xx2 = (vr2.p1.x + vr2.p2.x + vr2.p3.x + vr2.p4.x) / 4.0f; double yy2 = (vr2.p1.y + vr2.p2.y + vr2.p3.y + vr2.p4.y) / 4.0f; POINTD pf11 = {0}; pf11.x = xx1;pf11.y = yy1; POINTD pf22 = {0}; pf22.x = xx2;pf22.y = yy2; if(AUtil_IsInTriangle(vr1.p2, vr1.p3, vr1.p4, pf22)) return TRUE; if(AUtil_IsInTriangle(vr1.p2, vr1.p1, vr1.p4, pf22)) return TRUE; if(AUtil_IsInTriangle(vr2.p1, vr2.p2, vr2.p3, pf11)) return TRUE; if(AUtil_IsInTriangle(vr2.p1, vr2.p4, vr2.p3, pf11)) return TRUE; return FALSE; } //BOOL AUtil_IsVRECTOverlap(VRECT vr1, VRECT vr2) //{ // POINTD pts1[5] = {0}; // pts1[0] = vr1.p1; // pts1[1] = vr1.p2; // pts1[2] = vr1.p3; // pts1[3] = vr1.p4; // pts1[4] = vr1.p1; // // POINTD pts2[5] = {0}; // pts2[0] = vr2.p1; // pts2[1] = vr2.p2; // pts2[2] = vr2.p3; // pts2[3] = vr2.p4; // pts2[4] = vr2.p1; // // // for (int i = 0; i < 4; i++) // { // for (int k = 0; k < 4; k++) // { // POINTD intp; // BOOL overlap = AUtil_FindLineIntersection(pts1[i], pts1[i + 1], pts2[k], pts2[k + 1], &intp); // if (overlap) return TRUE; // } // } // // for (int i = 0; i < 4; i++) // { // BOOL overlap = FALSE; // overlap = AUtil_IsInTriangle(vr1.p1, vr1.p2, vr1.p3, pts2[i]); // if (overlap) return TRUE; // // overlap = AUtil_IsInTriangle(vr1.p1, vr1.p4, vr1.p3, pts2[i]); // if (overlap) return TRUE; // // overlap = AUtil_IsInTriangle(vr2.p1, vr2.p2, vr2.p3, pts1[i]); // if (overlap) return TRUE; // // overlap = AUtil_IsInTriangle(vr2.p1, vr2.p4, vr2.p3, pts1[i]); // if (overlap) return TRUE; // } // // return FALSE; //} void AUtil_GetRectForm(double vx, double vy, double angle, double offsetFront, double offsetBack, double offsetLeft, double offsetRight, VRECT* rect) { memset(rect, 0, sizeof(VRECT)); double arc = 3.1415926 * angle / 180; POINTD pp1 = {0}; pp1.x = (double)(vx + ((double)offsetFront) * cos(arc)); pp1.y = (double)(vy + ((double)offsetFront) * sin(arc)); POINTD pp2 = {0}; pp2.x = (double)(vx + ((double)offsetRight) * sin(arc)); pp2.y = (double)(vy - ((double)offsetRight) * cos(arc)); POINTD pp3 = {0}; pp3.x = (double)(vx - ((double)offsetBack) * cos(arc)); pp3.y = (double)(vy - ((double)offsetBack) * sin(arc)); POINTD pp4 = {0}; pp4.x = (double)(vx - ((double)offsetLeft) * sin(arc)); pp4.y = (double)(vy + ((double)offsetLeft) * cos(arc)); rect->p1.x = pp1.x + pp4.x - (double)vx; rect->p1.y = pp1.y + pp4.y - (double)vy; rect->p2.x = pp1.x + pp2.x - (double)vx; rect->p2.y = pp1.y + pp2.y - (double)vy; rect->p3.x = pp3.x + pp2.x - (double)vx; rect->p3.y = pp3.y + pp2.y - (double)vy; rect->p4.x = pp3.x + pp4.x - (double)vx; rect->p4.y = pp3.y + pp4.y - (double)vy; } void AUtil_FindPoint2LinePedal(POINTD line_point1, POINTD line_point2, POINTD pt, POINTD* pedal) { memset(pedal, 0, sizeof(POINTD)); double x1 = line_point1.x; double y1 = line_point1.y; double x2 = line_point2.x; double y2 = line_point2.y; double x = pt.x; double y = pt.y; if(fabs(x1 - x2) < 0.0001f) { pedal->x = x1; pedal->y = y; return; } if(fabs(y1 - y2) < 0.0001f) { pedal->x = x;