Navmesh 寻路

用cocos2dx引擎简单实现了一下navmesh的多边形划分,然后基于划分多边形的a*寻路。以及路径拐点优化算法
用cocos主要是方便使用一些渲染接口和定时器。重点是实现的原理。

首先画了一个带有孔洞的多边形

//多边形的顶点数据
	Vec2(100, 100),
    Vec2(300, 200),
    Vec2(500, 50),
    Vec2(650, 300),
    Vec2(900, 200),
    Vec2(1100, 30),
    Vec2(1230, 400),
    Vec2(1050, 550),
    Vec2(850, 450),
    Vec2(700, 600),
    Vec2(500, 600),
    Vec2(420, 270),
    Vec2(350, 300),
    Vec2(250, 550),
    Vec2(200, 400),
    Vec2(50, 300),
//孔洞的顶点数据
	Vec2(550, 400),
    Vec2(970, 230),
    Vec2(800, 450),
    Vec2(700, 420),
    Vec2(580, 550),

    Vec2(150, 150),
    Vec2(250, 220),
    Vec2(200, 300)

注意都是逆时针顺序
在这里插入图片描述

孔洞和整体多边形都是一个双向链表结构

void PolygonClass::insert(int id, Vec2 pos) {
	Vertex* vert = new Vertex(id, pos);
	if (_head == nullptr) {
		_head = vert;
		_head->next = vert;
		_head->prev = vert;
	}else {
		Vertex* tail = _head->prev;
		tail->next = vert;
		vert->next = _head;
		vert->prev = tail;
		_head->prev = vert;
	}
}

链表的节点Vertex即是地图上各个点的数据

class Vertex
{
public:
	Vertex(int id, Vec2 pos) {
		this->id = id;
		this->pos = pos;
	}
	bool isSameVert(Vertex* vert) {
		return vert->id == id && vert->pos == pos;
	}
	bool isSamePos(Vertex* vert) {
		return vert->pos == pos;
	}
	int id;
	Vec2 pos;
	Vertex* next;
	Vertex* prev;
};

孔洞连接

带有孔洞的多边形,首先要将孔洞和整体多边形连接起来。

孔洞是否在多边形内部

我在连接前加入了一个孔洞是否包含在多边形内部的逻辑判断
即相当于判断孔洞的所有顶点是否都在多边形内部

// 判断多边形hole是否被polygon完全包含
bool PolygonClass::isInnerHole(PolygonClass* hole) {
	// hole的所有点都被polygon包含
	Vertex* cur = hole->getHead();
	if (_head == nullptr)  return false;
	do {
		if (!this->isInnerVert(cur)) return false;
		cur = cur->next;
	} while (!cur->isSameVert(hole->getHead()));
	return true;
}

判定一个坐标点是否在一个多边形内部:从坐标点向右发射一条水平射线,判定水平射线与多边形所有边的交点数目,奇数则在内部,偶数则在外部。

bool PolygonClass::isInnerVec2(Vec2 vec) {
	//从 顶点 向右 发射一条向右水平射线, 水平射线与所有边的交点数目,为奇数,则在内部, 为偶数,则在外部
	Vertex* cur = _head;
	int intersectLines = 0;
	do {
		if (GeometryMath::isRayIntersectLine(vec, cur->pos, cur->next->pos)) intersectLines++;
		cur = cur->next;
	} while (!cur->isSameVert(_head));
	return intersectLines % 2 == 1;
}

判断一个射线与一条线段相交

线段与射线的几种关系:
1.射线与线段水平或重合,视作不相交。
2.线段在射线的上方或下方,不相交。
3.线段下方端点在线上, 与 线段上方端点的在线上的情况,即线段只有一个端点在射线上,只能选择一种作为相交,另一种作为不相交,如图下,
在这里插入图片描述在这里插入图片描述
这两种情况,相邻两条线都是上端点或下端点在射线上,无论视作相不相交,都不影响结果的判断

在这里插入图片描述
但是如果是从中间穿过的话,一个是上端点在射线上,一个是下端点在射线上,只能取一种情况作为相交
4.线段两个端点分别在射线的上下方,则求线段在射线水平线上的x点,如果x点在向右的射线范围内则相交,否则不相交

bool GeometryMath::isRayIntersectLine(Vec2 vR, Vec2 vL1, Vec2 vL2) {
	//线段水平,与射线重合或平行
	if (vL1.y == vL2.y) return false;
	//线段在 水平射线上方
	if (vL1.y > vR.y && vL2.y > vR.y) return false;
	//线段在 水平射线下方
	if (vL1.y < vR.y && vL2.y < vR.y) return false;
	float minY = min(vL1.y, vL2.y);
	float maxY = min(vL1.y, vL2.y);

	/*线段下方端点在线上, 与 线段上方端点的在线上的情况,只能选择一种作为相交,另一种作为不相交
	否则射线穿过的顶点的相交判断会有问题
	即maxY 或者 minY 只选择一种做判断*/
	if (maxY == vR.y) return false;

	//线段两个端点分别在 射线的上下, 求线段在 射线的水平线上的x点,判断x点与 射线起点x轴坐标(射线方向向右)
	float offsetY = vL2.y - vR.y;
	float offsetX = offsetY / (vL2.y - vL1.y) * (vL2.x - vL1.x);
	float x = vL2.x - offsetX;
	return x >= vR.x;
}

连接孔洞

连接孔洞与多边形,在孔洞上寻找一点v1,然后在多边形上寻找一点v2,保证v1与v2的连线不与任何孔洞以及多边形的边相交(两点的相邻边除外),并且连线在v2为顶点的角的内部以及在v1为顶点的角的外部。

// 从hole 选择一个顶点pA
// 从polygon选择一个顶点pB,保证与pA连线不与polygon以及hole上的任意非相邻边相交
pair<Vertex*, Vertex*> NavMeshScene::getLinkHoleVertex(PolygonClass* polygon, PolygonClass* hole) {
    Vertex* vertHole = hole->getHead();
    do {
        Vertex* vertPolygon = polygon->getHead();
        do {
            bool isInAngle = polygon->isLineInInnerAngle(vertHole, vertPolygon) && !hole->isLineInInnerAngle(vertPolygon, vertHole);
            if (isInAngle) {
                bool noIntersect = polygon->isVectorNoIntersectWithAdjacentEdge(vertPolygon, vertHole) && hole->isVectorNoIntersectWithAdjacentEdge(vertPolygon, vertHole);
                for (auto hole = _holes.begin(); hole != _holes.end(); hole++) {
                    if (!(*hole)->isVectorNoIntersectWithAdjacentEdge(vertPolygon, vertHole)) {
                        noIntersect = false;
                        break;
                    }
                }
                if (noIntersect) return make_pair(vertPolygon, vertHole);
            }
            vertPolygon = vertPolygon->next;
        } while (!vertPolygon->isSameVert(polygon->getHead()));
        vertHole = vertHole->next;
    } while (!vertHole->isSameVert(hole->getHead()));
    return make_pair(nullptr, nullptr);
}

判断线段是否在角的内侧

bool PolygonClass::isLineInInnerAngle(Vertex* vertL1, Vertex* vertL2) {
	if (vertL1->isSamePos(vertL2)) return false;
	if (vertL1->isSamePos(vertL2->prev)) return false;
	if (vertL1->isSamePos(vertL2->next)) return false;
	Vec2 vecL2Prev = vertL2->prev->pos;
	Vec2 vecL2= vertL2->pos;
	Vec2 vecL2Next = vertL2->next->pos;
	Vec2 vecL1 = vertL1->pos;
	if (GeometryMath::isInLeft(vecL2Prev, vecL2, vecL2Next)) return GeometryMath::checkVectorInConvexAngle(vecL1, vecL2Prev, vecL2, vecL2Next);
	return GeometryMath::checkVectorInConcaveAngle(vecL1, vecL2Prev, vecL2, vecL2Next);
}
判断线段在劣角内部

在这里插入图片描述

// 小于180°的角 ∠abc,点a,点b,点c是逆时针,判定线vb在角内, 线需在ab的左边,并且在bc的左边
bool GeometryMath::checkVectorInConvexAngle(Vec2 v, Vec2 a, Vec2 b, Vec2 c) {
	return isInLeft(a, b, v) && isInLeft(b, c, v);
}
判断线段在优角内部
//大于180°的角 ∠abc,点a,点b,点c是逆时针,判定线vb在角内,即线不在∠abc的外侧,即线不在∠cba里
bool GeometryMath::checkVectorInConcaveAngle(Vec2 v, Vec2 a, Vec2 b, Vec2 c) {
	return !checkVectorInConvexAngle(v, c, b, a);
}

角度大小判断

通过角的两条的相邻边的叉积判断

// 点 c 在a到b向量的左边, 即∠abc 小于180°
bool GeometryMath::isInLeft(Vec2 a, Vec2 b, Vec2 c) {
	float e = getVectorCross(a, b, c);
	return getVectorCross(a, b, c) < 0;
}
// 点 c 在a到b向量的右边, 即∠abc 大于180°
bool GeometryMath::isInRight(Vec2 a, Vec2 b, Vec2 c) {
	return getVectorCross(a, b, c) > 0;
}
// 点 c 与a到b向量共线, 即∠abc 等于180°
bool GeometryMath::isCollineation(Vec2 a, Vec2 b, Vec2 c) {
	return getVectorCross(a, b, c) == 0;
}
int GeometryMath::getVectorCross(Vec2 a, Vec2 b, Vec2 c) {
	Vec2 vectorBA = a - b;
	Vec2 vectorBC = c - b;
	return vectorBA.cross(vectorBC);
}

判断线段是否与多边形非相邻边相交

bool PolygonClass::isVectorNoIntersectWithAdjacentEdge(Vertex* vertL1, Vertex* vertL2) {
	Vertex* cur = _head;
	do {
		if (!vertL1->isSamePos(cur) && !vertL1->isSamePos(cur->next) && !vertL2->isSamePos(cur) && !vertL2->isSamePos(cur->next)) {
			if (GeometryMath::checkTwoVectorIntersect(vertL1->pos, vertL2->pos, cur->pos, cur->next->pos)) return false;
		}
		cur = cur->next;
	} while (!cur->isSameVert(_head));
	return true;
}

判断两条线段是否相交

// 检查 vectorAB 与 vectorCD 相交
bool GeometryMath::checkTwoVectorIntersect(Vec2 va, Vec2 vb, Vec2 vc, Vec2 vd) {
	if (isStrictlyIntersect(va, vb, vc, vd)) {
		return true;
	}
	if (isVertexInLine(va, vb, vc)) { 
		return true; }
	if (isVertexInLine(va, vb, vd)) {
		return true;
	}
	if (isVertexInLine(vc, vd, va)) {
		return true;
	}
	if (isVertexInLine(vc, vd, vb)) {
		return true;
	}
	return false;
}

判断两条线段严格相交

其中一个线端有某个端点与另一条线段共线,或者一条线段两个端点都在另一条线段的另一边,则不严格相交

// 检查 vectorAB 与 vectorCD 严格相交, 点va,点vb在 线vectorCD 两侧, 且点vc,点vd 在线vectorAB 两侧
bool GeometryMath::isStrictlyIntersect(Vec2 va, Vec2 vb, Vec2 vc, Vec2 vd) {
	if (isCollineation(va, vb, vc)) return false;
	if (isCollineation(va, vb, vd)) return false;
	if (isCollineation(vc, vd, va)) return false;
	if (isCollineation(vc, vd, vb)) return false;
	if (isInLeft(va, vb, vc) == isInLeft(va, vb, vd)) return false;
	if (isInLeft(vc, vd, va) == isInLeft(vc, vd, vb)) return false;
	return true;
}
判断点在线段内
// vert 在 vectorL12 线段之间
bool GeometryMath::isVertexInLine(Vec2 vL1, Vec2 vL2, Vec2 vert) {
	if (!isCollineation(vL1, vL2, vert)) return false;
	if (vL1.x == vL2.x) return (vert.y >= min(vL1.y, vL2.y)) && (vert.y <= max(vL1.y, vL2.y));
	else return (vert.x >= min(vL1.x, vL2.x)) && (vert.x <= max(vL1.x, vL2.x));
}

