| | |
| | | |
| | | MakeLine(&axial, &car->carXY[car->axial[AXIAL_FRONT]], &car->carXY[car->axial[AXIAL_REAR]]); |
| | | |
| | | MakeLine(&start, &map->point[calcZone.leftStart], &map->point[calcZone.rightStart]); |
| | | MakeLine(&end, &map->point[calcZone.leftEnd], &map->point[calcZone.rightEnd]); |
| | | DEBUG("START 线 %d -- %d", calcZone.leftStart, calcZone.rightStart); |
| | | DEBUG("END 线 %d -- %d", calcZone.leftEnd, calcZone.rightEnd); |
| | | |
| | | MakeLine(&start, &map->point[calcZone.leftStart], &map2->point[calcZone.rightStart]); |
| | | |
| | | if (IntersectionOf(start, axial) == GM_None) { |
| | | // 向起点查找 |
| | | do { |
| | | if (calcZone.leftStart == 0 && calcZone.rightStart == 0) { |
| | | break; |
| | | } |
| | | int tempLeft = calcZone.leftStart, tempRight = calcZone.rightStart; |
| | | |
| | | while (calcZone.leftStart > 0 || calcZone.rightStart > 0) { |
| | | if (calcZone.leftStart > 0) |
| | | calcZone.leftStart--; |
| | | tempLeft = calcZone.leftStart - 1; |
| | | if (calcZone.rightStart > 0) |
| | | calcZone.rightStart--; |
| | | MakeLine(&start, &map->point[calcZone.leftStart], &map->point[calcZone.rightStart]); |
| | | } while (IntersectionOf(start, axial) == GM_None); |
| | | tempRight = calcZone.rightStart - 1; |
| | | DEBUG("START 向起点查找 %d -- %d", tempLeft, tempRight); |
| | | MakeLine(&start, &map->point[tempLeft], &map2->point[tempRight]); |
| | | |
| | | if (IntersectionOf(start, axial) == GM_Intersection) { |
| | | // 保持之前的线 |
| | | break; |
| | | } else { |
| | | calcZone.leftStart = tempLeft; |
| | | calcZone.rightStart = tempRight; |
| | | |
| | | if (calcZone.leftStart == calcZone.leftEnd && calcZone.rightStart == calcZone.rightEnd) { |
| | | // 车辆丢失,重新搜索 |
| | | Line cat; |
| | | int l = 0, r = 0; |
| | | |
| | | while (l < map->num || r < map2->num) { |
| | | MakeLine(&cat, &map->point[l], &map2->point[r]); |
| | | |
| | | if (IntersectionOf(cat, axial) == GM_Intersection) { |
| | | |
| | | } |
| | | |
| | | if (l < map->num) |
| | | l++; |
| | | if (r < map2->num) |
| | | r++; |
| | | } |
| | | } |
| | | } |
| | | } |
| | | } else { |
| | | // 向终点查找 |
| | | do { |
| | |
| | | calcZone.leftStart++; |
| | | if (calcZone.rightStart < map2->num) |
| | | calcZone.rightStart++; |
| | | MakeLine(&start, &map->point[calcZone.leftStart], &map->point[calcZone.rightStart]); |
| | | } while (IntersectionOf(start, axial) != GM_None); |
| | | DEBUG("START 向终点查找 %d -- %d", calcZone.leftStart, calcZone.rightStart); |
| | | MakeLine(&start, &map->point[calcZone.leftStart], &map2->point[calcZone.rightStart]); |
| | | } while (IntersectionOf(start, axial) == GM_Intersection); |
| | | } |
| | | |
| | | MakeLine(&end, &map->point[calcZone.leftEnd], &map2->point[calcZone.rightEnd]); |
| | | |
| | | if (IntersectionOf(end, axial) == GM_None) { |
| | | // 向终点查找 |
| | | do { |
| | | int tempLeft = calcZone.leftEnd, tempRight = calcZone.rightEnd; |
| | | while (calcZone.leftEnd < map->num || calcZone.rightEnd < map2->num) { |
| | | if (calcZone.leftEnd >= map->num && calcZone.rightEnd >= map2->num) { |
| | | break; |
| | | } |
| | | if (calcZone.leftEnd < map->num) |
| | | calcZone.leftEnd++; |
| | | tempLeft = calcZone.leftEnd + 1; |
| | | if (calcZone.rightEnd < map2->num) |
| | | calcZone.rightEnd++; |
| | | MakeLine(&end, &map->point[calcZone.leftEnd], &map->point[calcZone.rightEnd]); |
| | | } while (IntersectionOf(end, axial) == GM_None); |
| | | tempRight = calcZone.rightEnd + 1; |
| | | DEBUG("END 向终点查找 %d -- %d", tempLeft, tempRight); |
| | | MakeLine(&end, &map->point[tempLeft], &map2->point[tempRight]); |
| | | |
| | | if (IntersectionOf(end, axial) == GM_Intersection) { |
| | | // 保持之前的线 |
| | | break; |
| | | } else { |
| | | calcZone.leftEnd = tempLeft; |
| | | calcZone.rightEnd = tempRight; |
| | | } |
| | | } |
| | | } else { |
| | | // 向起点查找 |
| | | do { |
| | |
| | | calcZone.leftEnd--; |
| | | if (calcZone.rightEnd > 0) |
| | | calcZone.rightEnd--; |
| | | MakeLine(&end, &map->point[calcZone.leftEnd], &map->point[calcZone.rightEnd]); |
| | | } while (IntersectionOf(end, axial) == GM_None); |
| | | DEBUG("END 向起点查找 %d -- %d", calcZone.leftEnd, calcZone.rightEnd); |
| | | MakeLine(&end, &map->point[calcZone.leftEnd], &map2->point[calcZone.rightEnd]); |
| | | } while (IntersectionOf(end, axial) == GM_Intersection); |
| | | } |
| | | |
| | | DEBUG("calcZone leftStart %d leftEnd %d rightStart %d rightEnd %d", calcZone.leftStart, calcZone.leftEnd, calcZone.rightStart, calcZone.rightEnd); |
| | | |
| | | if (calcZone.leftStart <= calcZone.leftEnd || calcZone.rightStart <= calcZone.rightEnd) { |
| | | // 离开场地 |
| | | DEBUG("离开曲线场地"); |
| | | testing = false; |
| | | } |
| | | |