用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的頂點需要順時針插入polygonVertex* 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 manuallyCREATE_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;
};#endifNavMeshScene.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 firstif (!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的頂點需要順時針插入polygonVertex* 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;
};#endifSearchPoint.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;
};#endifLine.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;
};#endifAStarQueue.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;
};#endifAStarNode.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;
};#endifGeometryMath.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:};#endifGeometryMath.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;
}