获取到连线点v1,v2后,要复制两个连线点帮助接入孔洞顶点,孔洞的顶点要以顺时针的方式插入多边形的顶点。如图在这里插入图片描述

void NavMeshScene::linkHole() {
    while (!_holes.empty()) {
        PolygonClass* hole = _holes[0];
        _holes.erase(_holes.begin());
        if (_polygons.front()->isInnerHole(hole)) {
            auto verts = getLinkHoleVertex(_polygons.front(), hole);
            if (verts.second == nullptr || verts.first == nullptr) _holes.push_back(hole);
            else {
                auto drawNode = DrawNode::create();
                drawNode->drawSegment(verts.first->pos, verts.second->pos, 0.5, Color4F(1, 0, 0, 1));
                this->addChild(drawNode);

                Vertex* vPolygon = verts.first;
                Vertex* vertPolygonNext = vPolygon->next;

                //hole的顶点需要顺时针插入polygon
                Vertex* vHole = verts.second;
                do {
                    Vertex* vert = new Vertex(vHole->id, vHole->pos);
                    vPolygon->next = vert;
                    vert->prev = vPolygon;
                    vPolygon = vert;
                    vHole = vHole->prev;
                } while (!vHole->isSameVert(verts.second));

                _newPointId++;
                Vertex* newVHole = new Vertex(_newPointId, verts.second->pos);
                newVHole->prev = vPolygon;
                vPolygon->next = newVHole;

                _newPointId++;
                Vertex* newVPolygon = new Vertex(_newPointId, verts.first->pos);
                newVHole->next = newVPolygon;
                newVPolygon->prev = newVHole;
                newVPolygon->next = vertPolygonNext;
                vertPolygonNext->prev = newVPolygon;

                float len = verts.first->pos.getDistance(verts.second->pos);
                _linkHoleLines.emplace(new Line(verts.first, verts.second, len, drawNode));

                delete hole;
                hole = nullptr;
            }
        }
    }
    _isLinkHole = true;
}

请添加图片描述

分割凸多边形

找到多边形上的一个凹点,在寻找多边形内部与该凹点不相邻的一点,确保两点的连线完全在多边形内部,将此作为分割线把多边形分割成两个新的多边形。再对新分割出来的多边形进行循环操作,直到新分割的多边形里找不到凹点,即为凸多边形。

寻找分割点

分割线在多边形内部的判断:
1.即为分割线在以两个分割点为顶点的角的内部
2.分割线不与多边形任一条边相交(分割点相邻边除外)。
注意这里有一个重点,如果是有孔洞的多边形,经过上文的孔洞连线处理。会有同位置的重复顶点。如果这个顶点刚好是凹点,做非相邻边判断时要特殊处理。
在这里插入图片描述

依旧是这张图,连接孔洞划分出的∠AFG,如果是凹点,那需要从F点出发找内部对角线。A′ F′ 不算F点的相邻边,但是又和F点同位置。此时如果分割线是往上,与A′ F′ 是不相交,如果是往下,与A′ F′ 又应该是相交的。
如果凹角是公共点,那新的内部对角线判断和其他边相交的时候,要忽略复制点连接的两条线。
例如从F出发寻找的内部对角线,相交线判断要忽略IF′ 和F′ A′ ,因为分割线必须在∠AFG内
如果A′ 是凹点,从A′ 出发的线,相交线判断要忽略EA和AF,因为线必须在∠F′ A′ B

//找到一个凹角顶点a
//从点a 找到一个 不相邻点b 且能连接成完全在内部的对角线,
//因为a为凹角,所以从a出发一定在∠a内, 则需要对角线在目标点的∠B内,且不与任何非相邻边相交
pair<Vertex*, Vertex*> PolygonClass::findClipVert() {
	auto srcVert = findConcaveVertex();
	if (srcVert == nullptr) {
		return make_pair(nullptr, nullptr);
	}
	auto tarVert = srcVert->next->next;
	do {
		if (!tarVert->isSamePos(srcVert) && !tarVert->isSamePos(srcVert->next) && !tarVert->isSamePos(srcVert->prev)) {
			bool isInAngle = isLineInInnerAngle(srcVert, tarVert) && isLineInInnerAngle(tarVert, srcVert);
			bool isNoIntersect = isVectorNoIntersectWithAdjacentEdge(srcVert, tarVert);
			if (isInAngle && isNoIntersect) return make_pair(srcVert, tarVert);
		}
		tarVert = tarVert->next;
	} while (!tarVert->isSameVert(srcVert->prev));
	return make_pair(nullptr, nullptr);
}

形成新的多边形

注意,在分割完所有凸多边形后,把之前连接孔洞的线也加入分割线,作为下一步合并多边形的判断

void NavMeshScene::clipPolygon() {
    while (!_polygons.empty()) {
        PolygonClass* polygon = _polygons.front();
        _polygons.pop();
        auto verts = polygon->findClipVert();
        if (verts.first == nullptr || verts.second == nullptr) {
            //凸边形
            polygon->setConvexPolygonId(_convexPolygonId);
            _convexPolygons.emplace(_convexPolygonId, polygon);
            _convexPolygonId++;

        }
        else {
            auto drawNode = DrawNode::create();
            this->addChild(drawNode);
            drawNode->drawSegment(verts.first->pos, verts.second->pos, 0.5, Color4F(0, 0, 0, 1));
            float len = verts.first->pos.getDistance(verts.second->pos);
            _clipLines.push(new Line(verts.first, verts.second, len, drawNode));

            auto tarPrev = verts.second->prev;
            auto srcNext = verts.first->next;
            verts.first->next = verts.second;
            verts.second->prev = verts.first;

            polygon->setHead(verts.first);

            PolygonClass* newPolygon = new PolygonClass();
            Vertex* tail = new Vertex(verts.first->id, verts.first->pos);
            tail->next = srcNext;
            srcNext->prev = tail;
            Vertex* head = new Vertex(verts.second->id, verts.second->pos);
            head->prev = tarPrev;
            head->next = tail;
            tarPrev->next = head;
            tail->prev = head;
            newPolygon->setHead(head);
            _polygons.push(polygon);
            _polygons.push(newPolygon);
        }
    }

    _isClipPolygon = true;
     // 在分割完所有凸多边形后,把连接孔洞的线也加入分割线,作为合并多边形的判断
    while (!_linkHoleLines.empty()) {
        _clipLines.push(_linkHoleLines.front());
        _linkHoleLines.pop();
    }
}

请添加图片描述

合并多边形

前一步的分割,在寻找分割点的时候并没有做特殊处理,因此可能出现在一个凹点,分割了多条线才使凹点小于180°的情况。而其中的某些分割线在去掉后,新的顶点角仍是凸点。
合并分割边的顺序是根据线段长度来判断,长度最长的优先进行能否合并判断,以尽量减少狭长的多边形。

判断分割线能否去除

在这里插入图片描述

如图,根据分割边bc,找到相邻的两个多边形A和B,找到多边形A中分割点b的前一个顶点a,以及多边形B中分割点b的下一个顶点d,可判断若合并之后的∠abd是否小于180°。同理对另一个分割点c做相同判断

//获取分割边分割的凸多边形
tuple<PolygonClass*, PolygonClass*, Vertex*, Vertex*> NavMeshScene::getMergeInfo(Line* line) {
    PolygonClass* firstPolygon = nullptr;
    PolygonClass* secondPolygon = nullptr;
    Vertex* vert1 = nullptr;
    Vertex* vert2 = nullptr;
    for (auto it : _convexPolygons) {
        Vertex* linePrev = it.second->getLinePrevVert(line);
        if (linePrev != nullptr) {
            if (firstPolygon == nullptr) {
                firstPolygon = it.second;
                vert1 = linePrev;
            }
            else {
                secondPolygon = it.second;
                vert2 = linePrev;
                break;
            }
        }
    }
    //能合并最大多边形判断
    if (firstPolygon->getSize() + secondPolygon->getSize() - 2 > PolygonMaxVertex) return make_tuple(nullptr, nullptr, nullptr, nullptr);
    if (!checkMergeLineAngle(vert1, vert2)) return make_tuple(nullptr, nullptr, nullptr, nullptr);
    return make_tuple(firstPolygon, secondPolygon, vert1, vert2);
}

//判断分割边分割的两个凸多边形合并仍是凸多边形, 即分割边两个共享点在合并的凸多边形里是 凸点
bool NavMeshScene::checkMergeLineAngle(Vertex* linePrevVert1, Vertex* linePrevVert2) {
    //求如果合并的新凸多边形, 公共边对应的两个角的顶点
    Vec2 v1Prev = linePrevVert1->pos;
    Vec2 v1 = linePrevVert1->next->pos;
    Vec2 v1Next = linePrevVert2->next->next->next->pos;
    Vec2 v2Prev = linePrevVert2->pos;
    Vec2 v2 = linePrevVert2->next->pos;
    Vec2 v2Next = linePrevVert1->next->next->next->pos;
    if (GeometryMath::isInRight(v1Prev, v1, v1Next)) return false;
    if (GeometryMath::isInRight(v2Prev, v2, v2Next)) return false;
    return true;
}

合并成新多边形

void NavMeshScene::mergePolygon() {
    while (!_clipLines.empty()) {
        // 获取最长的分割边
        auto line = _clipLines.top();
        _clipLines.pop();

        PolygonClass* firstPolygon;
        PolygonClass* secondPolygon;
        Vertex* vert1;
        Vertex* vert2;
        tie(firstPolygon, secondPolygon, vert1, vert2) = getMergeInfo(line);
        if (firstPolygon == nullptr) {
            _closeClipLines.push_back(line);
        }
        else {
            _convexPolygons.erase(secondPolygon->getConvexPolygonId());
            Vertex* cur = vert1->next;
            Vertex* mergeEnd = vert1->next->next;
            Vertex* next = vert2->next->next->next;
            do {
                Vertex* vNext = new Vertex(next->id, next->pos);
                cur->next = vNext;
                vNext->prev = cur;
                cur = cur->next;
                next = next->next;
            } while (!next->isSameVert(vert2->next));
            cur->next = mergeEnd;
            mergeEnd->prev = cur;
            delete secondPolygon;
            secondPolygon = nullptr;
            line->drawNode->removeFromParent();
        }
    }
    _isMergeTriangle = true;
}

请添加图片描述

请添加图片描述

至此,把地图划分成了多个凸多边形,以据多边形以及分割边可以进行a*寻路处理

a*寻路

在这里插入图片描述
NavMesh进行a*寻路,有几种判断顶点的方式
1.以多边形的中心为寻路顶点
2.以分割边的两个端点为寻路顶点
3以分割边的中点为寻路顶点
4.以分割边的两个端点以及中点为寻路顶点

这里选择的是以分割边的中点作为寻路顶点

a*的思路

