502 lines
15 KiB
C++
502 lines
15 KiB
C++
/* Copyright (C) Greg Snook, 2000.
|
|
* All rights reserved worldwide.
|
|
*
|
|
* This software is provided "as is" without express or implied
|
|
* warranties. You may freely copy and compile this source into
|
|
* applications you distribute provided that the copyright text
|
|
* below is included in the resulting source code, for example:
|
|
* "Portions Copyright (C) Greg Snook, 2000"
|
|
*/
|
|
#define NAVIGATIONCELL_CPP
|
|
/****************************************************************************************\
|
|
NavigationCell.cpp
|
|
|
|
NavigationCell component implementation for the Navimesh sample program.
|
|
Included as part of the Game Programming Gems sample code.
|
|
|
|
Created 3/18/00 by Greg Snook
|
|
greg@mightystudios.com
|
|
-------------------------------------------------------------------------------------
|
|
Notes/Revisions:
|
|
|
|
\****************************************************************************************/
|
|
|
|
#include "stdafx.h"
|
|
#include "navigationcell.h"
|
|
#include "navigationheap.h"
|
|
#include <stdlib.h>
|
|
|
|
#ifdef _DEBUG
|
|
#define new new(_NORMAL_BLOCK,__FILE__,__LINE__)
|
|
#endif
|
|
|
|
NavigationCell::NavigationCell() : m_SessionID(0),
|
|
m_Open( false ),
|
|
m_ArrivalCost( 0 ),
|
|
m_Heuristic( 0 ),
|
|
m_ArrivalWall( 0 ),
|
|
m_bUse( true ),
|
|
m_bEAttribute( false ),
|
|
m_iIndex( -1 ),
|
|
m_Type( CT_TERRAIN ),
|
|
m_nNavMeshType( 0 )
|
|
{
|
|
memset( m_nWallAttribute, 0, sizeof( int ) * 3 );
|
|
memset( m_Link, 0, sizeof( m_Link ) );
|
|
memset( m_WallDistance, 0, sizeof( m_WallDistance ) );
|
|
};
|
|
|
|
|
|
// ClassifyPathToCell
|
|
//------------------------------------------------------------------------------------------
|
|
//
|
|
// Classifies a Path in relationship to this cell. A path is represented by a 2D line
|
|
// where Point A is the start of the path and Point B is the desired position.
|
|
//
|
|
// If the path exits this cell on a side which is linked to another cell, that cell index
|
|
// is returned in the NextCell parameter and SideHit contains the side number of the wall
|
|
// exited through.
|
|
//
|
|
// If the path collides with a side of the cell which has no link (a solid edge),
|
|
// SideHit contains the side number (0-2) of the colliding wall.
|
|
//
|
|
// In either case PointOfIntersection will contain the point where the path intersected
|
|
// with the wall of the cell if it is provided by the caller.
|
|
//
|
|
//------------------------------------------------------------------------------------------
|
|
NavigationCell::PATH_RESULT NavigationCell::ClassifyPathToCell(const Line2D& MotionPath, NavigationCell** pNextCell, CELL_SIDE& Side, EtVector2* pPointOfIntersection) const
|
|
{
|
|
int InteriorCount = 0;
|
|
for (int i=0; i<3; ++i)
|
|
{
|
|
if (m_Side[i].ClassifyPoint(MotionPath.EndPointB()) != Line2D::RIGHT_SIDE)
|
|
{
|
|
if (m_Side[i].ClassifyPoint(MotionPath.EndPointA()) != Line2D::LEFT_SIDE)
|
|
{
|
|
Line2D::LINE_CLASSIFICATION IntersectResult = MotionPath.Intersection(m_Side[i], pPointOfIntersection);
|
|
if (IntersectResult == Line2D::SEGMENTS_INTERSECT || IntersectResult == Line2D::A_BISECTS_B)
|
|
{
|
|
*pNextCell = m_Link[i];
|
|
Side = (CELL_SIDE)i;
|
|
return (EXITING_CELL);
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
InteriorCount++;
|
|
}
|
|
}
|
|
if (InteriorCount == 3)
|
|
{
|
|
return (ENDING_CELL);
|
|
}
|
|
return (NO_RELATIONSHIP);
|
|
}
|
|
|
|
/* ProjectPathOnCellWall
|
|
------------------------------------------------------------------------------------------
|
|
|
|
ProjectPathOnCellWall projects a path intersecting the wall with the wall itself. This
|
|
can be used to convert a path colliding with a cell wall to a resulting path moving
|
|
along the wall. The input parameter MotionPath MUST contain a starting point (EndPointA)
|
|
which is the point of intersection with the path and cell wall number [SideNumber]
|
|
and an ending point (EndPointB) which resides outside of the cell.
|
|
|
|
------------------------------------------------------------------------------------------
|
|
*/
|
|
void NavigationCell::ProjectPathOnCellWall(CELL_SIDE SideNumber, Line2D& MotionPath)const
|
|
{
|
|
// compute the normalized vector of the cell wall in question
|
|
EtVector2 WallNormal = m_Side[SideNumber].EndPointB() - m_Side[SideNumber].EndPointA();
|
|
EtVec2Normalize( &WallNormal, &WallNormal );
|
|
|
|
// determine the vector of our current movement
|
|
EtVector2 MotionVector = MotionPath.EndPointB() - MotionPath.EndPointA();
|
|
|
|
// compute dot product of our MotionVector and the normalized cell wall
|
|
// this gives us the magnatude of our motion along the wall
|
|
float DotResult = EtVec2Dot( &MotionVector, &WallNormal );
|
|
|
|
// our projected vector is then the normalized wall vector times our new found magnatude
|
|
MotionVector = (DotResult * WallNormal);
|
|
|
|
// redirect our motion path along the new reflected direction
|
|
MotionPath.SetEndPointB(MotionPath.EndPointA() + MotionVector);
|
|
|
|
//
|
|
// Make sure starting point of motion path is within the cell
|
|
//
|
|
EtVector2 NewPoint = MotionPath.EndPointA();
|
|
ForcePointToCellCollumn(NewPoint);
|
|
MotionPath.SetEndPointA(NewPoint);
|
|
|
|
//
|
|
// Make sure destination point does not intersect this wall again
|
|
//
|
|
NewPoint = MotionPath.EndPointB();
|
|
ForcePointToWallInterior(SideNumber, NewPoint);
|
|
MotionPath.SetEndPointB(NewPoint);
|
|
}
|
|
|
|
//: ForcePointToWallInterior
|
|
//----------------------------------------------------------------------------------------
|
|
//
|
|
// Force a 2D point to the interior side of the specified wall.
|
|
//
|
|
//-------------------------------------------------------------------------------------://
|
|
bool NavigationCell::ForcePointToWallInterior(CELL_SIDE SideNumber, EtVector2& TestPoint)const
|
|
{
|
|
float Distance = m_Side[SideNumber].SignedDistance(TestPoint);
|
|
float Epsilon = 0.001f;
|
|
|
|
if (Distance <= Epsilon)
|
|
{
|
|
if (Distance <= 0.0f)
|
|
{
|
|
Distance -= Epsilon;
|
|
}
|
|
|
|
Distance = (float)fabs(Distance);
|
|
Distance = (Epsilon>Distance ? Epsilon : Distance);
|
|
|
|
// this point needs adjustment
|
|
EtVector2 Normal = m_Side[SideNumber].Normal();
|
|
TestPoint += (Normal * Distance);
|
|
return (true);
|
|
}
|
|
return (false);
|
|
}
|
|
|
|
//: ForcePointToWallInterior
|
|
//----------------------------------------------------------------------------------------
|
|
//
|
|
// Force a 3D point to the interior side of the specified wall.
|
|
//
|
|
//-------------------------------------------------------------------------------------://
|
|
bool NavigationCell::ForcePointToWallInterior(CELL_SIDE SideNumber, EtVector3& TestPoint)const
|
|
{
|
|
EtVector2 TestPoint2D(TestPoint.x,TestPoint.z);
|
|
bool PointAltered = ForcePointToWallInterior(SideNumber, TestPoint2D);
|
|
|
|
if (PointAltered)
|
|
{
|
|
TestPoint.x = TestPoint2D.x;
|
|
TestPoint.z = TestPoint2D.y;
|
|
}
|
|
|
|
return (PointAltered);
|
|
}
|
|
|
|
//: ForcePointToCellCollumn
|
|
//----------------------------------------------------------------------------------------
|
|
//
|
|
// Force a 2D point to the interior cell by forcing it to the interior of each wall
|
|
//
|
|
//-------------------------------------------------------------------------------------://
|
|
|
|
bool NavigationCell::ForcePointToCellCollumn(EtVector2& TestPoint)const
|
|
{
|
|
//bool PointAltered = false;
|
|
|
|
// create a motion path from the center of the cell to our point
|
|
Line2D TestPath(EtVector2(m_CenterPoint.x, m_CenterPoint.z), TestPoint);
|
|
EtVector2 PointOfIntersection;
|
|
CELL_SIDE Side;
|
|
NavigationCell* NextCell;
|
|
|
|
PATH_RESULT result = ClassifyPathToCell(TestPath, &NextCell, Side, &PointOfIntersection);
|
|
// compare this path to the cell.
|
|
|
|
if (result == EXITING_CELL)
|
|
{
|
|
EtVector2 PathDirection(PointOfIntersection.x - m_CenterPoint.x, PointOfIntersection.y - m_CenterPoint.z);
|
|
|
|
PathDirection *= 0.9f;
|
|
|
|
TestPoint.x = m_CenterPoint.x + PathDirection.x;
|
|
TestPoint.y = m_CenterPoint.z + PathDirection.y;
|
|
return (true);
|
|
}
|
|
else if (result == NO_RELATIONSHIP)
|
|
{
|
|
TestPoint.x = m_CenterPoint.x;
|
|
TestPoint.y = m_CenterPoint.z;
|
|
return (true);
|
|
}
|
|
|
|
return (false);
|
|
}
|
|
|
|
//: ForcePointToCellCollumn
|
|
//----------------------------------------------------------------------------------------
|
|
//
|
|
// Force a 3D point to the interior cell by forcing it to the interior of each wall
|
|
//
|
|
//-------------------------------------------------------------------------------------://
|
|
bool NavigationCell::ForcePointToCellCollumn(EtVector3& TestPoint)const
|
|
{
|
|
EtVector2 TestPoint2D(TestPoint.x,TestPoint.z);
|
|
bool PointAltered = ForcePointToCellCollumn(TestPoint2D);
|
|
|
|
if (PointAltered)
|
|
{
|
|
TestPoint.x=TestPoint2D.x;
|
|
TestPoint.z=TestPoint2D.y;
|
|
}
|
|
return (PointAltered);
|
|
}
|
|
|
|
//: ProcessCell
|
|
//----------------------------------------------------------------------------------------
|
|
//
|
|
// Process this cells neighbors using A*
|
|
//
|
|
//-------------------------------------------------------------------------------------://
|
|
bool NavigationCell::ProcessCell(NavigationHeap* pHeap)
|
|
{
|
|
if (m_SessionID==pHeap->SessionID())
|
|
{
|
|
// once we have been processed, we are closed
|
|
m_Open = false;
|
|
|
|
// querry all our neigbors to see if they need to be added to the Open heap
|
|
for (int i=0;i<3;++i)
|
|
{
|
|
if (m_Link[i])
|
|
{
|
|
// abs(i-m_ArrivalWall) is a formula to determine which distance measurement to use.
|
|
// The Distance measurements between the wall midpoints of this cell
|
|
// are held in the order ABtoBC, BCtoCA and CAtoAB.
|
|
// We add this distance to our known m_ArrivalCost to compute
|
|
// the total cost to reach the next adjacent cell.
|
|
m_Link[i]->QueryForPath(pHeap, this, m_ArrivalCost+m_WallDistance[abs(i-m_ArrivalWall)]);
|
|
}
|
|
}
|
|
return(true);
|
|
}
|
|
return(false);
|
|
}
|
|
|
|
//: QueryForPath
|
|
//----------------------------------------------------------------------------------------
|
|
//
|
|
// Process this cell using the A* heuristic
|
|
//
|
|
//-------------------------------------------------------------------------------------://
|
|
bool NavigationCell::QueryForPath(NavigationHeap* pHeap, NavigationCell* Caller, float arrivalcost)
|
|
{
|
|
if (m_SessionID!=pHeap->SessionID())
|
|
{
|
|
// this is a new session, reset our internal data
|
|
m_SessionID = pHeap->SessionID();
|
|
|
|
if (Caller)
|
|
{
|
|
m_Open = true;
|
|
ComputeHeuristic(pHeap->Goal());
|
|
m_ArrivalCost = arrivalcost;
|
|
|
|
// remember the side this caller is entering from
|
|
if (Caller == m_Link[0])
|
|
{
|
|
m_ArrivalWall = 0;
|
|
}
|
|
else if (Caller == m_Link[1])
|
|
{
|
|
m_ArrivalWall = 1;
|
|
}
|
|
else if (Caller == m_Link[2])
|
|
{
|
|
m_ArrivalWall = 2;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// we are the cell that contains the starting location
|
|
// of the A* search.
|
|
m_Open = false;
|
|
m_ArrivalCost = 0;
|
|
m_Heuristic = 0;
|
|
m_ArrivalWall = 0;
|
|
}
|
|
|
|
// add this cell to the Open heap
|
|
pHeap->AddCell(this);
|
|
return(true);
|
|
}
|
|
else if (m_Open)
|
|
{
|
|
// m_Open means we are already in the Open Heap.
|
|
// If this new caller provides a better path, adjust our data
|
|
// Then tell the Heap to resort our position in the list.
|
|
if ((arrivalcost + m_Heuristic) < (m_ArrivalCost + m_Heuristic))
|
|
{
|
|
m_ArrivalCost = arrivalcost;
|
|
|
|
// remember the side this caller is entering from
|
|
if (Caller == m_Link[0])
|
|
{
|
|
m_ArrivalWall = 0;
|
|
}
|
|
else if (Caller == m_Link[1])
|
|
{
|
|
m_ArrivalWall = 1;
|
|
}
|
|
else if (Caller == m_Link[2])
|
|
{
|
|
m_ArrivalWall = 2;
|
|
}
|
|
|
|
// ask the heap to resort our position in the priority heap
|
|
pHeap->AdjustCell(this);
|
|
return(true);
|
|
}
|
|
}
|
|
// this cell is closed
|
|
return(false);
|
|
}
|
|
|
|
//: ComputeHeuristic
|
|
//----------------------------------------------------------------------------------------
|
|
//
|
|
// Compute the A* Heuristic for this cell given a Goal point
|
|
//
|
|
//-------------------------------------------------------------------------------------://
|
|
void NavigationCell::ComputeHeuristic(const EtVector3& Goal)
|
|
{
|
|
// our heuristic is the estimated distance (using the longest axis delta) between our
|
|
// cell center and the goal location
|
|
|
|
float XDelta = fabsf(Goal.x - m_CenterPoint.x);
|
|
float YDelta = fabsf(Goal.y - m_CenterPoint.y);
|
|
float ZDelta = fabsf(Goal.z - m_CenterPoint.z);
|
|
|
|
#ifdef _WIN32
|
|
m_Heuristic = __max(__max(XDelta,YDelta), ZDelta);
|
|
#else
|
|
m_Heuristic = max(max(XDelta,YDelta), ZDelta);
|
|
#endif
|
|
}
|
|
|
|
bool NavigationCell::FindLastCollision( EtVector3 &vStart, EtVector3 &vEnd, NavigationCell **ppLastCollisionCell, int &nLastCollisionWall )
|
|
{
|
|
int i;
|
|
EtVector2 vStart2D( vStart.x, vStart.z );
|
|
EtVector2 vEnd2D( vEnd.x, vEnd.z );
|
|
Line2D MovingSegment( vStart2D, vEnd2D );
|
|
NavigationCell *pNowCell = this;
|
|
NavigationCell *pPrevCell = this;
|
|
|
|
DWORD dwCount = 0;
|
|
while( 1 )
|
|
{
|
|
if( dwCount++ >= 100 )
|
|
{
|
|
nLastCollisionWall = -1;
|
|
return false;
|
|
}
|
|
|
|
bool bCollision = false;
|
|
for( i = 0; i < 3; i++ )
|
|
{
|
|
if( pNowCell->Link( i ) == pPrevCell )
|
|
{
|
|
continue;
|
|
}
|
|
|
|
Line2D::POINT_CLASSIFICATION StartSideResult1 = pNowCell->m_Side[ i ].ClassifyPoint( vStart2D );
|
|
Line2D::POINT_CLASSIFICATION EndSideResult1 = pNowCell->m_Side[ i ].ClassifyPoint( vEnd2D );
|
|
|
|
Line2D::POINT_CLASSIFICATION StartSideResult2 = MovingSegment.ClassifyPoint( pNowCell->m_Side[ i ].EndPointA() );
|
|
Line2D::POINT_CLASSIFICATION EndSideResult2 = MovingSegment.ClassifyPoint( pNowCell->m_Side[ i ].EndPointB() );
|
|
if( ( StartSideResult1 != EndSideResult1 ) && ( StartSideResult2 != EndSideResult2 ) )
|
|
{
|
|
pPrevCell = pNowCell;
|
|
*ppLastCollisionCell = pNowCell;
|
|
nLastCollisionWall = i;
|
|
if( pNowCell->Link( i ) == NULL )
|
|
{
|
|
return true;
|
|
}
|
|
|
|
pNowCell = pNowCell->Link( i );
|
|
bCollision = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if( !bCollision )
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
float NavigationCell::Length( int nSide )
|
|
{
|
|
EtVector3 vSide = m_Vertex[ ( nSide + 1 ) % 3 ] - m_Vertex[ nSide ];
|
|
return EtVec3Length( &vSide );
|
|
}
|
|
|
|
bool NavigationCell::SaveToStream( CStream* pStream )
|
|
{
|
|
for( int i = 0; i < 3; i++ )
|
|
{
|
|
pStream->Write( &(m_Vertex[i].x), sizeof(float) );
|
|
pStream->Write( &(m_Vertex[i].y), sizeof(float) );
|
|
pStream->Write( &(m_Vertex[i].z), sizeof(float) );
|
|
}
|
|
|
|
int iAttribute = (int)m_bEAttribute;
|
|
pStream->Write( &iAttribute, sizeof(int) );
|
|
pStream->Write( &m_Type, sizeof( CELL_TYPE ) );
|
|
pStream->Write( m_nWallAttribute, sizeof( int ) * 3 );
|
|
pStream->Write( &m_nNavMeshType, sizeof( int ) );
|
|
|
|
return true;
|
|
}
|
|
|
|
bool NavigationCell::LoadFromStream( CStream* pStream, int nVersion, EtMatrix *pWorldMat )
|
|
{
|
|
memset( m_Link, 0, sizeof(m_Link) );
|
|
|
|
for( int i = 0; i < 3; i++ )
|
|
{
|
|
pStream->Read( &(m_Vertex[i].x), sizeof(float) );
|
|
pStream->Read( &(m_Vertex[i].y), sizeof(float) );
|
|
pStream->Read( &(m_Vertex[i].z), sizeof(float) );
|
|
if( pWorldMat )
|
|
{
|
|
EtVec3TransformCoord( m_Vertex + i, m_Vertex + i, pWorldMat );
|
|
}
|
|
}
|
|
|
|
int iAttribute;
|
|
pStream->Read( &iAttribute, sizeof(int) );
|
|
m_bEAttribute = ( iAttribute != 0 );
|
|
|
|
if( nVersion >= 10 )
|
|
{
|
|
pStream->Read( &m_Type, sizeof( CELL_TYPE ) );
|
|
}
|
|
if( nVersion >= 11 )
|
|
{
|
|
pStream->Read( m_nWallAttribute, sizeof( int ) * 3 );
|
|
}
|
|
if( nVersion >= 12 )
|
|
{
|
|
pStream->Read( &m_nNavMeshType, sizeof( int ) );
|
|
}
|
|
|
|
ComputeCellData();
|
|
return true;
|
|
}
|
|
|
|
|
|
//****************************************************************************************
|
|
// end of file ( NavigationCell.cpp )
|
|
|