RomkaZVO/FC1
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Files
main
FC1/CryAISystem/GraphUtility.cpp
romkazvo 34d6c5d489 123
2023-08-07 19:29:24 +08:00

1167 lines
32 KiB
C++
Raw Permalink Blame History

#include "stdafx.h"
#include "Graph.h"
#include "Heuristic.h"
#include "CAISystem.h"
#if !defined(LINUX)
#include <assert.h>
#endif
#include "Cry_Math.h"
#include <ISystem.h>
#include <Cry_Camera.h>
#include <IRenderer.h>
#include <ILog.h>
#include <I3DEngine.h>
#include <algorithm>
#include <IConsole.h>
#include "IPhysics.h"
//
//--------------------------------------------------------------------------------------------------
int CGraph::Rearrange( ListNodes& nodesList, const Vec3d& cutStart, const Vec3d& cutEnd )
{
int counter=0;
GraphNode *curNode;
ListNodes::iterator it;
int ndNumber=0;
for(it=nodesList.begin(); it!=nodesList.end(); it++, ndNumber++)
{
int counter=0;
curNode = (*it);
// VectorOfLinks::iterator vi;
if( curNode->vertex.size()!=3 || curNode == m_pSafeFirst)
{
nodesList.erase( it );
it=nodesList.begin();
}
}
counter = ProcessMegaMerge( nodesList, cutStart, cutEnd );
return counter;
}
//
//--------------------------------------------------------------------------------------------------
int CGraph::ProcessMegaMerge( ListNodes& nodesList, const Vec3d& vCutStart, const Vec3d& vCutEnd )
{
ListNodes leftSideNodes;
ListNodes rightSideNodes;
int Counter=0;
Vec3d cutDirection = vCutEnd-vCutStart;
//Obstacles leftOutline;
//Obstacles rightOutline;
ObstacleIndexList leftOutline;
ObstacleIndexList rightOutline;
//ListPositions leftOutline;
//ListPositions rightOutline;
GraphNode *curNode;
VectorOfLinks::iterator vi;
ObstacleData tmpObst;
tmpObst.vPos = vCutEnd;
int cutEnd = m_pAISystem->m_VertexList.FindVertex( tmpObst );
tmpObst.vPos = vCutStart;
int cutStart = m_pAISystem->m_VertexList.FindVertex( tmpObst );
if( cutStart<0 || cutEnd<0 )
{
m_pAISystem->m_pSystem->GetILog()->Log("\001 CGraph::ProcessMegaMerge cut is not in m_VertexList %d %d", cutStart, cutEnd);
AIWarning( "CGraph::ProcessMegaMerge cut is not in m_VertexList %d %d", cutStart, cutEnd);
assert(0);
return 0;
}
if( nodesList.empty() )
return 0;
ListNodes::iterator it;
DbgCheckList( nodesList );
Vec3d cutN = cutDirection.normalized();
for(it=nodesList.begin(); it!=nodesList.end(); it++)
{
curNode = (*it);
if (std::find(rightSideNodes.begin(),rightSideNodes.end(),curNode) != rightSideNodes.end()) // already in list - skip
continue;
if (std::find(leftSideNodes.begin(),leftSideNodes.end(),curNode) != leftSideNodes.end()) // already in list - skip
continue;
GraphLink* pNewLink = curNode->FindNewLink();
if( pNewLink )
{
GraphNode *nbrNode = pNewLink->pLink;
VectorOfLinks::iterator nbrVi;
float curCross = curNode->GetCross( vCutStart, cutN, *pNewLink );
for (nbrVi=nbrNode->link.begin();nbrVi!=nbrNode->link.end();nbrVi++)
if((*nbrVi).pLink == curNode )
break;
// mark neighbor - it's a new link in edge
(*nbrVi).fMaxRadius = -1.f;
float nbrCross = nbrNode->GetCross( vCutStart, cutN, (*nbrVi) );
if( fabs(nbrCross) > fabs(curCross) )
curCross = -nbrCross;
if( curCross<0 )
{
leftSideNodes.push_back( curNode );
rightSideNodes.push_back( nbrNode );
}
else
{
leftSideNodes.push_back( nbrNode );
rightSideNodes.push_back( curNode );
}
}
}
if(leftSideNodes.size()!=rightSideNodes.size())
{
assert(0); // triangulation optimisation error -- newly-marked nodes are not simmetrical
return 0;
}
DbgCheckList( nodesList );
for(it=nodesList.begin(); it!=nodesList.end(); it++)
{
curNode = (*it);
if (std::find(leftSideNodes.begin(),leftSideNodes.end(),curNode) != leftSideNodes.end()) // already in list - skip
continue;
if (std::find(rightSideNodes.begin(),rightSideNodes.end(),curNode) != rightSideNodes.end()) // already in list - skip
continue;
if( ( curNode->vertex[0] == cutStart ) ||
( curNode->vertex[1] == cutStart ) ||
( curNode->vertex[2] == cutStart ) ||
( curNode->vertex[0] == cutEnd ) ||
( curNode->vertex[1] == cutEnd ) ||
( curNode->vertex[2] == cutEnd ) )
continue; // don't touch begin/end of cut
bool bHasPointOnEdge=false;
int vIdx( 0 );
for( ; vIdx<3 && !bHasPointOnEdge; vIdx++ )
{
for(ListNodes::iterator li=rightSideNodes.begin();li!=rightSideNodes.end() && !bHasPointOnEdge;li++)
{
GraphNode *refNode = (*li);
GraphLink *pNewLink = refNode->FindNewLink();
if( !pNewLink )
continue;
if( refNode->vertex[pNewLink->nStartIndex] == curNode->vertex[vIdx] ||
refNode->vertex[pNewLink->nEndIndex] == curNode->vertex[vIdx] )
// found it - it's on cut - add the hode to outline
bHasPointOnEdge = true;
}
}
if( !bHasPointOnEdge )
continue;
vIdx--;
// so just check now if it's left or right side and add to appropriate list
int otherVertexIdx1 = 2 - vIdx;
int otherVertexIdx2 = 3 - (otherVertexIdx1 + vIdx);
Vec3d theOtherVertex = m_pAISystem->m_VertexList.GetVertex(curNode->vertex[otherVertexIdx1]).vPos
- vCutStart;
theOtherVertex.normalize();
float curCross = cutN.x * theOtherVertex.y - cutN.y * theOtherVertex.x;
theOtherVertex = m_pAISystem->m_VertexList.GetVertex(curNode->vertex[otherVertexIdx2]).vPos
- vCutStart;
theOtherVertex.normalize();
float cross2 = cutN.x * theOtherVertex.y - cutN.y * theOtherVertex.x;
if( fabs(curCross) < fabs(cross2) )
curCross = cross2;
if( curCross<0 )
leftSideNodes.push_back( curNode );
else
rightSideNodes.push_back( curNode );
/*
Vec3d theOtherVertex = m_pAISystem->m_VertexList.GetVertex(
curNode->vertex[3-((*vi).nStartIndex + (*vi).nEndIndex)]).vPos
- vCutStart;
float cross = cutDirection.x * theOtherVertex.y - cutDirection.y * theOtherVertex.x;
if( cross<0 )
{
if (std::find(leftSideNodes.begin(),leftSideNodes.end(),(*vi).pLink) != leftSideNodes.end())
{
bool hasPointOnEdge = false;
for(ListNodes::iterator li=leftSideNodes.begin();li!=leftSideNodes.end() && !hasPointOnEdge;li++)
{
GraphNode *refNode = (*li);
VectorOfLinks::iterator refLinks;
for(refLinks=refNode->link.begin();refLinks!=refNode->link.end();refLinks++)
if((*refLinks).IsNewLink())
{
if( ( refNode->vertex[(*refLinks).nStartIndex] == curNode->vertex[(*vi).nStartIndex] ) ||
( refNode->vertex[(*refLinks).nStartIndex] == curNode->vertex[(*vi).nEndIndex] ) ||
( refNode->vertex[(*refLinks).nEndIndex] == curNode->vertex[(*vi).nStartIndex] ) ||
( refNode->vertex[(*refLinks).nEndIndex] == curNode->vertex[(*vi).nEndIndex] )
)
{
hasPointOnEdge = true;
}
break;
}
}
if( hasPointOnEdge )
{
leftSideNodes.push_back( curNode );
startFromBeginning = true;
break;
}
// break;
}
}
else
if (std::find(rightSideNodes.begin(),rightSideNodes.end(),(*vi).pLink) != rightSideNodes.end())
{
bool hasPointOnEdge = false;
for(ListNodes::iterator li=rightSideNodes.begin();li!=rightSideNodes.end() && !hasPointOnEdge;li++)
{
GraphNode *refNode = (*li);
VectorOfLinks::iterator refLinks;
for(refLinks=refNode->link.begin();refLinks!=refNode->link.end();refLinks++)
if((*refLinks).IsNewLink())
{
if( (refNode->vertex[(*refLinks).nStartIndex] == curNode->vertex[(*vi).nStartIndex] ) ||
(refNode->vertex[(*refLinks).nStartIndex] == curNode->vertex[(*vi).nEndIndex] ) ||
(refNode->vertex[(*refLinks).nEndIndex] == curNode->vertex[(*vi).nStartIndex] ) ||
(refNode->vertex[(*refLinks).nEndIndex] == curNode->vertex[(*vi).nEndIndex] )
)
{
hasPointOnEdge = true;
}
break;
}
}
if( hasPointOnEdge )
{
//curNode->data.fSlope = 1111;
rightSideNodes.push_back( curNode );
startFromBeginning = true;
break;
}
// break;
}
*/
// }
// if( startFromBeginning )
// it=nodesList.begin();
// else
// it++;
}
//return 0;
if( leftSideNodes.size() > 1 )
{
// create left outline
leftOutline.push_front( cutEnd );
leftOutline.push_front( cutStart );
if(!CreateOutline( leftSideNodes, nodesList, leftOutline ))
return 0; // some strange problems (see CreateOutline) - don't optimize this segment, return
}
else
leftSideNodes.clear();
DbgCheckList( nodesList );
if( rightSideNodes.size() > 1 )
{
// create right outline
rightOutline.push_front( cutEnd );
rightOutline.push_front( cutStart );
if(!CreateOutline( rightSideNodes, nodesList, rightOutline ))
return 0; // some strange problems (see CreateOutline) - don't optimize this segment, return
}
else
rightSideNodes.clear();
m_DEBUG_outlineListL.clear();
// m_DEBUG_outlineListL.insert(m_DEBUG_outlineListL.begin(),leftOutline.begin(), leftOutline.end() );
m_DEBUG_outlineListR.clear();
// m_DEBUG_outlineListR.insert(m_DEBUG_outlineListR.begin(),rightOutline.begin(), rightOutline.end() );
DbgCheckList( nodesList );
for( it=leftSideNodes.begin(); !leftSideNodes.empty() && it!=leftSideNodes.end(); it++)
{
curNode = (*it);
curNode->data.fSlope = 1111;
Disconnect( curNode );
ListNodes::iterator itList = std::find(nodesList.begin(),nodesList.end(),curNode);
nodesList.erase( itList );
}
DbgCheckList( nodesList );
for( it=rightSideNodes.begin(); !rightSideNodes.empty() && it!=rightSideNodes.end(); it++)
{
curNode = (*it);
Disconnect( curNode );
ListNodes::iterator itList = std::find(nodesList.begin(),nodesList.end(),curNode);
nodesList.erase( itList );
}
//*/
//return 0;
//DbgCheckList( nodesList );
// ProcessRearrange( nodesList, cutStart, cutEnd );
DbgCheckList( nodesList );
TriangulateOutline( nodesList, leftOutline, false );
//return 0;
DbgCheckList( nodesList );
TriangulateOutline( nodesList, rightOutline, true );
DbgCheckList( nodesList );
ConnectNodes( nodesList );
DbgCheckList( nodesList );
for( it=nodesList.begin(); it!=nodesList.end(); it++)
{
curNode = (*it);
for (vi=curNode->link.begin();vi!=curNode->link.end();vi++)
if( (*vi).fMaxRadius<0.0f ||
// (curNode->vertex[(*vi).nStartIndex].vPos.x==cutStart.x &&
// curNode->vertex[(*vi).nStartIndex].vPos.y==cutStart.y &&
/// curNode->vertex[(*vi).nEndIndex].vPos.x==cutEnd.x &&
// curNode->vertex[(*vi).nEndIndex].vPos.y==cutEnd.y ) ||
// (curNode->vertex[(*vi).nEndIndex].vPos.x==cutStart.x &&
// curNode->vertex[(*vi).nEndIndex].vPos.y==cutStart.y &&
// curNode->vertex[(*vi).nStartIndex].vPos.x==cutEnd.x &&
// curNode->vertex[(*vi).nStartIndex].vPos.y==cutEnd.y )
// )
(curNode->vertex[(*vi).nStartIndex]==cutStart &&
curNode->vertex[(*vi).nEndIndex]==cutEnd) ||
(curNode->vertex[(*vi).nEndIndex]==cutStart&&
curNode->vertex[(*vi).nStartIndex]==cutEnd)
)
(*vi).fMaxRadius = -10.0f;
}
DbgCheckList( nodesList );
return Counter;
}
//
//--------------------------------------------------------------------------------------------------
// creates outline of insideNodes, removes all triangles in outline from nodesList
bool CGraph::CreateOutline( ListNodes& insideNodes, ListNodes& nodesList, ObstacleIndexList& outline)
{
ListNodes::iterator it;
VectorOfLinks::iterator vi;
GraphNode *curNode;
//GraphNode *prevNode;
Vec3d curOtherVertex;
bool doneHere = false;
ListNodes insideNodesCurrent = insideNodes;
for(it=insideNodesCurrent.begin(); !doneHere; it++)
//for(it=insideNodesCurrent.begin(); it!=insideNodesCurrent.end(); it++)
{
if( it==insideNodesCurrent.end() )
{
m_pAISystem->m_pSystem->GetILog()->Log("\001 CGraph::CreateOutline can't find OutlineBegin ");
AIWarning( "CGraph::CreateOutline can't find OutlineBegin ");
assert( 0 );
return false;
}
curNode = (*it);
for (vi=curNode->link.begin();vi!=curNode->link.end();vi++)
{
if( (*vi).IsNewLink() ) // it's on the edge - it's a new node
if( (curNode->vertex[(*vi).nStartIndex] == (*outline.begin()) ) ||
(curNode->vertex[(*vi).nEndIndex] == (*outline.begin()) ) )
{
int theOtherVertexIdx = curNode->vertex[3-((*vi).nStartIndex + (*vi).nEndIndex)];
// Vec3d theOtherVertex = m_pAISystem->m_VertexList.GetVertex(theOtherVertexIdx).vPos;
if( std::find( outline.begin(), outline.end(),theOtherVertexIdx ) == outline.end())
outline.push_front( theOtherVertexIdx );
// else
// assert(0);
it = insideNodesCurrent.erase( it );
doneHere=true;
break;
}
}
if (doneHere)
break;
}
// assert(curNode); //
if( !curNode )
{
m_pAISystem->m_pSystem->GetILog()->Log("\001 CGraph::CreateOutline curNode is NULL ");
AIWarning( "CGraph::CreateOutline curNode is NULL ");
assert( 0 );
return false;
}
while( !insideNodesCurrent.empty() )
{
GraphNode *nextCandidate = NULL;
// Vec3d theOtherVertex;
// Vec3d nextOtherVertex;
int theOtherVertexIdx;
int nextOtherVertexIdx=0;
bool bAddVertex = true;
for (vi=curNode->link.begin(); vi!=curNode->link.end()&&bAddVertex; vi++)
if((*vi).IsNewLink())
bAddVertex = false;
//
// if has links on base
for (vi=curNode->link.begin();vi!=curNode->link.end();vi++)
{
GraphNode *nextNode = (*vi).pLink;
if ((it=std::find(insideNodesCurrent.begin(),insideNodesCurrent.end(),nextNode)) != insideNodesCurrent.end())
{
VectorOfLinks::iterator viNext;
for (viNext=nextNode->link.begin();viNext!=nextNode->link.end();viNext++)
if( (*viNext).IsNewLink() )
{
nextCandidate = nextNode;
insideNodesCurrent.erase( it );
break;
}
}
}
//
// if has noBase links
for (vi=curNode->link.begin();vi!=curNode->link.end();vi++)
{
GraphNode *nextNode = (*vi).pLink;
if ((it=std::find(insideNodesCurrent.begin(),insideNodesCurrent.end(),nextNode)) != insideNodesCurrent.end())
{
VectorOfLinks::iterator viNext;
for (viNext=nextNode->link.begin();viNext!=nextNode->link.end();viNext++)
if( (*viNext).pLink == curNode )
{
if( !nextCandidate ) // no base link - use this one
{
nextCandidate = nextNode;
insideNodesCurrent.erase( it );
nextOtherVertexIdx = nextNode->vertex[3-((*viNext).nStartIndex + (*viNext).nEndIndex)];
}
else
bAddVertex = false;
break;
}
}
}
if(bAddVertex)
{
if( std::find( outline.begin(), outline.end(),theOtherVertexIdx ) == outline.end())
outline.push_front( theOtherVertexIdx );
}
theOtherVertexIdx = nextOtherVertexIdx;
// assert(nextCandidate); //
if( !nextCandidate )
{
m_pAISystem->m_pSystem->GetILog()->Log("\001 CGraph::CreateOutline nextCandidate is NULL ");
AIWarning( "CGraph::CreateOutline nextCandidate is NULL ");
assert( 0 );
return false;
}
curNode = nextCandidate;
}
return true;
}
//
//--------------------------------------------------------------------------------------------------
void CGraph::TriangulateOutline( ListNodes& nodesList, ObstacleIndexList& outline, bool orientation )
{
if(outline.empty())
return;
//ListPositions::reverse_iterator crItr=outline.rbegin();
//Vec3d vCut1 = *crItr++;
//Vec3d vCut2 = *crItr;
GraphNode *candidateNode;
ObstacleData obst;
GraphNode tmpNode;
ObstacleIndexList originalOutline;
originalOutline.insert(originalOutline.begin(), outline.begin(), outline.end());
//m_DEBUG_outlineListR.clear();
while(outline.size()>3)
{
// GraphNode tmpNode;
// ListPositions::iterator candidateRemove=outline.end();
ObstacleIndexList::iterator candidateRemove=outline.end();
bool notGood=false;
float candidateDegeneracy=-1.0f;
candidateNode = CreateNewNode();
int cntr=1;
for(ObstacleIndexList::iterator curItr=outline.begin(); curItr!=outline.end(); curItr++,cntr++)
{
ObstacleIndexList::iterator vItr1=curItr;
ObstacleIndexList::iterator vItr2=curItr;
if( cntr==outline.size() )
{
vItr1 = outline.begin();
vItr2 = vItr1;
++vItr2;
}
else if( cntr==outline.size()-1 )
{
vItr2 = outline.begin();
++vItr1;
}
else
{
++vItr1;
++vItr2;
++vItr2;
}
tmpNode.vertex.clear();
// obst.vPos = *curItr;
tmpNode.vertex.push_back( *curItr );
// obst.vPos = *vItr1;
tmpNode.vertex.push_back( *vItr1 );
// obst.vPos = *vItr2;
tmpNode.vertex.push_back( *vItr2 );
notGood = false;
if(tmpNode.IsAntiClockwise()!=orientation)
notGood = true;
tmpNode.MakeAntiClockwise();
// check if enithing inside the curremt triangle
for(ObstacleIndexList::iterator tmpItr=originalOutline.begin(); tmpItr!=originalOutline.end() && !notGood; tmpItr++)
if (
// IsEquivalent()
*tmpItr == *curItr || *tmpItr == *vItr1 || *tmpItr == *vItr2 )
continue;
// else if(PointInTriangle(*tmpItr, &tmpNode))
else if(PointInTriangle(m_pAISystem->m_VertexList.GetVertex(*tmpItr).vPos, &tmpNode))
notGood = true;
if( notGood ) // this triangle can't be used
continue;
float dVal = tmpNode.GetDegeneracyValue();
/*
if( (vCut1==*curItr && vCut2==*vItr1) ||
(vCut2==*curItr && vCut1==*vItr1) ||
(vCut1==*curItr && vCut2==*vItr2) ||
(vCut2==*curItr && vCut1==*vItr2) ||
(vCut1==*vItr1 && vCut2==*vItr2) ||
(vCut2==*vItr1 && vCut1==*vItr2) )
dVal += 100;
*/
if( dVal>candidateDegeneracy ) // better candidate
{
candidateDegeneracy = dVal;
candidateNode->vertex = tmpNode.vertex;
candidateRemove = vItr1;
}
}
//
//add the best candidate - remove vertex from outline
candidateNode->MakeAntiClockwise();
FillGraphNodeData( candidateNode );
nodesList.push_back( candidateNode );
assert(candidateRemove != outline.end());
outline.erase( candidateRemove );
}
//
// add the last triangle
candidateNode = CreateNewNode();
// obst.vPos = outline.front();
// outline.pop_front();
candidateNode->vertex.push_back( outline.front() );
outline.pop_front();
// obst.vPos = outline.back();
candidateNode->vertex.push_back( outline.back() );
// obst.vPos = outline.front();
candidateNode->vertex.push_back( outline.front() );
candidateNode->MakeAntiClockwise();
FillGraphNodeData( candidateNode );
nodesList.push_back( candidateNode );
}
/*
*
//
//--------------------------------------------------------------------------------------------------
void CGraph::TriangulateOutline( ListNodes& nodesList, ListPositions& outline, bool orientation )
{
if(outline.empty())
return;
GraphNode *candidateNode;
ObstacleData obst;
GraphNode tmpNode;
while(outline.size()>3)
{
// GraphNode tmpNode;
ListPositions::iterator candidateRemove=outline.end();
ListPositions::iterator tmpVertex;
ListPositions::reverse_iterator tmpLastVertex;
bool notGood=false;
float candidateDegeneracy=-1.0f;
candidateNode = CreateNewNode();
//
// first triangle - begin-next-next
tmpNode.vertex.clear();
tmpVertex = outline.begin();
obst.vPos = *tmpVertex;
tmpNode.vertex.push_back( obst );
tmpVertex++;
obst.vPos = *tmpVertex;
tmpNode.vertex.push_back( obst );
tmpVertex++;
obst.vPos = *tmpVertex;
tmpNode.vertex.push_back( obst );
notGood = false;
if(tmpNode.IsAntiClockwise()!=orientation)
notGood = true;
tmpNode.MakeAntiClockwise();
// check if enithing inside the curremt triangle
for(tmpVertex++; tmpVertex!=outline.end()&&!notGood;tmpVertex++)
if(PointInTriangle(*tmpVertex, &tmpNode))
notGood = true;
if(!notGood)
{
float dVal = tmpNode.GetDegeneracyValue();
if( dVal>candidateDegeneracy ) // better candidate
{
candidateDegeneracy = dVal;
candidateNode->vertex = tmpNode.vertex;
candidateRemove = ++outline.begin();
}
}
//
// second triangle - begin-next-end
tmpNode.vertex.clear();
tmpVertex = outline.begin();
obst.vPos = *tmpVertex;
tmpNode.vertex.push_back( obst );
tmpVertex++;
obst.vPos = *tmpVertex;
tmpNode.vertex.push_back( obst );
obst.vPos = *(outline.rbegin());
tmpNode.vertex.push_back( obst );
notGood = false;
if(tmpNode.IsAntiClockwise()!=orientation)
notGood = true;
tmpNode.MakeAntiClockwise();
// check if enithing inside the curremt triangle
tmpLastVertex = outline.rbegin();
tmpLastVertex++;
for(tmpVertex++;
//*tmpVertex!= *tmpLastVertex;
!IsEquivalent(*tmpVertex,*tmpLastVertex,VEC_EPSILON)
&& !notGood ;
tmpVertex++
)
if(PointInTriangle(*tmpVertex, &tmpNode))
notGood = true;
if(!notGood)
{
float dVal = tmpNode.GetDegeneracyValue();
if( dVal>candidateDegeneracy ) // better candidate
{
candidateDegeneracy = dVal;
candidateNode->vertex = tmpNode.vertex;
candidateRemove = outline.begin();
}
}
//
// third triangle - begin-end-preend
tmpNode.vertex.clear();
obst.vPos = *outline.begin();
tmpNode.vertex.push_back( obst );
tmpLastVertex = outline.rbegin();
tmpLastVertex++; // get preend
obst.vPos = *tmpLastVertex;
tmpNode.vertex.push_back( obst );
tmpLastVertex = outline.rbegin(); // get end
obst.vPos = *tmpLastVertex;
tmpNode.vertex.push_back( obst );
notGood = false;
if(tmpNode.IsAntiClockwise()!=orientation)
notGood = true;
tmpNode.MakeAntiClockwise();
tmpLastVertex = outline.rbegin();
tmpLastVertex++; // set to preend
// check if enithing inside the curremt triangle
for(tmpLastVertex++;
//*tmpLastVertex!=*outline.begin()
!IsEquivalent(*tmpLastVertex,*outline.begin(),VEC_EPSILON)
&&!notGood;
tmpLastVertex++
)
if(PointInTriangle(*tmpLastVertex, &tmpNode))
notGood = true;
if(!notGood)
{
float dVal = tmpNode.GetDegeneracyValue();
if( dVal>candidateDegeneracy ) // better candidate
{
candidateDegeneracy = dVal;
candidateNode->vertex = tmpNode.vertex;
candidateRemove = (++outline.rbegin()).base();
}
}
//
// last triangle - end-preend-prepreend
tmpNode.vertex.clear();
tmpLastVertex = outline.rbegin();
tmpLastVertex++;
tmpLastVertex++; // pre-preend
obst.vPos = *tmpLastVertex;
tmpNode.vertex.push_back( obst );
tmpLastVertex--; // pre-end
obst.vPos = *tmpLastVertex;
tmpNode.vertex.push_back( obst );
tmpLastVertex--; // end
obst.vPos = *tmpLastVertex;
tmpNode.vertex.push_back( obst );
notGood = false;
if(tmpNode.IsAntiClockwise()!=orientation)
notGood = true;
tmpNode.MakeAntiClockwise();
tmpLastVertex++;
tmpLastVertex++; // set to pre-preend
// check if enithing inside the curremt triangle
for(tmpLastVertex++; tmpLastVertex!=(outline.rend())&&!notGood;tmpLastVertex++)
if(PointInTriangle(*tmpLastVertex, &tmpNode))
notGood = true;
if(!notGood)
{
float dVal = tmpNode.GetDegeneracyValue();
if( dVal>candidateDegeneracy ) // better candidate
{
candidateDegeneracy = dVal;
candidateNode->vertex = tmpNode.vertex;
candidateRemove = (++(++outline.rbegin())).base();
// candidateRemove--;
}
}
//
//add the best candidate - remove vertex from outline
candidateNode->MakeAntiClockwise();
FillGraphNodeData( candidateNode );
nodesList.push_back( candidateNode );
assert(candidateRemove != outline.end());
ListPositions::iterator dbg=std::find(m_DEBUG_outlineListR.begin(), m_DEBUG_outlineListR.end(), *candidateRemove);
if(dbg!=m_DEBUG_outlineListR.end())
(*dbg).z = -500;
outline.erase( candidateRemove );
}
//
// add the last triangle
candidateNode = CreateNewNode();
obst.vPos = outline.front();
// obst.bOccupied = false;
outline.pop_front();
candidateNode->vertex.push_back( obst );
obst.vPos = outline.back();
// obst.bOccupied = false;
candidateNode->vertex.push_back( obst );
obst.vPos = outline.front();
//obst.bOccupied = false;
candidateNode->vertex.push_back( obst );
candidateNode->MakeAntiClockwise();
FillGraphNodeData( candidateNode );
nodesList.push_back( candidateNode );
}
*/
//
//--------------------------------------------------------------------------------------------------
int CGraph::ProcessRearrange( ListNodes& nodesList, const Vec3d& cutStart, const Vec3d& cutEnd )
{
return 0;
/*
int counter = 0;
ListNodes::iterator it;
VectorOfLinks::iterator vi;
Obstacles::iterator oi;
int dbgcounter=0;
for(it=nodesList.begin();it!=nodesList.end();it++)
{
dbgcounter++;
GraphNode *curNode = (*it);
// if(curNode->link.empty())
if(curNode->link.size()!=3)
continue;
bool onCut = false;
for (oi=curNode->vertex.begin();oi!=curNode->vertex.end(); oi++)
if( IsEquivalent((*oi).vPos,cutStart,VEC_EPSILON) || IsEquivalent((*oi).vPos,cutEnd,VEC_EPSILON) )
{
onCut = true;
break;
}
if(!onCut)
continue;
for (vi=curNode->link.begin();vi!=curNode->link.end();vi++)
{
if ( (*vi).fMaxRadius<0.0f ) // it's cut edge
continue;
if ((*vi).pLink->link.size()!=3)
continue;
if (std::find(nodesList.begin(),nodesList.end(),(*vi).pLink) != nodesList.end()) // link is in list as well
{
DbgCheckList( nodesList );
if(ProcessRearrange( curNode, (*vi).pLink, nodesList ))
{
it=nodesList.begin();
counter++;
DbgCheckList( nodesList );
break;
}
DbgCheckList( nodesList );
// break;
}
}
}
return counter;
*/
}
//
//--------------------------------------------------------------------------------------------------
bool CGraph::ProcessRearrange( GraphNode *node1, GraphNode *node2, ListNodes& nodesList )
{
return false;
/*
float dValue;
float dValue1 = node1->GetDegeneracyValue();
float dValue2 = node2->GetDegeneracyValue();
GraphNode tmpNode;
ObstacleData obst[4];
unsigned int other1VertexIdx;
unsigned int other2VertexIdx;
if(dValue1>dValue2)
dValue1 = dValue2;
if( dValue1>.02f )
return false;
VectorOfLinks::iterator vi;
for (vi=node2->link.begin();vi!=node2->link.end();vi++)
if( (*vi).pLink == node1 )
{
other2VertexIdx = 3 - ((*vi).nStartIndex + (*vi).nEndIndex);
break;
}
for (vi=node1->link.begin();vi!=node1->link.end();vi++)
if( (*vi).pLink == node2 )
{
other1VertexIdx = 3 - ((*vi).nStartIndex + (*vi).nEndIndex);
break;
}
//check if it is possible configuration for rearranement
Vec3d newEdge = node1->vertex[other1VertexIdx].vPos - node2->vertex[other2VertexIdx].vPos;
Vec3d v1 = node1->vertex[(*vi).nStartIndex].vPos - node2->vertex[other2VertexIdx].vPos;
Vec3d v2 = node1->vertex[(*vi).nEndIndex].vPos - node2->vertex[other2VertexIdx].vPos;
float cross1 = v1.x * newEdge.y - v1.y * newEdge.x;
float cross2 = v2.x * newEdge.y - v2.y * newEdge.x;
if( cross1<0 && cross2<0 || cross1>0 && cross2>0 )
return false; // can't rearrange this two triangles
obst[0] = node1->vertex[other1VertexIdx];
obst[1] = node1->vertex[(*vi).nStartIndex];
obst[2] = node2->vertex[other2VertexIdx];
obst[3] = node1->vertex[(*vi).nEndIndex];
// obst[0].bOccupied = false;
// obst[1].bOccupied = false;
// obst[2].bOccupied = false;
// obst[3].bOccupied = false;
tmpNode.vertex.push_back( obst[0] );
tmpNode.vertex.push_back( obst[1] );
tmpNode.vertex.push_back( obst[2] );
dValue = tmpNode.GetDegeneracyValue();
if( dValue<=dValue1 )
return false;
tmpNode.vertex.clear();
tmpNode.vertex.push_back( obst[0] );
tmpNode.vertex.push_back( obst[2] );
tmpNode.vertex.push_back( obst[3] );
dValue = tmpNode.GetDegeneracyValue();
if( dValue<=dValue1 )
return false;
DbgCheckList( nodesList );
Disconnect( node1, true );
//DbgCheckList( nodesList );
Disconnect( node2, true );
//( nodesList );
ListNodes::iterator nIt = std::find(nodesList.begin(),nodesList.end(),node1);
if(nIt == nodesList.end())
DbgCheckList( nodesList );
// if( nIt!=nodesList.end() )
nodesList.erase( nIt );
nIt = std::find(nodesList.begin(),nodesList.end(),node2);
if(nIt == nodesList.end())
DbgCheckList( nodesList );
nodesList.erase( nIt );
DbgCheckList( nodesList );
node1 = CreateNewNode();
node1->vertex.push_back( obst[0] );
node1->vertex.push_back( obst[1] );
node1->vertex.push_back( obst[2] );
node1->MakeAntiClockwise();
FillGraphNodeData( node1 );
node2 = CreateNewNode();
node2->vertex.push_back( obst[0] );
node2->vertex.push_back( obst[2] );
node2->vertex.push_back( obst[3] );
node2->MakeAntiClockwise();
FillGraphNodeData( node2 );
nodesList.push_front( node1 );
nodesList.push_front( node2 );
return true;
*/
}
//
//--------------------------------------------------------------------------------------------------
//
//--------------------------------------------------------------------------------------------------
//
//--------------------------------------------------------------------------------------------------
float GraphNode::GetDegeneracyValue()
{
CVertexList *pVList = &GetAISystem()->m_VertexList;
Vec3d firstE = pVList->GetVertex(vertex[1]).vPos - pVList->GetVertex(vertex[0]).vPos;
Vec3d secondE = pVList->GetVertex(vertex[2]).vPos - pVList->GetVertex(vertex[0]).vPos;
firstE.Normalize();
secondE.Normalize();
float cosAngle0 = 1.0f - firstE.Dot( secondE );
firstE = pVList->GetVertex(vertex[0]).vPos - pVList->GetVertex(vertex[1]).vPos;
secondE = pVList->GetVertex(vertex[2]).vPos - pVList->GetVertex(vertex[1]).vPos;
firstE.Normalize();
secondE.Normalize();
float cosAngle1 = 1.0f - firstE.Dot( secondE );
firstE = pVList->GetVertex(vertex[0]).vPos - pVList->GetVertex(vertex[2]).vPos;
secondE = pVList->GetVertex(vertex[1]).vPos - pVList->GetVertex(vertex[2]).vPos;
firstE.Normalize();
secondE.Normalize();
float cosAngle2 = 1.0f - firstE.Dot( secondE );
if( cosAngle0<=cosAngle1 && cosAngle0<=cosAngle2 )
return cosAngle0;
if( cosAngle1<=cosAngle2 && cosAngle1<=cosAngle0 )
return cosAngle1;
return cosAngle2;
}
//
//--------------------------------------------------------------------------------------------------
void GraphNode::MakeAntiClockwise()
{
if (vertex.size() < 3)
return;
CVertexList *pVList = &GetAISystem()->m_VertexList;
int od1 = vertex[0];
int od2 = vertex[1];
int od3 = vertex[2];
Vec3d one = pVList->GetVertex(od2).vPos - pVList->GetVertex(od1).vPos;
Vec3d two = pVList->GetVertex(od3).vPos - pVList->GetVertex(od2).vPos;
float fcrossz = one.x * two.y - two.x * one.y;
if (fcrossz<0)
{
// rearrange the first and second
vertex.clear();
vertex.push_back(od2);
vertex.push_back(od1);
vertex.push_back(od3);
// MakeAntiClockwise();
}
}
//
//--------------------------------------------------------------------------------------------------
bool GraphNode::IsAntiClockwise()
{
if (vertex.size() < 3)
return false;
CVertexList *pVList = &GetAISystem()->m_VertexList;
int od1 = vertex[0];
int od2 = vertex[1];
int od3 = vertex[2];
Vec3d one = pVList->GetVertex(od2).vPos - pVList->GetVertex(od1).vPos;
Vec3d two = pVList->GetVertex(od3).vPos - pVList->GetVertex(od2).vPos;
float fcrossz = one.x * two.y - two.x * one.y;
if (fcrossz<0)
return false;
return true;
}
//
//--------------------------------------------------------------------------------------------------
bool CGraph::DbgCheckList( ListNodes& nodesList ) const
{
bool isGood = true;
GraphNode *curNode;
ListNodes::iterator it;
int ndNumber=0;
for(it=nodesList.begin(); it!=nodesList.end(); it++, ndNumber++)
{
int counter=0;
curNode = (*it);
VectorOfLinks::iterator vi;
for (vi=curNode->link.begin();vi!=curNode->link.end();vi++)
if( (*vi).IsNewLink() ) // it's on the edge - it's a new node
counter++;
if(counter > 1)
isGood = false;
if( curNode->vertex.size()<3 )
isGood = false;
if (curNode->nRefCount>1 && curNode->nRefCount<4)
isGood = false;
ListNodes::iterator li;
ListNodes::iterator itnext = it;
if (++itnext != nodesList.end())
{
li = std::find(itnext,nodesList.end(),(*it));
if (li!=nodesList.end())
isGood = false;
}
}
return isGood;
}
//
//--------------------------------------------------------------------------------------------------
float GraphNode::GetCross( const Vec3d &vCutStart, const Vec3d &vDir, const GraphLink &theLink )
{
Vec3d theOtherVertex = GetAISystem()->m_VertexList.GetVertex(vertex[3-(theLink.nStartIndex + theLink.nEndIndex)]).vPos
- vCutStart;
theOtherVertex.normalize();
return vDir.x * theOtherVertex.y - vDir.y * theOtherVertex.x;
}
//
//--------------------------------------------------------------------------------------------------
GraphLink* GraphNode::FindNewLink()
{
VectorOfLinks::iterator vi;
for (vi=link.begin();vi!=link.end();vi++)
if( (*vi).IsNewLink() ) // it's on the edge - it's a new node
return &(*vi);
return NULL;
}
//
//--------------------------------------------------------------------------------------------------
Reference in New Issue View Git Blame Copy Permalink