维护两个列表
openList:放的是当前可到达的顶点。最初的起点(起点所在多边形的边的中点)先放入这个表
closeList:放的是顶点数据被锁死不需要再修改的顶点,即已经求出最短路径的顶点
顶点数据包括:
route: 从起点到这个顶点移动的代价,或者说距离。
heuristicCost: 这个顶点到终点的估算代价/距离。这里直接用的是顶点与终点的距离做启发函数。
parentNode: 与route值的计算有关。route表示了从起点到这个顶点的路径消耗,parent指明了路径的方向,
srcPolygonIdx: 这里也记录了来源多边形,以便判断顶点的穿过方向
重复下面的步骤:
1.从openList中选中F值最小的顶点,作为探索顶点
2.把探索顶点换到closeList。因为探索顶点不会有新的更短的到起始位置的路径消耗。
3.以探索顶点寻找新的可到达的顶点,即探索顶点能到达的下一个凸多边形内其他边的中点。
对这些顶点进行判断:
*如果顶点不可通过或者已经在closeList忽略
*如果顶点不在openList中,以探索顶点作为父节点,计算顶点的route,heuristicCost等对应顶点数据,放入openList
*如果顶点已经在openList,顶点有旧的路径消耗(route),用探索顶点作为新的路径来计算route值,与旧的route值比较。如果新的route值小,则把顶点父节点设为当前探索顶点,route值刷新,来源多边形srcPolygonIdx也刷新。(也就是以当前探索顶点作为新路径和顶点记录的旧路径比较看哪个是这个顶点到起点的最短路径消耗。并记录下来)

重复上面三个步骤,直到:
1.到达了终点所在的凸多边形,寻路完成
2.openList已经空了,表示路径不存在,起点无法到达终点

注:如果终点和起点在同一多边形,可直接到达。凸多边形内任意两个可以直线到达

void NavMeshScene::moveToPath(Vec2 vec) {
    int curPolygonIdx = -1;
    int tarPolygonIdx = -1;
    Vec2 srcVec = _player->getPosition();
    for (auto it : _convexPolygons) {
        if (it.second->isInnerVec2(srcVec)) {
            curPolygonIdx = it.first;
        }
        if (it.second->isInnerVec2(vec)) {
            tarPolygonIdx = it.first;
        }
        if (curPolygonIdx >= 0 && tarPolygonIdx >= 0) break;
    }
    if (tarPolygonIdx < 0) {
        CCLOG("=============>>   cannot move !!!!!!! ");
        return;
    }
    _moveLine->clear();
    _openList.clear();
    _closeList.clear();
    if (curPolygonIdx == tarPolygonIdx) { 
        _moveVec.push(vec); 
        _moveLine->drawSegment(srcVec, vec, 0.5, Color4F(1, 1, 1, 0.7));
    }
    else {
        aStarSearch(curPolygonIdx, tarPolygonIdx, vec);
    }
}
void NavMeshScene::aStarSearch(int curPolygonIdx, int tarPolygonIdx, Vec2 tarPos) {
    PolygonClass* curPolygon = _convexPolygons[curPolygonIdx];
    vector<int> searchPointIdx = curPolygon->getSearchPointIdx();
    for (auto idx : searchPointIdx) {
        auto searchPoint = _searchPoints[idx];
        float route = _player->getPosition().getDistance(searchPoint->vecMid);
        float heuristicCost = searchPoint->vecMid.getDistance(tarPos);
        _openList.insert(new AStarNode(idx, route, heuristicCost, curPolygonIdx));
    }
    
    aStarAlgorithm(tarPolygonIdx, tarPos);
}

void NavMeshScene::aStarAlgorithm(int tarPolygonIdx, Vec2 tarPos) {
    if (_openList.empty()) return;
    auto node = _openList.getExploredNode();
    _closeList.insert(node);
    auto searchPoint = _searchPoints[node->searchPointIdx];
    auto toPlygonIdx = searchPoint->getToPolygonIdx(node->srcPolygonIdx);
    if (toPlygonIdx == tarPolygonIdx) {
        _moveVec.push(tarPos);
        if (PathSmoothing) createSmoothPath(node, tarPos);
        else createPath(node, tarPos);
        return;
    }
    auto toPolygon = _convexPolygons[toPlygonIdx];
    vector<int> searchPointIdx = toPolygon->getSearchPointIdx();
    for (auto idx : searchPointIdx) {
        if (_closeList.getNode(idx) == nullptr) {
            float route = node->route + searchPoint->vecMid.getDistance(_searchPoints[idx]->vecMid);
            auto openNode = _openList.getNode(idx);
            if (openNode == nullptr) {
                _openList.insert(new AStarNode(idx, route, _searchPoints[idx]->vecMid.getDistance(tarPos), toPlygonIdx, node, node->passedLineNums + 1));
            }
            else if(route < openNode->route) {
                openNode->route = route;
                openNode->heuristicCost = _searchPoints[idx]->vecMid.getDistance(tarPos);
                openNode->srcPolygonIdx = toPlygonIdx;
                openNode->setParent(node);
                openNode->setPassedLineNums(node->getPassedLineNums() + 1);
            }
        }
    }
    aStarAlgorithm(tarPolygonIdx, tarPos);
}

请添加图片描述

路径顺滑,拐点优化

navmesh如果分割都是完全的三角形的话,路径顺滑有一个Funnel算法。
凸多边形可以沿用这个思路进行路径的拐点优化
大噶思路是,以当前所在位置,以及下一个路径点所在边的两个端点作为当前可到达的端点,三点可形成一个扇形漏斗的视角范围,用此范围和下下一个路径点所在边的两个端点进行对比:
1.新的两个端点都在 漏斗左侧:则此时所能到达的左边端点作为路径拐点加了路径列表,并以此拐点进行新的漏斗视角对比,继续检测下一组端点。。
2.新的两个端点都在 漏斗右侧:与第1条类似,只是以所能到达的右边端点作为路径拐点
3.新的两个端点 都在 漏斗内测:则把新的两个端点更新为最新的所能到达端点,并更新漏洞视角范围,继续检测下一组端点。
4.新的左侧端点在漏斗内,右侧端点在漏斗右侧:则把新的左侧端点更新为当前能到达的左侧端点,继续检测下一组端点。
5.新的左侧端点在漏斗左侧, 右侧端点在漏斗内:与第4条类似,只是把新的右侧端点更新为当前所能到达的右侧端点。
6.新的两个端点形成的漏斗扇形大于当前所能到达的漏斗扇形范围:则不更新能到达的端点位置,直接继续检测下一组端点。
直到终点位置在所能到达的漏斗扇形范围内,则把终点位置直接加入路径列表。因为漏斗范围内的一切位置都可直接到达。

void NavMeshScene::createSmoothPath(AStarNode* node, Vec2 tarPos) {
    vector<pair<Vec2, Vec2>> ret;
    ret.resize(node->getPassedLineNums() + 1);
    int idx = node->getPassedLineNums();
    ret[idx] = make_pair(tarPos, tarPos);
    do {
        idx--;
        auto searchPoint = _searchPoints[node->searchPointIdx];
        auto polygon = _convexPolygons[node->srcPolygonIdx];
        //从出发的多边形角度看,由于多边形是逆时针,先出现的点在出发者的右边,下一个点即为出发者的左边
        if (polygon->isV1BeforeV2(searchPoint->vec1, searchPoint->vec2)) {
            ret[idx] = make_pair(searchPoint->vec2, searchPoint->vec1);
        }
        else {
            ret[idx] = make_pair(searchPoint->vec1, searchPoint->vec2);
        }
        node = node->parentNode;
    } while (node != nullptr);

    Vec2 lastVec = _player->getPosition();

    int canGoLeftIdx = 0;
    int canGoRightIdx = 0;
    int checkLeftIdx = canGoLeftIdx + 1;
    int checkRightIdx = canGoRightIdx + 1;
    stack<Vec2> temp;
    while (checkLeftIdx < ret.size() && checkRightIdx < ret.size()) {
        Vec2 canGoLeftPos = ret[canGoLeftIdx].first;
        Vec2 canGoRightPos = ret[canGoRightIdx].second;
        Vec2 checkLeftPos = ret[checkLeftIdx].first;
        Vec2 checkRightPos = ret[checkRightIdx].second;

        int LLVCross = GeometryMath::getVectorCross(lastVec, canGoLeftPos, checkLeftPos);
        int LRVCross = GeometryMath::getVectorCross(lastVec, canGoLeftPos, checkRightPos);
        int RLVCross = GeometryMath::getVectorCross(lastVec, canGoRightPos, checkLeftPos);
        int RRVCross = GeometryMath::getVectorCross(lastVec, canGoRightPos, checkRightPos);

        if (LRVCross < 0) {
            // 新的两个端点都在 漏斗 leftPos 左侧
            temp.push(canGoLeftPos);
            _moveLine->drawSegment(lastVec, canGoLeftPos, 0.5, Color4F(1, 1, 1, 0.7));
            lastVec = canGoLeftPos;
            canGoLeftIdx = canGoLeftIdx;
            canGoRightIdx = canGoLeftIdx;
            checkLeftIdx = canGoLeftIdx + 1;
            checkRightIdx = canGoRightIdx + 1;
        }
        else if (RLVCross > 0) {
            //新的两个端点都在 漏斗 rightPos 右侧
            temp.push(canGoRightPos);
            _moveLine->drawSegment(lastVec, canGoRightPos, 0.5, Color4F(1, 1, 1, 0.7));
            lastVec = canGoRightPos;
            canGoLeftIdx = canGoRightIdx;
            canGoRightIdx = canGoRightIdx;
            checkLeftIdx = canGoLeftIdx + 1;
            checkRightIdx = canGoRightIdx + 1;
        }
        else if (LLVCross >= 0 && RRVCross <= 0) {
            //新的两个端点 都在 漏斗内测
            canGoLeftIdx = checkLeftIdx;
            canGoRightIdx = checkRightIdx;
            checkLeftIdx = canGoLeftIdx + 1;
            checkRightIdx = canGoRightIdx + 1;
        }
        else if (LLVCross >= 0 && RRVCross > 0) {
            //新的左侧端点 在漏斗内, 右侧端点 在漏斗 rightPos 右侧
            canGoLeftIdx = checkLeftIdx;
            checkLeftIdx = checkLeftIdx + 1;
            checkRightIdx = checkRightIdx + 1;
        }
        else if (LLVCross < 0 && RRVCross <= 0) {
            //新的左侧端点 在漏斗 leftPos 左侧, 右侧端点 在漏斗内
            canGoRightIdx = checkRightIdx;
            checkLeftIdx = checkLeftIdx + 1;
            checkRightIdx = checkRightIdx + 1;
        }
        else if (LLVCross < 0 && RRVCross > 0) {
            //新的左侧端点 在漏斗 leftPos 左侧, 右侧端点 在漏斗 rightPos 右侧
            checkLeftIdx = checkLeftIdx + 1;
            checkRightIdx = checkRightIdx + 1;
        }
    }
    _moveLine->drawSegment(lastVec, tarPos, 0.5, Color4F(1, 1, 1, 0.7));
    while (!temp.empty()) {
        _moveVec.push(temp.top());
        temp.pop();
    }
}

请添加图片描述

一些示例

请添加图片描述
请添加图片描述

一些改进的地方:
1.最终分割的多边形有很多狭长的凸多边形,包括减少合并的次数。主要是在从凹点寻找另一个分割顶点的时候,直接是从相邻顶点的下一个顶点依次判断下去,因此容易出现狭长的凸多边形。
2.a寻路的时候有的时候会看起来像绕远路,主要是a的启发函数用的直接是顶点和终点的距离,如果有孔洞这种可能会找了更远的路。
3.一些数据结构的优化,包括寻找最长分割边,以及下一个探索顶点等。都要一些排序。同时也要顾及到索引

源码

怕上面没解释清楚,直接贴了源码供参考

NavMeshScene.h
#ifndef __NAVMESH_SCENE_H__
#define __NAVMESH_SCENE_H__

#include "cocos2d.h"
#include "Polygon.h"
#include "Line.h"
#include "SearchPoint.h"
#include "AStarQueue.h"
USING_NS_CC;
using namespace std;

class NavMeshScene : public Scene
{
public:
    static Scene* createScene();

    virtual bool init();

    virtual bool onTouchBegan(Touch* touch, Event* unused_event);

    // implement the "static create()" method manually
    CREATE_FUNC(NavMeshScene);

    void update(float dt);

    void linkHole();
    void clipPolygon();
    void mergePolygon();
    void parseNavMeshData();

    pair<Vertex*, Vertex*> getLinkHoleVertex(PolygonClass* polygon, PolygonClass* hole);

    tuple<PolygonClass*, PolygonClass*, Vertex*, Vertex*> getMergeInfo(Line* line);

    bool checkMergeLineAngle(Vertex* linePrevVert1, Vertex* linePrevVert2);

    void moveToPath(Vec2 vec);

    void aStarSearch(int curPolygonIdx, int tarPolygonIdx, Vec2 tarPos);

    void aStarAlgorithm(int tarPolygonIdx, Vec2 tarPos);

    void createPath(AStarNode* aNode, Vec2 tarPos);

    void createSmoothPath(AStarNode* aNode, Vec2 tarPos);
protected:

    EventListenerTouchOneByOne* _touchListener;
    Vec2 _touchBeganPosition;
    DrawNode* _mapDrawNode;
    DrawNode* _player;
    DrawNode* _moveLine;

    int _newPointId;
    int _convexPolygonId;
    vector<PolygonClass* > _holes;
    queue<PolygonClass* > _polygons;
    unordered_map<int, PolygonClass*> _convexPolygons;
    queue<Line* > _linkHoleLines;

    struct cmp {
        bool operator () (Line* l1, Line* l2) const
        {
            return l1->len < l2->len;
        };
    };

    priority_queue<Line*, vector<Line*>, cmp> _clipLines;
    vector<Line*> _closeClipLines;
    vector<SearchPoint*> _searchPoints;
    stack<Vec2> _moveVec;

    AStarQueue _openList;
    AStarQueue _closeList;

    bool _isLinkHole;
    bool _isClipPolygon;
    bool _isMergeTriangle;
    bool _isParseNavMesh;
    
    float _speed = 1000;
};

#endif

NavMeshScene.cpp
#include "NavMeshScene.h"

//vector<Vec2> VecArr = {
//    Vec2(100,100),
//    Vec2(800,100),
//    Vec2(800,600),
//    Vec2(100,600),
//
//    Vec2(400, 300),
//    Vec2(500, 300),
//    Vec2(500, 400),
//    Vec2(400, 400),
//
//    Vec2(250, 200),
//    Vec2(350, 200),
//    Vec2(250, 300),
//
//    Vec2(550, 200),
//    Vec2(650, 200),
//    Vec2(650, 300),
//
//    Vec2(650, 500),
//    Vec2(550, 500),
//    Vec2(650, 400),
//
//    Vec2(250, 400),
//    Vec2(350, 500),
//    Vec2(250, 500),
//
//    Vec2(160, 140),
//    Vec2(740, 140),
//    Vec2(740, 180),
//    Vec2(160, 180),
//
//    Vec2(160, 560),
//    Vec2(160, 520),
//    Vec2(740, 520),
//    Vec2(740, 560),
//};
//int polygonVertexNum = 4;
//vector<int> holeVertexNum = { 4, 3, 3, 3, 3, 4, 4 };
vector<Vec2> VecArr = {
    Vec2(100,100),
    Vec2(130,70),
    Vec2(170,110),
    Vec2(210,100),
    Vec2(240,250),
    Vec2(300,50),
    Vec2(330,170),
    Vec2(400,300),
    Vec2(420,30),
    Vec2(490,50),
    Vec2(600,20),
    Vec2(650,100),
    Vec2(680,120),
    Vec2(720,130),
    Vec2(730,115),
    Vec2(765,170),
    Vec2(800,150),
    Vec2(820,160),
    Vec2(855,200),
    Vec2(900,175),
    Vec2(930,165),
    Vec2(990,230),
    Vec2(1115,320),
    Vec2(1115,620),
    Vec2(100,620),

    Vec2(150, 120),
    Vec2(170, 120),
    Vec2(170, 550),
    Vec2(150, 550),

    Vec2(380, 330),
    Vec2(420, 320),
    Vec2(450, 380),
    Vec2(350, 420),

    Vec2(365, 450),
    Vec2(500, 500),
    Vec2(330, 500),

    Vec2(300, 550),
    Vec2(700, 500),
    Vec2(800, 600),
    Vec2(300, 600),

    Vec2(1000, 350),
    Vec2(1050, 550),
    Vec2(900, 600),
    Vec2(800, 300),

    Vec2(500, 300),
    Vec2(600, 370),
    Vec2(550, 350),

    Vec2(480, 250),
    Vec2(525, 240),
    Vec2(585, 275),
    Vec2(645, 240),
    Vec2(605, 290),
    Vec2(555, 310),

    Vec2(450, 100),
    Vec2(550, 100),
    Vec2(700, 220),
    Vec2(680, 220),

     Vec2(210, 150),
    Vec2(270, 450),
    Vec2(290, 120),
    Vec2(350, 300),
    Vec2(340, 420),
    Vec2(320, 480),
    Vec2(300, 530),
    Vec2(290, 500),
    Vec2(270, 550),
    Vec2(240, 535),
    Vec2(235, 500),
    Vec2(230, 470),
    Vec2(220, 460),
    Vec2(200, 445),
};
int polygonVertexNum = 25;
vector<int> holeVertexNum = {4,4,3,4,4,3,6,4,14};
//vector<Vec2> VecArr = {
//    Vec2(100, 100),
//    Vec2(300, 200),
//    Vec2(500, 50),
//    Vec2(650, 300),
//    Vec2(900, 200),
//    Vec2(1100, 30),
//    Vec2(1230, 400),
//    Vec2(1050, 550),
//    Vec2(850, 450),
//    Vec2(700, 600),
//    Vec2(500, 600),
//    Vec2(420, 270),
//    Vec2(350, 300),
//    Vec2(250, 550),
//    Vec2(200, 400),
//    Vec2(50, 300),
//
//    Vec2(550, 400),
//    Vec2(970, 230),
//    Vec2(800, 450),
//    Vec2(700, 420),
//    Vec2(580, 550),
//
//    Vec2(150, 150),
//    Vec2(250, 220),
//    Vec2(200, 300)
//};
//int polygonVertexNum = 16;
//vector<int> holeVertexNum = {5,3};
int PolygonMaxVertex = 10;

bool PathSmoothing = true;

Scene* NavMeshScene::createScene()
{
    return NavMeshScene::create();
}

static void problemLoading(const char* filename)
{
    printf("Error while loading: %s\n", filename);
    printf("Depending on how you compiled you might have to add 'Resources/' in front of filenames in NavMeshScene.cpp\n");
}


// on "init" you need to initialize your instance
bool NavMeshScene::init()
{
    //
    // 1. super init first
    if (!Scene::init())
    {
        return false;
    }

    auto visibleSize = Director::getInstance()->getVisibleSize();
    Vec2 origin = Director::getInstance()->getVisibleOrigin();

    _newPointId = VecArr.size();
    _convexPolygonId = 1;

    auto layer = LayerColor::create(Color4B(255,255,255,255));
    layer:setContentSize(visibleSize);
    this->addChild(layer);

    _mapDrawNode = DrawNode::create();
    this->addChild(_mapDrawNode);

    PolygonClass *polygon = new PolygonClass();
    for (int i = 0; i < polygonVertexNum; i++) {
        Vec2 v1 = VecArr[i];
        Vec2 v2;
        if (i == polygonVertexNum - 1) {
            v2 = VecArr[0];
        }
        else
        {
            v2 = VecArr[i + 1];
        }
//        _mapDrawNode->drawSegment(v1, v2, 0.5, Color4F(0, 1, 0, 1));
        polygon->insert(i, v1);
    }
//    auto vecData = polygon->getVertVecData();
//    Vec2* ptr = reinterpret_cast<Vec2*>(vecData.data());
//    _mapDrawNode->drawPolygon(ptr, vecData.size(), Color4F(1,1,1,1), 0, Color4F(0,0,0,0));
//
//    vector<Vec2> v1 = {Vec2(100, 100),Vec2(300, 200),Vec2(500, 50)};
//    Vec2* ptr1 = reinterpret_cast<Vec2*>(v1.data());
//    _mapDrawNode->drawPolygon(ptr1, v1.size(), Color4F(0,1,0,1), 0, Color4F(0,0,0,0));
//    vector<Vec2> v2 = {Vec2(500, 50),Vec2(650, 300),Vec2(900, 200),Vec2(1100, 30)};
//    Vec2* ptr2 = reinterpret_cast<Vec2*>(v2.data());
//    _mapDrawNode->drawPolygon(ptr2, v2.size(), Color4F(0,1,0,1), 0, Color4F(0,0,0,0));
//    vector<Vec2> v3 = {Vec2(500, 600),Vec2(420, 270),Vec2(350, 300),Vec2(250, 550)};
//    Vec2* ptr3 = reinterpret_cast<Vec2*>(v3.data());
//    _mapDrawNode->drawPolygon(ptr3, v3.size(), Color4F(0,1,0,1), 0, Color4F(0,0,0,0));
//    vector<Vec2> v4 = {Vec2(1050, 550),Vec2(850, 450),Vec2(700, 600)};
//    Vec2* ptr4 = reinterpret_cast<Vec2*>(v4.data());
//    _mapDrawNode->drawPolygon(ptr4, v4.size(), Color4F(0,1,0,1), 0, Color4F(0,0,0,0));
    
    for (int i = 0; i < polygonVertexNum; i++) {
        Vec2 v1 = VecArr[i];
        Vec2 v2;
        if (i == polygonVertexNum - 1) {
            v2 = VecArr[0];
        }
        else
        {
            v2 = VecArr[i + 1];
        }
        _mapDrawNode->drawSegment(v1, v2, 0.5, Color4F(0, 1, 0, 1));
    }
    
    _polygons.push(polygon);

    int posOffsetIdx = 0;
    for (int num : holeVertexNum) {
        PolygonClass* hole = new PolygonClass();
        for (int i = 0; i < num; i++) {
            Vec2 v1 = VecArr[polygonVertexNum + posOffsetIdx];
            Vec2 v2;
            if (i == num - 1) {
                v2 = VecArr[polygonVertexNum + posOffsetIdx - num + 1];
            }
            else {
                v2 = VecArr[polygonVertexNum + posOffsetIdx + 1];
            }
            _mapDrawNode->drawSegment(v1, v2, 0.5, Color4F(0, 1, 1, 1));
            hole->insert(polygonVertexNum + posOffsetIdx, v1);
            posOffsetIdx = posOffsetIdx + 1;
        }
        _holes.push_back(hole);
//        auto vecData = hole->getVertVecData();
//        Vec2* ptr = reinterpret_cast<Vec2*>(vecData.data());
//        _mapDrawNode->drawPolygon(ptr, vecData.size(), Color4F(0,1,0,1), 0, Color4F(0,0,0,0));
    }

    _moveLine = DrawNode::create();
    this->addChild(_moveLine);
    
//    auto node = DrawNode::create();
//    this->addChild(node);
//    node->drawSegment(Vec2(1200,400), Vec2(1400,400), 0.5, Color4F(0,0,0,1));

    _touchListener = EventListenerTouchOneByOne::create();
    _touchListener->setSwallowTouches(true);
    _touchListener->onTouchBegan = CC_CALLBACK_2(NavMeshScene::onTouchBegan, this);
    this->getEventDispatcher()->addEventListenerWithSceneGraphPriority(_touchListener, layer);

    this->scheduleUpdate();
    return true;
}

bool NavMeshScene::onTouchBegan(Touch* touch, Event* event)
{
    _touchBeganPosition = touch->getLocation();

    CCLOG("==========》 %f, %f", _touchBeganPosition.x, _touchBeganPosition.y);

    if (!_isLinkHole) {
        linkHole();
        return true;
    }
    if (!_isClipPolygon) {
        clipPolygon();
        return true;
    }
    if (!_isMergeTriangle) {
        mergePolygon();
        return true;
    }
    if (!_isParseNavMesh) {
        parseNavMeshData();
        return true;
    }

    moveToPath(_touchBeganPosition);
    return true;
}

void NavMeshScene::update(float dt)
{
    if (!_moveVec.empty()) {
        Vec2 tarPos = _moveVec.top();
        float speed = _speed * dt;
        Vec2 srcPos = _player->getPosition();
        Vec2 velocity = tarPos - srcPos;
        velocity.normalize();
        velocity *= speed;
        if (tarPos.x == srcPos.x) {
            if (velocity.y >= abs(tarPos.y - srcPos.y)) {
                _player->setPosition(tarPos);
                _moveVec.pop();
            }
            else {
                _player->setPosition(srcPos + velocity);
            }
        }
        else {
            if(abs(velocity.x) >= abs(tarPos.x - srcPos.x)){
                _player->setPosition(tarPos);
                _moveVec.pop();
            }
            else {
                _player->setPosition(srcPos + velocity);
            }
        }
   }
}

void NavMeshScene::linkHole() {
    while (!_holes.empty()) {
        PolygonClass* hole = _holes[0];
        _holes.erase(_holes.begin());
        if (_polygons.front()->isInnerHole(hole)) {
            auto verts = getLinkHoleVertex(_polygons.front(), hole);
            if (verts.second == nullptr || verts.first == nullptr) _holes.push_back(hole);
            else {
                auto drawNode = DrawNode::create();
                drawNode->drawSegment(verts.first->pos, verts.second->pos, 1, Color4F(1, 0, 0, 1));
                this->addChild(drawNode);

                Vertex* vPolygon = verts.first;
                Vertex* vertPolygonNext = vPolygon->next;

                //hole的顶点需要顺时针插入polygon
                Vertex* vHole = verts.second;
                do {
                    Vertex* vert = new Vertex(vHole->id, vHole->pos);
                    vPolygon->next = vert;
                    vert->prev = vPolygon;
                    vPolygon = vert;
                    vHole = vHole->prev;
                } while (!vHole->isSameVert(verts.second));

                _newPointId++;
                Vertex* newVHole = new Vertex(_newPointId, verts.second->pos);
                newVHole->prev = vPolygon;
                vPolygon->next = newVHole;

                _newPointId++;
                Vertex* newVPolygon = new Vertex(_newPointId, verts.first->pos);
                newVHole->next = newVPolygon;
                newVPolygon->prev = newVHole;
                newVPolygon->next = vertPolygonNext;
                vertPolygonNext->prev = newVPolygon;

                float len = verts.first->pos.getDistance(verts.second->pos);
                _linkHoleLines.emplace(new Line(verts.first, verts.second, len, drawNode));

                delete hole;
                hole = nullptr;
            }
        }
    }
    _isLinkHole = true;
    return;
}

// 从hole 选择一个顶点pA
// 从polygon选择一个顶点pB,保证与pA连线不与polygon以及hole上的任意非相邻边相交
pair<Vertex*, Vertex*> NavMeshScene::getLinkHoleVertex(PolygonClass* polygon, PolygonClass* hole) {
    Vertex* vertHole = hole->getHead();
    do {
        Vertex* vertPolygon = polygon->getHead();
        do {
            bool isInAngle = polygon->isLineInInnerAngle(vertHole, vertPolygon) && !hole->isLineInInnerAngle(vertPolygon, vertHole);
            if (isInAngle) {
                bool noIntersect = polygon->isVectorNoIntersectWithAdjacentEdge(vertPolygon, vertHole) && hole->isVectorNoIntersectWithAdjacentEdge(vertPolygon, vertHole);
                for (auto hole = _holes.begin(); hole != _holes.end(); hole++) {
                    if (!(*hole)->isVectorNoIntersectWithAdjacentEdge(vertPolygon, vertHole)) {
                        noIntersect = false;
                        break;
                    }
                }
                if (noIntersect) return make_pair(vertPolygon, vertHole);
            }
            vertPolygon = vertPolygon->next;
        } while (!vertPolygon->isSameVert(polygon->getHead()));
        vertHole = vertHole->next;
    } while (!vertHole->isSameVert(hole->getHead()));
    return make_pair(nullptr, nullptr);
}

void NavMeshScene::clipPolygon() {
    while (!_polygons.empty()) {
        PolygonClass* polygon = _polygons.front();
        _polygons.pop();
        auto verts = polygon->findClipVert();
        if (verts.first == nullptr || verts.second == nullptr) {
            //凸边形
            polygon->setConvexPolygonId(_convexPolygonId);
            _convexPolygons.emplace(_convexPolygonId, polygon);
            _convexPolygonId++;

        }
        else {
            auto drawNode = DrawNode::create();
            this->addChild(drawNode);
            drawNode->drawSegment(verts.first->pos, verts.second->pos, 1, Color4F(0, 0, 0, 1));
            float len = verts.first->pos.getDistance(verts.second->pos);
            _clipLines.push(new Line(verts.first, verts.second, len, drawNode));

            auto tarPrev = verts.second->prev;
            auto srcNext = verts.first->next;
            verts.first->next = verts.second;
            verts.second->prev = verts.first;

            polygon->setHead(verts.first);

            PolygonClass* newPolygon = new PolygonClass();
            Vertex* tail = new Vertex(verts.first->id, verts.first->pos);
            tail->next = srcNext;
            srcNext->prev = tail;
            Vertex* head = new Vertex(verts.second->id, verts.second->pos);
            head->prev = tarPrev;
            head->next = tail;
            tarPrev->next = head;
            tail->prev = head;
            newPolygon->setHead(head);
            _polygons.push(polygon);
            _polygons.push(newPolygon);
        }
    }

    _isClipPolygon = true;
     // 在分割完所有凸多边形后,把连接孔洞的线也加入分割线,作为合并多边形的判断
    while (!_linkHoleLines.empty()) {
        _clipLines.push(_linkHoleLines.front());
        _linkHoleLines.pop();
    }
}

//判断分割边分割的两个凸多边形合并仍是凸多边形, 即分割边两个共享点在合并的凸多边形里是 凸点
bool NavMeshScene::checkMergeLineAngle(Vertex* linePrevVert1, Vertex* linePrevVert2) {
    //求如果合并的新凸多边形, 公共边对应的两个角的顶点
    Vec2 v1Prev = linePrevVert1->pos;
    Vec2 v1 = linePrevVert1->next->pos;
    Vec2 v1Next = linePrevVert2->next->next->next->pos;
    Vec2 v2Prev = linePrevVert2->pos;
    Vec2 v2 = linePrevVert2->next->pos;
    Vec2 v2Next = linePrevVert1->next->next->next->pos;
    if (GeometryMath::isInRight(v1Prev, v1, v1Next)) return false;
    if (GeometryMath::isInRight(v2Prev, v2, v2Next)) return false;
    return true;
}

//获取分割边分割的凸多边形
tuple<PolygonClass*, PolygonClass*, Vertex*, Vertex*> NavMeshScene::getMergeInfo(Line* line) {
    PolygonClass* firstPolygon = nullptr;
    PolygonClass* secondPolygon = nullptr;
    Vertex* vert1 = nullptr;
    Vertex* vert2 = nullptr;
    for (auto it : _convexPolygons) {
        Vertex* linePrev = it.second->getLinePrevVert(line);
        if (linePrev != nullptr) {
            if (firstPolygon == nullptr) {
                firstPolygon = it.second;
                vert1 = linePrev;
            }
            else {
                secondPolygon = it.second;
                vert2 = linePrev;
                break;
            }
        }
    }
    //能合并最大多边形判断
    if (firstPolygon->getSize() + secondPolygon->getSize() - 2 > PolygonMaxVertex) return make_tuple(nullptr, nullptr, nullptr, nullptr);
    if (!checkMergeLineAngle(vert1, vert2)) return make_tuple(nullptr, nullptr, nullptr, nullptr);
    return make_tuple(firstPolygon, secondPolygon, vert1, vert2);
}

void NavMeshScene::mergePolygon() {
    while (!_clipLines.empty()) {
        // 获取最长的分割边
        auto line = _clipLines.top();
        _clipLines.pop();

        PolygonClass* firstPolygon;
        PolygonClass* secondPolygon;
        Vertex* vert1;
        Vertex* vert2;
        tie(firstPolygon, secondPolygon, vert1, vert2) = getMergeInfo(line);
        if (firstPolygon == nullptr) {
            _closeClipLines.push_back(line);
        }
        else {
            _convexPolygons.erase(secondPolygon->getConvexPolygonId());
            Vertex* cur = vert1->next;
            Vertex* mergeEnd = vert1->next->next;
            Vertex* next = vert2->next->next->next;
            do {
                Vertex* vNext = new Vertex(next->id, next->pos);
                cur->next = vNext;
                vNext->prev = cur;
                cur = cur->next;
                next = next->next;
            } while (!next->isSameVert(vert2->next));
            cur->next = mergeEnd;
            mergeEnd->prev = cur;
            delete secondPolygon;
            secondPolygon = nullptr;
            line->drawNode->removeFromParent();
        }
    }
    _isMergeTriangle = true;
}

void NavMeshScene::parseNavMeshData() {
    _mapDrawNode->clear();
    float i = 1;
    float nums = _convexPolygons.size() + i;
    for (auto it : _convexPolygons) {
        auto vecData = it.second->getVertVecData();
//        int t = RandomHelper::random_real<float>(200, 1200);;
        float r = RandomHelper::random_real<float>(0, 1);
        float g = RandomHelper::random_real<float>(0, 1);
        float b = RandomHelper::random_real<float>(0, 1);
        Vec2* ptr = reinterpret_cast<Vec2*>(vecData.data());
        _mapDrawNode->drawPolygon(ptr, vecData.size(), Color4F(r,g,b,1), 0, Color4F(0,0,0,0));
        i++;

        /*auto num = Label::create();
        this->addChild(num);
        num->setString(to_string(it.second->getConvexPolygonId()));
        num->setPosition(it.second->getInnerVec2());*/
    }
    for (int i = 0; i < _closeClipLines.size(); i++) {
        auto line = _closeClipLines[i];
        line->drawNode->removeFromParent();
        PolygonClass* firstPolygon = nullptr;
        PolygonClass* secondPolygon = nullptr;
        for (auto it : _convexPolygons) {
            if (it.second->hasLine(line)) {
                if (firstPolygon == nullptr) firstPolygon = it.second;
                else if (secondPolygon == nullptr) {
                    secondPolygon = it.second;
                    break;
                }
            }
        }
        firstPolygon->insertSearchPointIdx(i);
        secondPolygon->insertSearchPointIdx(i);
        _searchPoints.push_back(new SearchPoint(line->vert1->pos, line->vert2->pos, firstPolygon->getConvexPolygonId(), secondPolygon->getConvexPolygonId()));
    }

    _player = DrawNode::create();
    this->addChild(_player);
    _player->drawDot(Vec2(0, 0), 10, Color4F(1, 1, 1, 1));
    auto polygon = (*_convexPolygons.begin()).second;
    _player->setPosition(polygon->getInnerVec2());

    _isParseNavMesh = true;
}

void NavMeshScene::moveToPath(Vec2 vec) {
    int curPolygonIdx = -1;
    int tarPolygonIdx = -1;
    Vec2 srcVec = _player->getPosition();
    for (auto it : _convexPolygons) {
        if (it.second->isInnerVec2(srcVec)) {
            curPolygonIdx = it.first;
        }
        if (it.second->isInnerVec2(vec)) {
            tarPolygonIdx = it.first;
        }
        if (curPolygonIdx >= 0 && tarPolygonIdx >= 0) break;
    }
    if (tarPolygonIdx < 0) {
        CCLOG("=============>>   cannot move !!!!!!! ");
        return;
    }
    _moveLine->clear();
    _openList.clear();
    _closeList.clear();
    if (curPolygonIdx == tarPolygonIdx) { 
        _moveVec.push(vec); 
        _moveLine->drawSegment(srcVec, vec, 1, Color4F(1, 1, 1, 0.7));
    }
    else {
        aStarSearch(curPolygonIdx, tarPolygonIdx, vec);
    }
}

void NavMeshScene::aStarSearch(int curPolygonIdx, int tarPolygonIdx, Vec2 tarPos) {
    PolygonClass* curPolygon = _convexPolygons[curPolygonIdx];
    vector<int> searchPointIdx = curPolygon->getSearchPointIdx();
    for (auto idx : searchPointIdx) {
        auto searchPoint = _searchPoints[idx];
        float route = _player->getPosition().getDistance(searchPoint->vecMid);
        float heuristicCost = searchPoint->vecMid.getDistance(tarPos);
        _openList.insert(new AStarNode(idx, route, heuristicCost, curPolygonIdx));
    }
    
    aStarAlgorithm(tarPolygonIdx, tarPos);
}

void NavMeshScene::aStarAlgorithm(int tarPolygonIdx, Vec2 tarPos) {
    if (_openList.empty()) return;
    auto node = _openList.getExploredNode();
    _closeList.insert(node);
    auto searchPoint = _searchPoints[node->searchPointIdx];
    auto toPlygonIdx = searchPoint->getToPolygonIdx(node->srcPolygonIdx);
    if (toPlygonIdx == tarPolygonIdx) {
        _moveVec.push(tarPos);
        if (PathSmoothing) createSmoothPath(node, tarPos);
        else createPath(node, tarPos);
        return;
    }
    auto toPolygon = _convexPolygons[toPlygonIdx];
    vector<int> searchPointIdx = toPolygon->getSearchPointIdx();
    for (auto idx : searchPointIdx) {
        if (_closeList.getNode(idx) == nullptr) {
            float route = node->route + searchPoint->vecMid.getDistance(_searchPoints[idx]->vecMid);
            auto openNode = _openList.getNode(idx);
            if (openNode == nullptr) {
                _openList.insert(new AStarNode(idx, route, _searchPoints[idx]->vecMid.getDistance(tarPos), toPlygonIdx, node, node->passedLineNums + 1));
            }
            else if(route < openNode->route) {
                openNode->route = route;
                openNode->heuristicCost = _searchPoints[idx]->vecMid.getDistance(tarPos);
                openNode->srcPolygonIdx = toPlygonIdx;
                openNode->setParent(node);
                openNode->setPassedLineNums(node->getPassedLineNums() + 1);
            }
        }
    }
    aStarAlgorithm(tarPolygonIdx, tarPos);
}

void NavMeshScene::createPath(AStarNode* node, Vec2 tarPos) {
    Vec2 last, next;
    last = tarPos;
    do {
        next = _searchPoints[node->searchPointIdx]->vecMid;
        _moveVec.push(next);
        _moveLine->drawSegment(last, next, 1, Color4F(1, 1, 1, 0.7));
        last = next;
        node = node->parentNode;
    } while (node != nullptr);
    _moveLine->drawSegment(last, _player->getPosition(), 1, Color4F(1, 1, 1, 0.7));
}

void NavMeshScene::createSmoothPath(AStarNode* node, Vec2 tarPos) {
    vector<pair<Vec2, Vec2>> ret;
    ret.resize(node->getPassedLineNums() + 1);
    int idx = node->getPassedLineNums();
    ret[idx] = make_pair(tarPos, tarPos);
    do {
        idx--;
        auto searchPoint = _searchPoints[node->searchPointIdx];
        auto polygon = _convexPolygons[node->srcPolygonIdx];
        //从出发的多边形角度看,由于多边形是逆时针,先出现的点在出发者的右边,下一个点即为出发者的左边
        if (polygon->isV1BeforeV2(searchPoint->vec1, searchPoint->vec2)) {
            ret[idx] = make_pair(searchPoint->vec2, searchPoint->vec1);
        }
        else {
            ret[idx] = make_pair(searchPoint->vec1, searchPoint->vec2);
        }
        node = node->parentNode;
    } while (node != nullptr);

    Vec2 lastVec = _player->getPosition();

    int canGoLeftIdx = 0;
    int canGoRightIdx = 0;
    int checkLeftIdx = canGoLeftIdx + 1;
    int checkRightIdx = canGoRightIdx + 1;
    stack<Vec2> temp;
    while (checkLeftIdx < ret.size() && checkRightIdx < ret.size()) {
        Vec2 canGoLeftPos = ret[canGoLeftIdx].first;
        Vec2 canGoRightPos = ret[canGoRightIdx].second;
        Vec2 checkLeftPos = ret[checkLeftIdx].first;
        Vec2 checkRightPos = ret[checkRightIdx].second;

        int LLVCross = GeometryMath::getVectorCross(lastVec, canGoLeftPos, checkLeftPos);
        int LRVCross = GeometryMath::getVectorCross(lastVec, canGoLeftPos, checkRightPos);
        int RLVCross = GeometryMath::getVectorCross(lastVec, canGoRightPos, checkLeftPos);
        int RRVCross = GeometryMath::getVectorCross(lastVec, canGoRightPos, checkRightPos);

        if (LRVCross < 0) {
            // 新的两个端点都在 漏斗 leftPos 左侧
            temp.push(canGoLeftPos);
            _moveLine->drawSegment(lastVec, canGoLeftPos, 1, Color4F(1, 1, 1, 0.7));
            lastVec = canGoLeftPos;
            canGoLeftIdx = canGoLeftIdx;
            canGoRightIdx = canGoLeftIdx;
            checkLeftIdx = canGoLeftIdx + 1;
            checkRightIdx = canGoRightIdx + 1;
        }
        else if (RLVCross > 0) {
            //新的两个端点都在 漏斗 rightPos 右侧
            temp.push(canGoRightPos);
            _moveLine->drawSegment(lastVec, canGoRightPos, 1, Color4F(1, 1, 1, 0.7));
            lastVec = canGoRightPos;
            canGoLeftIdx = canGoRightIdx;
            canGoRightIdx = canGoRightIdx;
            checkLeftIdx = canGoLeftIdx + 1;
            checkRightIdx = canGoRightIdx + 1;
        }
        else if (LLVCross >= 0 && RRVCross <= 0) {
            //新的两个端点 都在 漏斗内测
            canGoLeftIdx = checkLeftIdx;
            canGoRightIdx = checkRightIdx;
            checkLeftIdx = canGoLeftIdx + 1;
            checkRightIdx = canGoRightIdx + 1;
        }
        else if (LLVCross >= 0 && RRVCross > 0) {
            //新的左侧端点 在漏斗内, 右侧端点 在漏斗 rightPos 右侧
            canGoLeftIdx = checkLeftIdx;
            checkLeftIdx = checkLeftIdx + 1;
            checkRightIdx = checkRightIdx + 1;
        }
        else if (LLVCross < 0 && RRVCross <= 0) {
            //新的左侧端点 在漏斗 leftPos 左侧, 右侧端点 在漏斗内
            canGoRightIdx = checkRightIdx;
            checkLeftIdx = checkLeftIdx + 1;
            checkRightIdx = checkRightIdx + 1;
        }
        else if (LLVCross < 0 && RRVCross > 0) {
            //新的左侧端点 在漏斗 leftPos 左侧, 右侧端点 在漏斗 rightPos 右侧
            checkLeftIdx = checkLeftIdx + 1;
            checkRightIdx = checkRightIdx + 1;
        }
    }
    _moveLine->drawSegment(lastVec, tarPos, 1, Color4F(1, 1, 1, 0.7));
    while (!temp.empty()) {
        _moveVec.push(temp.top());
        temp.pop();
    }
}

Polygon.h
#ifndef __POLYGON_H__
#define __POLYGON_H__

#include "Vertex.h"
#include "GeometryMath.h"
#include "cocos2d.h"
#include "Line.h"
USING_NS_CC;
using namespace std;

class PolygonClass
{
	public:
		PolygonClass(): _head(nullptr) {};
		void insert(int id, Vec2 pos);
		bool isInnerHole(PolygonClass* hole);
		bool isInnerVert(Vertex* vert);
		bool isInnerVec2(Vec2 vec);
		Vertex* getHead() { return _head; };
		bool isLineInInnerAngle(Vertex* vertL1, Vertex* vertL2);
		bool isVectorNoIntersectWithAdjacentEdge(Vertex* vertL1, Vertex* vertL2);
		pair<Vertex*, Vertex*> findClipVert();
		Vertex* findConcaveVertex();
		void setHead(Vertex* head);
		Vertex* getLinePrevVert(Line* line);
		bool hasLine(Line* line);
		int getSize();
		void setConvexPolygonId(int id) { _convexPolygonId = id; };
		int getConvexPolygonId() { return _convexPolygonId; };
		vector<Vec2> getVertVecData();
		Vec2 getInnerVec2();
		void display();
		void insertSearchPointIdx(int searchPointIdx) { _searchPointIdx.push_back(searchPointIdx); };
		vector<int> getSearchPointIdx() { return _searchPointIdx; };
		bool isV1BeforeV2(Vec2 v1, Vec2 v2);
		~PolygonClass();
	private:
		Vertex* _head;
		int _convexPolygonId;
		vector<int> _searchPointIdx;
};

#endif
Polygon.cpp
#include "Polygon.h"

void PolygonClass::insert(int id, Vec2 pos) {
	Vertex* vert = new Vertex(id, pos);
	if (_head == nullptr) {
		_head = vert;
		_head->next = vert;
		_head->prev = vert;
	}else {
		Vertex* tail = _head->prev;
		tail->next = vert;
		vert->next = _head;
		vert->prev = tail;
		_head->prev = vert;
	}
}

PolygonClass::~PolygonClass() {
	if (_head == nullptr) return;
	auto cur = _head->next;
	while (!cur->isSameVert(_head)) {
		auto temp = cur;
		cur = cur->next;
		delete temp;
	}
	delete _head;
}

// 判断多边形hole是否被polygon完全包含
bool PolygonClass::isInnerHole(PolygonClass* hole) {
	// hole的所有点都被polygon包含
	Vertex* cur = hole->getHead();
	if (_head == nullptr)  return false;
	do {
		if (!this->isInnerVert(cur)) return false;
		cur = cur->next;
	} while (!cur->isSameVert(hole->getHead()));
	return true;
}

// 判断点是否在多边形中
bool PolygonClass::isInnerVert(Vertex* vert) {
	return isInnerVec2(vert->pos);
}

bool PolygonClass::isInnerVec2(Vec2 vec) {
	//从 顶点 向右 发射一条向右水平射线, 水平射线与所有边的交点数目,为奇数,则在内部, 为偶数,则在外部
	Vertex* cur = _head;
	int intersectLines = 0;
	do {
		if (GeometryMath::isRayIntersectLine(vec, cur->pos, cur->next->pos)) intersectLines++;
		cur = cur->next;
	} while (!cur->isSameVert(_head));
	return intersectLines % 2 == 1;
}

bool PolygonClass::isLineInInnerAngle(Vertex* vertL1, Vertex* vertL2) {
	if (vertL1->isSamePos(vertL2)) return false;
	if (vertL1->isSamePos(vertL2->prev)) return false;
	if (vertL1->isSamePos(vertL2->next)) return false;
	Vec2 vecL2Prev = vertL2->prev->pos;
	Vec2 vecL2= vertL2->pos;
	Vec2 vecL2Next = vertL2->next->pos;
	Vec2 vecL1 = vertL1->pos;
	if (GeometryMath::isInLeft(vecL2Prev, vecL2, vecL2Next)) return GeometryMath::checkVectorInConvexAngle(vecL1, vecL2Prev, vecL2, vecL2Next);
	/*else if (GeometryMath::isInRight(vecL2Prev, vecL2, vecL2Next)) return GeometryMath::checkVectorInConcaveAngle(vecL1, vecL2Prev, vecL2, vecL2Next);
	else return GeometryMath::isInRight(vecL2Prev, vecL2, vecL1);*/
	return GeometryMath::checkVectorInConcaveAngle(vecL1, vecL2Prev, vecL2, vecL2Next);
}

bool PolygonClass::isVectorNoIntersectWithAdjacentEdge(Vertex* vertL1, Vertex* vertL2) {
	Vertex* cur = _head;
	do {
		// 防止 连接点的相邻边 与 新增线 重合
		/*if (GeometryMath::isVertexInLine(v1, v2, curPos) && GeometryMath::isVertexInLine(v1, v2, nextPos)) return false;*/
		if (!vertL1->isSamePos(cur) && !vertL1->isSamePos(cur->next) && !vertL2->isSamePos(cur) && !vertL2->isSamePos(cur->next)) {
			if (GeometryMath::checkTwoVectorIntersect(vertL1->pos, vertL2->pos, cur->pos, cur->next->pos)) return false;
		}
		cur = cur->next;
	} while (!cur->isSameVert(_head));
	return true;
}

Vertex* PolygonClass::findConcaveVertex() {
	Vertex* cur = _head;
	do {
		if(!GeometryMath::isInLeft(cur->prev->pos,cur->pos,cur->next->pos)) return cur;
		cur = cur->next;
	}while (!cur->isSameVert(_head));
	return nullptr;
}

//找到一个凹角顶点a
//从点a 找到一个 不相邻点b 且能连接成完全在内部的对角线,
//因为a为凹角,所以从a出发一定在∠a内, 则需要对角线在目标点的∠B内,且不与任何非相邻边相交
pair<Vertex*, Vertex*> PolygonClass::findClipVert() {
	auto srcVert = findConcaveVertex();
	if (srcVert == nullptr) {
		return make_pair(nullptr, nullptr);
	}
	auto tarVert = srcVert->next->next;
	do {
		if (!tarVert->isSamePos(srcVert) && !tarVert->isSamePos(srcVert->next) && !tarVert->isSamePos(srcVert->prev)) {
			bool isInAngle = isLineInInnerAngle(srcVert, tarVert) && isLineInInnerAngle(tarVert, srcVert);
			bool isNoIntersect = isVectorNoIntersectWithAdjacentEdge(srcVert, tarVert);
			if (isInAngle && isNoIntersect) return make_pair(srcVert, tarVert);
		}
		tarVert = tarVert->next;
	} while (!tarVert->isSameVert(srcVert->prev));
	return make_pair(nullptr, nullptr);
}

void PolygonClass::setHead(Vertex* head) {
	_head = head;
}

Vertex* PolygonClass::getLinePrevVert(Line* line) {
	auto vert1 = line->vert1;
	auto vert2 = line->vert2;
	auto cur = _head;
	do {
		if (cur->isSamePos(vert1) && cur->next->isSamePos(vert2)) return cur->prev;
		if (cur->isSamePos(vert2) && cur->next->isSamePos(vert1)) return cur->prev;
		cur = cur->next;
	} while (!cur->isSameVert(_head));
	return nullptr;
}

bool PolygonClass::hasLine(Line* line) {
	return getLinePrevVert(line) != nullptr;
}

int PolygonClass::getSize() {
	int num = 0;
	auto cur = _head;
	do {
		num++;
		cur = cur->next;
	}while(!cur->isSameVert(_head));
	return num;
}

vector<Vec2> PolygonClass::getVertVecData() {
	vector<Vec2> data;
	auto cur = _head;
	do {
		data.push_back(cur->pos);
		cur = cur->next;
	} while (!cur->isSameVert(_head));
	return data;
}

Vec2 PolygonClass::getInnerVec2() {
	Vec2 v1 = _head->pos.getMidpoint(_head->next->pos);
	Vec2 v2 = _head->pos.getMidpoint(_head->prev->pos);
	return v1.getMidpoint(v2);
}

bool PolygonClass::isV1BeforeV2(Vec2 v1, Vec2 v2) {
	auto cur = _head;
	do {
		if (cur->pos == v1 && cur->next->pos == v2) return true;
		else if (cur->pos == v2 && cur->next->pos == v1) return false;
		cur = cur->next;
	} while (!cur->isSameVert(_head));
	return false;
}

void PolygonClass::display()
{
	Vertex* cur = _head;
	do {
		CCLOG("==========》 %d, %f, %f", cur->id, cur->pos.x, cur->pos.y);
		cur = cur->next;
	} while (!cur->isSameVert(_head));
}

Vertex.h
#pragma once
#ifndef __VERTEX_H__
#define __VERTEX_H__
#include "cocos2d.h"
USING_NS_CC;
using namespace std;
class Vertex
{
public:
	Vertex(int id, Vec2 pos) {
		this->id = id;
		this->pos = pos;
	}
	bool isSameVert(Vertex* vert) {
		return vert->id == id && vert->pos == pos;
	}
	bool isSamePos(Vertex* vert) {
		return vert->pos == pos;
	}
	int id;
	Vec2 pos;
	Vertex* next;
	Vertex* prev;
};

#endif

SearchPoint.h
#pragma once
#ifndef __SEARCH_POINT_H__
#define __SEARCH_POINT_H__
#include "cocos2d.h"
USING_NS_CC;
using namespace std;
class SearchPoint
{
public:
	SearchPoint(Vec2 vec1, Vec2 vec2, int polygonId1, int polygonId2)
		: 
		vec1(vec1), 
		vec2(vec2), 
		polygonId1(polygonId1), 
		polygonId2(polygonId2)
	{
		vecMid = vec1.getMidpoint(vec2);
	}
	int getToPolygonIdx(int fromPolygonIdx) { return fromPolygonIdx == polygonId1 ? polygonId2 : polygonId1; }
	Vec2 vecMid;
	Vec2 vec1;
	Vec2 vec2;
	int polygonId1;
	int polygonId2;
};

#endif

Line.h
#pragma once
#ifndef __LINE_H__
#define __LINE_H__
#include "cocos2d.h"
#include "Vertex.h"
USING_NS_CC;
using namespace std;
class Line
{
public:
	Line(Vertex* vert1, Vertex* vert2, float len, DrawNode* drawNode) {
		this->vert1 = new Vertex(vert1->id, vert1->pos);
		this->vert2 = new Vertex(vert2->id, vert2->pos);
		this->len = len;
		this->drawNode = drawNode;
	}

	Vertex* vert1;
	Vertex* vert2;
	float len;
	DrawNode* drawNode;
};

#endif

AStarQueue.h
#pragma once
#ifndef __ASTAR_QUEUE_H__
#define __ASTAR_QUEUE_H__
#include "cocos2d.h"
#include "AStarNode.h"
USING_NS_CC;
using namespace std;

class AStarQueue
{
public:
	AStarQueue(){
		
	}
	void insert(AStarNode* aNode) { _map.emplace(aNode->searchPointIdx, aNode); };
	AStarNode* getExploredNode() {
		if (_map.empty()) return nullptr;
		AStarNode* aNode = (*_map.begin()).second;
		for (auto it : _map) {
			if (it.second->getAStarCost() < aNode->getAStarCost()){
				aNode = it.second;
			}
		}
		_map.erase(aNode->searchPointIdx);
		return aNode;
	};
	bool empty() { return _map.empty(); };
	AStarNode* getNode(int searchPointIdx) {
		auto it = _map.find(searchPointIdx);
		if (it == _map.end()) {
			return nullptr;
		}
		else {
			return (*it).second;
		}
	};
	void clear() {
		for (auto it : _map) {
			delete it.second;
			it.second = nullptr;
		}
		_map.clear();
	}

	map<int, AStarNode*> _map;
};

#endif

AStarNode.h
#pragma once
#ifndef __ASTAR_NODE_H__
#define __ASTAR_NODE_H__
#include "cocos2d.h"
USING_NS_CC;
using namespace std;
class AStarNode
{
public:
	AStarNode(int searchPointIdx, float route, float heuristicCost, int srcPolygonIdx)
		: 
		searchPointIdx(searchPointIdx),
		route(route),
		heuristicCost(heuristicCost),
		srcPolygonIdx(srcPolygonIdx),
		parentNode(nullptr),
		passedLineNums(1)
	{}
	AStarNode(int searchPointIdx, float route, float heuristicCost, int srcPolygonIdx, AStarNode* aNode, int passedLineNums)
		:
		searchPointIdx(searchPointIdx),
		route(route),
		heuristicCost(heuristicCost),
		srcPolygonIdx(srcPolygonIdx),
		parentNode(aNode),
		passedLineNums(passedLineNums)
	{}
	void setParent(AStarNode* aNode) {
		parentNode = aNode;
	}
	void setPassedLineNums(int num) {
		passedLineNums = num;
	}
	int getPassedLineNums(){
		return passedLineNums;
	}
	float getAStarCost() { return route + heuristicCost; };
	int searchPointIdx;
	AStarNode* parentNode;
	float route;
	float heuristicCost;
	int srcPolygonIdx;
	int passedLineNums;
};

#endif

GeometryMath.h
#pragma once
#ifndef __GEOMETRY_MATH_H__
#define __GEOMETRY_MATH_H__

#include "cocos2d.h"
USING_NS_CC;
using namespace std;

class GeometryMath
{
public:
	static bool isRayIntersectLine(Vec2 vR, Vec2 vL1, Vec2 vL2);
	static bool isInLeft(Vec2 a, Vec2 b, Vec2 c);
	static bool isInRight(Vec2 a, Vec2 b, Vec2 c);
	static bool isCollineation(Vec2 a, Vec2 b, Vec2 c);
	static int getVectorCross(Vec2 a, Vec2 b, Vec2 c);
	static bool checkVectorInConvexAngle(Vec2 v, Vec2 a, Vec2 b, Vec2 c);
	static bool checkVectorInConcaveAngle(Vec2 v, Vec2 a, Vec2 b, Vec2 c);
	static bool checkTwoVectorIntersect(Vec2 va, Vec2 vb, Vec2 vc, Vec2 vd);
	static bool isVertexInLine(Vec2 vL1, Vec2 vL2, Vec2 vert);
	static bool isStrictlyIntersect(Vec2 va, Vec2 vb, Vec2 vc, Vec2 vd);
private:

};


#endif

GeometryMath.cpp
#include "GeometryMath.h"

bool GeometryMath::isRayIntersectLine(Vec2 vR, Vec2 vL1, Vec2 vL2) {
	//线段水平,与射线重合或平行
	if (vL1.y == vL2.y) return false;
	//线段在 水平射线上方
	if (vL1.y > vR.y && vL2.y > vR.y) return false;
	//线段在 水平射线下方
	if (vL1.y < vR.y && vL2.y < vR.y) return false;
	float minY = min(vL1.y, vL2.y);
	float maxY = min(vL1.y, vL2.y);

	/*线段下方端点在线上, 与 线段上方端点的在线上的情况,只能选择一种作为相交,另一种作为不相交
	否则射线穿过的顶点的相交判断会有问题
	即maxY 或者 minY 只选择一种做判断*/
	if (maxY == vR.y) return false;

	//线段两个端点分别在 射线的上下, 求线段在 射线的水平线上的x点,判断x点与 射线起点x轴坐标(射线方向向右)
	float offsetY = vL2.y - vR.y;
	float offsetX = offsetY / (vL2.y - vL1.y) * (vL2.x - vL1.x);
	float x = vL2.x - offsetX;
	return x >= vR.x;
}

// 点 c 在a到b向量的左边, 即∠abc 小于180°
bool GeometryMath::isInLeft(Vec2 a, Vec2 b, Vec2 c) {
	float e = getVectorCross(a, b, c);
	return getVectorCross(a, b, c) < 0;
}

// 点 c 在a到b向量的右边, 即∠abc 大于180°
bool GeometryMath::isInRight(Vec2 a, Vec2 b, Vec2 c) {
	return getVectorCross(a, b, c) > 0;
}

// 点 c 与a到b向量共线, 即∠abc 等于180°
bool GeometryMath::isCollineation(Vec2 a, Vec2 b, Vec2 c) {
	return getVectorCross(a, b, c) == 0;
}

int GeometryMath::getVectorCross(Vec2 a, Vec2 b, Vec2 c) {
	Vec2 vectorBA = a - b;
	Vec2 vectorBC = c - b;
	return vectorBA.cross(vectorBC);
}

// 小于180°的角 ∠abc,点a,点b,点c是逆时针,判定线vb在角内, 线需在ab的左边,并且在bc的左边
bool GeometryMath::checkVectorInConvexAngle(Vec2 v, Vec2 a, Vec2 b, Vec2 c) {
	return isInLeft(a, b, v) && isInLeft(b, c, v);
}

//大于180°的角 ∠abc,点a,点b,点c是逆时针,判定线vb在角内,即线不在∠abc的外侧,即线不在∠cba里
bool GeometryMath::checkVectorInConcaveAngle(Vec2 v, Vec2 a, Vec2 b, Vec2 c) {
	return !checkVectorInConvexAngle(v, c, b, a);
}

// vert 在 vectorL12 线段之间
bool GeometryMath::isVertexInLine(Vec2 vL1, Vec2 vL2, Vec2 vert) {
	if (!isCollineation(vL1, vL2, vert)) return false;
	if (vL1.x == vL2.x) return (vert.y >= min(vL1.y, vL2.y)) && (vert.y <= max(vL1.y, vL2.y));
	else return (vert.x >= min(vL1.x, vL2.x)) && (vert.x <= max(vL1.x, vL2.x));
}

// 检查 vectorAB 与 vectorCD 相交
bool GeometryMath::checkTwoVectorIntersect(Vec2 va, Vec2 vb, Vec2 vc, Vec2 vd) {
	if (isStrictlyIntersect(va, vb, vc, vd)) {
		return true;
	}
	if (isVertexInLine(va, vb, vc)) { 
		return true; }
	if (isVertexInLine(va, vb, vd)) {
		return true;
	}
	if (isVertexInLine(vc, vd, va)) {
		return true;
	}
	if (isVertexInLine(vc, vd, vb)) {
		return true;
	}
	return false;
}

// 检查 vectorAB 与 vectorCD 严格相交, 点va,点vb在 线vectorCD 两侧, 且点vc,点vd 在线vectorAB 两侧
bool GeometryMath::isStrictlyIntersect(Vec2 va, Vec2 vb, Vec2 vc, Vec2 vd) {
	if (isCollineation(va, vb, vc)) return false;
	if (isCollineation(va, vb, vd)) return false;
	if (isCollineation(vc, vd, va)) return false;
	if (isCollineation(vc, vd, vb)) return false;
	if (isInLeft(va, vb, vc) == isInLeft(va, vb, vd)) return false;
	if (isInLeft(vc, vd, va) == isInLeft(vc, vd, vb)) return false;
	return true;
}

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:/a/178595.html

如若内容造成侵权/违法违规/事实不符,请联系我们进行投诉反馈qq邮箱809451989@qq.com,一经查实,立即删除!

相关文章

P4 C++ 条件与分支(if)

前言 今天我们来看看条件语句&#xff0c;换句话说&#xff0c;也就是 if 语句、if else 和 else if 等等这写语句。 我知道大家基本上已经非常了解 if 语句和所有 C 中的分支语句&#xff0c;但我还是鼓励你们继续看完这一讲&#xff0c;这里可能包含一些新东西。我们还会深入…

【精选】Ajax技术知识点合集

Ajax技术详解 Ajax简介 Ajax 即“Asynchronous Javascript And XML”&#xff08;异步 JavaScript 和 XML&#xff09;&#xff0c;是指一种创建 交互式、快速动态应用的网页开发技术&#xff0c;无需重新加载整个网页的情况下&#xff0c;能够更新页面局 部数据的技术。通过在…

[ 云计算 | AWS 实践 ] 基于 Amazon S3 协议搭建个人云存储服务

本文收录于【#云计算入门与实践 - AWS】专栏中&#xff0c;收录 AWS 入门与实践相关博文。 本文同步于个人公众号&#xff1a;【云计算洞察】 更多关于云计算技术内容敬请关注&#xff1a;CSDN【#云计算入门与实践 - AWS】专栏。 本系列已更新博文&#xff1a; [ 云计算 | …

世微 dc-dc降压恒流 LED汽车大灯 单灯 14V5A 68W车灯驱动方案 AP5191

产品描述 AP5191是一款PWM工作模式,高效率、外围简单、外置功率MOS管&#xff0c;适用于4.5-150V输入的高精度降压LED恒流驱动芯片。输出最大功率150W&#xff0c;最大电流6A。AP5191可实现线性调光和PWM调光&#xff0c;线性调光脚有效电压范围0.55-2.6V.AP5191 工作频率可以…

Oracle与Redis Enterprise协同,作为企业缓存解决方案

来源&#xff1a;虹科云科技 虹科干货丨Oracle与Redis Enterprise协同&#xff0c;作为企业缓存解决方案 欢迎关注虹科&#xff0c;为您提供最新资讯&#xff01; 单独使用Oracle作为企业缓存数据库时&#xff0c;会出现哪些问题呢&#xff1f;使用Redis Enterprise与Oracle共…

基于STM32的手势识别算法研究与应用

基于STM32的手势识别算法在人机交互和智能设备控制中具有重要的应用价值。本文将介绍基于STM32的手势识别算法的研究原理和实现步骤&#xff0c;并提供相应的代码示例。 1. 手势识别概述 手势识别是一种通过分析人体的手部动作和姿势来识别和理解人的意图的技术。基于STM32的…

Jina AI 的 8K 向量模型上线 AWS Marketplace,支持本地部署!

在当前多模态 AI 和大模型技术风头正劲的背景下&#xff0c;Jina AI 始终领跑于创新前沿&#xff0c;技术领先。2023 年 10 月 30 日&#xff0c;Jina AI 隆重推出 jina-embeddings-v2&#xff0c;这是全球首款支持 8192 输入长度的开源向量大模型&#xff0c;其性能媲美 OpenA…

如何将设置为静态IP的VMware虚拟机进行克隆以便可以复刻相应的环境

一定要关闭需要克隆的虚拟机右键要选择克隆的虚拟机&#xff0c;选择管理->克隆&#xff0c;进入克隆虚拟机向导 设定克隆出来的虚拟机名称以及位置&#xff0c;选择完成 克隆完成之后将会生成虚拟机&#xff0c;示例中生成的虚拟机为ubuntu-dev2 因为原本的虚拟机为静态ip的…

Pytorch中的tensor维度理解

Pytorch中的tensor维度理解 文章目录 Pytorch中的tensor维度理解摘要打消心理恐惧&#xff0c;从三维学起三维tensor参考文献 摘要 面对pytorch编程中的tensor时&#xff0c;我不时会感到恐惧。对里面数据是怎么排布的&#xff0c;一直没有一个直观的理解。今天我想把这个事情…

微软离Altman越近,离OpenAI就越远!

大数据产业创新服务媒体 ——聚焦数据 改变商业 在OpenAI这场连续剧中&#xff08;之所以说是连续剧&#xff0c;这个事情肯定没完&#xff0c;后面肯定还会出续集&#xff09;&#xff0c;让我倍感意外的是&#xff0c;Altman刚跟OpenAI分手&#xff0c;“离婚手续”都还没办…

模拟量采集----测量输入的电流

生活中的模拟量有很多 大多都为电压信号和电流信号 今天讲如何测量输入的电流信号 通过欧姆定律可知 电流测量的测量&#xff1a;是将电流加载在固定阻值的电阻上&#xff0c;来测量这个电阻二端的电压 最后反算出电流的大小 所用的公式是IU/R 我们使用仿真软件来看测量…

连接k8s和凌鲨

通过连接k8s和凌鲨&#xff0c;可以让研发过程中的重用操作更加方便。 更新容器镜像调整部署规模查看日志运行命令 架构 所有操作通过k8s proxy连接&#xff0c;通过设置namespace label赋予访问权限。只有赋予特定label的namespace才能被访问。 使用步骤 部署k8s proxy 你…

深度学习图像修复算法 - opencv python 机器视觉 计算机竞赛

文章目录 0 前言2 什么是图像内容填充修复3 原理分析3.1 第一步&#xff1a;将图像理解为一个概率分布的样本3.2 补全图像 3.3 快速生成假图像3.4 生成对抗网络(Generative Adversarial Net, GAN) 的架构3.5 使用G(z)生成伪图像 4 在Tensorflow上构建DCGANs最后 0 前言 &#…

基于SSM的旅游管理系统设计与实现

末尾获取源码 开发语言&#xff1a;Java Java开发工具&#xff1a;JDK1.8 后端框架&#xff1a;SSM 前端&#xff1a;采用JSP技术开发 数据库&#xff1a;MySQL5.7和Navicat管理工具结合 服务器&#xff1a;Tomcat8.5 开发软件&#xff1a;IDEA / Eclipse 是否Maven项目&#x…

如何用java的虚拟线程连接数据库

我觉得这个很简单 首先确保你idea支持jdk21. 然后把idea编译成的目标字节码设置为21版本的 然后编写代码。 创建虚拟线程的方式有&#xff1a; Runnable runnable () -> {System.out.println("Hello, world!"); };// 创建虚拟线程 Thread virtualThread Thre…

likeshop单商户商城系统 任意文件上传漏洞复现

0x01 产品简介 likeshop单商户标准商城系统适用于B2C、单商户、自营商城场景。完美契合私域流量变现闭环交易使用。 系统拥有丰富的营销玩法&#xff0c;强大的分销能力&#xff0c;支持电子面单和小程序直播等功能。无论运营还是二开都是性价比极高的100%开源商城系统。 0x02…

【OpenCV实现图像:使用OpenCV生成拼图效果】

文章目录 概要通用配置不考虑间隔代码实现考虑间隔代码实现小结 概要 概要&#xff1a; 拼图效果是一种将图像切割为相邻正方形并重新排列的艺术效果。在生成拼图效果时&#xff0c;可以考虑不同的模式&#xff0c;包括是否考虑间隔和如何处理不能整除的部分。 不考虑间隔&a…

java代码调用twitter-api用例实战

一、申请twitter开发者账号 首先先申请twitter开发者免费的API&#xff0c;要填写申请的内容&#xff0c;放心大胆地写&#xff0c;申请完&#xff0c;会提供免费的API接口。 以下是我申请到的三个免费API 申请完开始进行测试调用。 读官方文档账户认证那块&#xff1a;https…

Python 2.7 在 Debian 服务器上获取 URL 时的 SSL 验证失败问题与解决方案

在使用Python的requests库从Debian稳定服务器上获取简单URL时&#xff0c;遇到了SSL证书错误。 根据用户的问题描述&#xff0c;您遇到了SSL证书验证失败的问题。 要解决这个问题&#xff0c;您可以采取以下步骤&#xff1a; 1. 升级到Python 2.7的最新版本&#xff1a; 首…

基于SSM的课程资源管理系统

末尾获取源码 开发语言&#xff1a;Java Java开发工具&#xff1a;JDK1.8 后端框架&#xff1a;SSM 前端&#xff1a;采用JSP技术开发 数据库&#xff1a;MySQL5.7和Navicat管理工具结合 服务器&#xff1a;Tomcat8.5 开发软件&#xff1a;IDEA / Eclipse 是否Maven项目&#x…