//////////////////////////////////////////////////////////////////////////////
//
// Tools_Mode_Waypointing.cpp
//
// Copyright (c) 2005 Rebellion. All rights reserved.
//
// Nick Davis
//
//////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////
//
// includes
//
//////////////////////////////////////////////////////////////////////////////

// core
#include "Core/Asura.h"

// header
#include "Waypointing/Tools_Mode_Waypointing.h"

// asura
#ifdef ASURA_USE_BOTANICALS
#include "Botanicals/Tools_Botanicals_Object.h"
#include "Botanicals/Asura_Botanicals_InstanceManager.h"
#endif
#include "Camera/Asura_Camera.h"
#include "Core/Asura_HashedList.h"
#include "Core/Asura_LinkList.h"
#include "Core/Asura_Timers.h"
#include "Core/Asura_UserInput.h"
#include "Core/Asura_Windows.h"
#include "Core/Memory/Asura_Mem.h"
#include "Core/Memory/Asura_ResourceAllocator.h"
#include "Debugging/Asura_Profiling_System.h"
#include "Debugging/Asura_Debug_Timers.h"
#include "Entity/PhysicalObject/Asura_ServerEntity_PhysicalObject.h"
#include "Entity/Triggers/Asura_ServerEntity_WaypointBlockerVolume.h"
#include "Entity/Asura_Entity.h"
#include "Entity/Asura_Entity_Classification.h"
#include "Interface/Asura_Cursor.h"
#include "Interface/Asura_Dialog.h"
#include "Interface/Asura_Menubar_System.h"
#include "Interface/Asura_Menubar.h"
#include "Interface/Asura_Overlay.h"
#include "Interface/Asura_Overlay_System.h"
#include "Mesh/Asura_CollisionMesh.h"
#include "Mesh/Asura_Mesh.h"
#include "Movement/Tools_Movement.h"
#include "Physics/Asura_Physics.h"
#include "Physics/Asura_Physics_CCD.h"
#include "Physics/Asura_Physics_MovementQuery.h"
#include "Physics/Asura_Physics_Object.h"
#include "Physics/Asura_Physics_RigidBody.h"
#include "Rendering/Asura_Module.h"
#include "Rendering/Asura_Render.h"
#include "Rendering/Asura_Text_Output.h"
#include "Rendering/Asura_Viewport.h"
#include "Shapes/Asura_Shape_System.h"

#include "Rendering/Asura_Direct3D.h"

// tools
#include "Tools_Overlays.h"
#include "Core/Asura_AABB_Tree.h"
#include "Core/Asura_Timers.h"
#include "Entity/Tools_Entity_Data.h"
#include "Entity/Tools_Entity_Object.h"
#include "Flags/Tools_Mode_Flags.h"
#include "Load&Save/Tools_BatchProcessing.h"
#include "Load&Save/Tools_ModeDataFile.h"
#include "Load&Save/Tools_SaveAsura.h"
#include "MeshEdit/Tools_Mode_MeshEdit.h"
#include "Misc/Tools_Adjacency.h"
#include "Misc/Tools_Help.h"
#include "Misc/Tools_Large_AABB_Tree.h"
#include "Misc/Tools_LOS.h"
#include "Misc/Tools_MessageLog.h"
#include "Misc/Tools_Polygon_Selection.h"
#include "Misc/Tools_Render.h"
#include "Misc/Tools_Timed_Messages.h"
#include "Objects/Tools_Object_System.h"
#include "Rendering/Tools_RenderCollisionMesh.h"
#include "Streaming/Tools_Streaming_System.h"
#include "Texturing/Tools_Texturing_Find.h"
#ifdef ASURA_USE_STREAMING
#include "Waypointing/Tools_InterZoneWaypointObject.h"
#endif
#include "Waypointing/Tools_Waypoint_Analysis.h"
#include "Waypointing/Tools_Waypoint_Editing.h"
#include "Waypointing/Tools_Waypoint_Object.h"
#include "Waypointing/Tools_Waypoint_Selection.h"


#ifdef ASURA_USE_CONTAINER_SYSTEM
#include "Entity/Container/Tools_Entity_Container.h"
#include "Entity/Container/Asura_ServerEntity_Container.h"
#endif //#ifdef ASURA_USE_CONTAINER_SYSTEM

//////////////////////////////////////////////////////////////////////////////
// Defines
//////////////////////////////////////////////////////////////////////////////

#if defined(_DEBUG)
#define TOOLS_ALLOW_WAYPOINT_DEBUGGING	1
#endif	// defined(_DEBUG)

//////////////////////////////////////////////////////////////////////////////
//
// Helper Classes
//
//////////////////////////////////////////////////////////////////////////////

class Tools_MessageLogAction_GotoAndSelectBadlyFlaggedPolys : public Tools_MessageLogAction
{
public:
    Tools_MessageLogAction_GotoAndSelectBadlyFlaggedPolys( const int iModuleIndex ) : m_iModuleIndex( iModuleIndex ) {}
    virtual void Execute();

private:
    int m_iModuleIndex;
};

void Tools_MessageLogAction_GotoAndSelectBadlyFlaggedPolys::Execute()
{
    const Asura_Mesh* pxMesh = Asura_Module_System::GetMeshForModule( m_iModuleIndex );
    if ( pxMesh->AreNormalsValid() )
    {
        for ( int iPoly = 0; iPoly < pxMesh->NumberOfPolygons; ++iPoly )
        {
            const u_int uPolyFlags = static_cast< u_int >( pxMesh->PolygonFlagDataPtr ? pxMesh->PolygonFlagDataPtr[ iPoly ].Flags : 0 );
            const bool bGotWaypointingFlag = ( uPolyFlags & AsuraPolygonFlag_Floor ) ||
                                             ( uPolyFlags & AsuraPolygonFlag_Step ) ||
                                             ( uPolyFlags & AsuraPolygonFlag_DenseWaypoints );
            if ( bGotWaypointingFlag && ( uPolyFlags & AsuraPolygonFlag_NoCollisions ) )
            {
                Tools_Polygon_Selection::SelectPolygon( m_iModuleIndex, iPoly );
            }
        }
    }

    const Asura_Matrix_3x3* pxCameraOrientation = Asura_Camera::GetOrientation();
    Asura_Vector_3 xPosition = Asura_Module_System::GetCentreOfModule( m_iModuleIndex );
    xPosition.x -= pxCameraOrientation->e13 * 2.0f;
    xPosition.y -= pxCameraOrientation->e23 * 2.0f;
    xPosition.z -= pxCameraOrientation->e33 * 2.0f;
    Tools_Movement::TeleportCameraToPosition( &xPosition, pxCameraOrientation );
}

//////////////////////////////////////////////////////////////////////////////
//
// help strings
//
//////////////////////////////////////////////////////////////////////////////

static const char* WaypointingModeHelpStrings [] =
{
    "F :           Apply floor flag",
    "Shift+F :     Remove floor flag",
    "< :           Undo",
    "> :           Redo",
    "",
    "F3 :          Show/Hide waypoint links",
    "F5 :          Show/Hide waypoints",
    NULL
};

//////////////////////////////////////////////////////////////////////////////
//
// Tools_Mode_Waypointing
//
//////////////////////////////////////////////////////////////////////////////

// default values for the waypoint/link generation parameters
// these should guarantee a reasonable set of default navigation data
Tools_Mode_Waypointing*  Tools_Mode_Waypointing::s_pxThis                            = NULL;
Tools_Waypoint_Analysis* Tools_Mode_Waypointing::s_pxWaypointAnalysis                = NULL;
Tools_Waypoint_Editing*  Tools_Mode_Waypointing::s_pxWaypointEditing                 = NULL;
bool                     Tools_Mode_Waypointing::s_bIsBatchWaypointing               = false;

const char* Tools_Mode_Waypointing::s_aszTabNames[ TAB_NUM_TAB_NAMES ] =
{
    "Settings",
    "Rendering",
    "Selection",
    "Analysis",
    "Editing",
    "Testing"
};
Asura_Hash_ID           Tools_Mode_Waypointing::s_iTabDialogueControlID             = Asura_GetHashID( "Waypointing_Tab_Dialogue" );
bool                    Tools_Mode_Waypointing::s_bShowFloorFlag                    = false;
bool                    Tools_Mode_Waypointing::s_bShowWaypoints                    = false;
bool                    Tools_Mode_Waypointing::s_bShowWaypointNormals              = false;
bool                    Tools_Mode_Waypointing::s_bShowLinks                        = false;
bool                    Tools_Mode_Waypointing::s_bShowOnlyInZone                   = false;
bool                    Tools_Mode_Waypointing::s_bShowOnlyInRange                  = false;
int                     Tools_Mode_Waypointing::s_iZoneIDToRender                   = 0;
float                   Tools_Mode_Waypointing::s_fRangeToRender                    = 100.0f;
bool                    Tools_Mode_Waypointing::s_bShowCoverpoints                  = false;
bool                    Tools_Mode_Waypointing::s_bShowCoverpointExits              = false;
bool                    Tools_Mode_Waypointing::s_bShowCoverpointFlags              = false;
bool                    Tools_Mode_Waypointing::s_bShowCoverpointDistances          = false;
bool                    Tools_Mode_Waypointing::s_bShowCoverpointGradientColour     = false;
float                   Tools_Mode_Waypointing::s_fShowCoverpointDistanceScale      = 50.0f;
bool                    Tools_Mode_Waypointing::s_bShowTestWaypoints                = false;
bool                    Tools_Mode_Waypointing::s_bShowTestLinks                    = false;
bool                    Tools_Mode_Waypointing::s_bShowIntermediateWaypoints        = false;
bool                    Tools_Mode_Waypointing::s_bShowIntermediateLinks            = false;
bool					Tools_Mode_Waypointing::s_bShowUserDefinedWaypoints			= true;
bool                    Tools_Mode_Waypointing::s_bShowWaypointBlockerVolumes       = false;
int                     Tools_Mode_Waypointing::s_iNumIntermediateWaypointsInBatch  = 1000;
int                     Tools_Mode_Waypointing::s_iIntermediateWaypointBatchIndex   = 0;
bool                    Tools_Mode_Waypointing::s_bShowWaypointInfoUnderCursor      = false;
bool                    Tools_Mode_Waypointing::s_bWaypointingOverlayOpen           = true;
Asura_Vector_2          Tools_Mode_Waypointing::s_xWaypointingOverlayPosition       = Asura_Vector_2( 3.0f, 28.0f );
Asura_Handle            Tools_Mode_Waypointing::s_iMenuHandle                       = ASURA_INVALID_HANDLE;
bool                    Tools_Mode_Waypointing::s_bGenerateTestWaypointsOnly        = false;
int                     Tools_Mode_Waypointing::s_iNumZones                         = 0;
unsigned short          Tools_Mode_Waypointing::s_usNumWaypointsInZone[ ASURA_NAVVERTEX_MAX_ZONE_ID + 1 ];
float                   Tools_Mode_Waypointing::s_fCoverpointReferencesPerWaypoint  = 0.0f;

Tools_Nav_Vertex*       Tools_Mode_Waypointing::s_apxWaypointStorage[ NUM_WAYPOINT_BINS ];
int                     Tools_Mode_Waypointing::s_aiNumWaypoints[ NUM_WAYPOINT_BINS ];
Tools_Nav_Vertex*       Tools_Mode_Waypointing::s_pxWaypointStorage   = NULL;
int                     Tools_Mode_Waypointing::s_iNumWaypoints       = 0;
Tools_Nav_Coverpoint*   Tools_Mode_Waypointing::s_pxCoverpointStorage = NULL;
int                     Tools_Mode_Waypointing::s_iNumCoverpoints     = 0;

bool                    Tools_Mode_Waypointing::s_bHashStorageAllocated = false;
int**                   Tools_Mode_Waypointing::s_ppiHashTable          = NULL;
int*                    Tools_Mode_Waypointing::s_piNumHashEntries      = NULL;

bool*                   Tools_Mode_Waypointing::s_bVisibleModules = NULL;
bool					Tools_Mode_Waypointing::s_bBBInNonExteriorModule = false;

Tools_Nav_Vertex*       Tools_Mode_Waypointing::s_pxTestWaypointStorage = NULL;
int                     Tools_Mode_Waypointing::s_iNumTestWaypoints     = 0;

Asura_Vector_3*         Tools_Mode_Waypointing::s_pxPhysicsPositions   = NULL;
u_int                   Tools_Mode_Waypointing::s_uNumPhysicsPositions = 0;

bool                                          Tools_Mode_Waypointing::s_bPhysicsDataInitialised = false;
Asura_List< Asura_Physics_RigidBodyInstance > Tools_Mode_Waypointing::s_xRigidBodyIgnoreList;
Asura_List< Asura_Physics_RigidBodyInstance > Tools_Mode_Waypointing::s_xRigidBodyNonIgnoreList;

float                   Tools_Mode_Waypointing::s_fMinSubdivideSizeSq              = 1.0f;
float                   Tools_Mode_Waypointing::s_fMinDenseSubdivideSizeSq         = 0.5625f;
int                     Tools_Mode_Waypointing::s_iMaxSubdivideRecursionLevel      = 4;
float                   Tools_Mode_Waypointing::s_fMinWaypointClearanceRadius      = 0.3f;
float                   Tools_Mode_Waypointing::s_fMinWaypointRadius               = 0.5f;
float                   Tools_Mode_Waypointing::s_fMaxWaypointRadius               = 10.0f;
float                   Tools_Mode_Waypointing::s_fMinDenseWaypointRadius          = 0.5f;
float                   Tools_Mode_Waypointing::s_fMaxDenseWaypointRadius          = 5.0f;
float                   Tools_Mode_Waypointing::s_fMinLoneWaypointRadius           = 0.0f;
bool                    Tools_Mode_Waypointing::s_bCliffStopWaypoints              = false;
float                   Tools_Mode_Waypointing::s_fWallBlockHeight                 = -0.005f;
float                   Tools_Mode_Waypointing::s_fWaypointOverlapFraction         = 0.01f;
void                    ( *Tools_Mode_Waypointing::s_pfnWaypointCalcThreadFn )( const void* pParams ) = Tools_Mode_Waypointing::CalcWPVolumesInBin_ThreadFunction;

Asura_Physics_Object::COLLISION_PROXY Tools_Mode_Waypointing::s_eCollisionProxy	   = Asura_Physics_Object::COLLISION_PROXY_OBB;
Asura_Physics_Object* Tools_Mode_Waypointing::s_pxStandardPhysicsObject            = NULL;
Asura_Collision_Box   Tools_Mode_Waypointing::s_xTestCollisionBox( -0.3f, 0.3f, -2.0f, 0.0f, -0.3f, 0.3f );
bool                  Tools_Mode_Waypointing::s_bCrouchEnabled                     = false;
Asura_Physics_Object* Tools_Mode_Waypointing::s_pxCrouchPhysicsObject              = NULL;
Asura_Collision_Box   Tools_Mode_Waypointing::s_xCrouchCollisionBox( -0.3f, 0.3f, -2.0f, 0.0f, -0.3f, 0.3f );
bool                  Tools_Mode_Waypointing::s_bProneEnabled                      = false;
Asura_Physics_Object* Tools_Mode_Waypointing::s_pxPronePhysicsObject               = NULL;
Asura_Collision_Box	  Tools_Mode_Waypointing::s_xProneCollisionBox( -0.3f, 0.3f, -2.0f, 0.0f, -0.3f, 0.3f );
float                 Tools_Mode_Waypointing::s_fLinkTraversalSpeed                = 10.0f;
bool                  Tools_Mode_Waypointing::s_bFallDirectlyDownwards             = true;
float                 Tools_Mode_Waypointing::s_fPlacedObjectMovementScale         = 0.1f;
float                 Tools_Mode_Waypointing::s_fPlacedObjectAllowedMovement       = 1.0f;
float                 Tools_Mode_Waypointing::s_fMinLinkLength                     = 0.5f;
float                 Tools_Mode_Waypointing::s_fMaxLinkLength                     = 21.0f;
float                 Tools_Mode_Waypointing::s_fLinkOverlapFraction               = 0.33f;
float                 Tools_Mode_Waypointing::s_fFallLinkDistance                  = 0.75f;
float                 Tools_Mode_Waypointing::s_fLongFallLinkDistance              = 1.5f;
float                 Tools_Mode_Waypointing::s_fMaxLinkFallDistance               = 4.0f;
float                 Tools_Mode_Waypointing::s_fMaxJumpImpulse                    = 0.0f;
float                 Tools_Mode_Waypointing::s_fCanReachDistance                  = 0.2f;
Asura_Vector_3        Tools_Mode_Waypointing::s_xMovedObjectAllowedOffset( 0.2f, -0.5f, 0.2f );
bool                  Tools_Mode_Waypointing::s_bDontCullLinkedToWaypoints         = false;
bool                  Tools_Mode_Waypointing::s_bShouldCullOneWayLinksWithNoReturn = true;

bool                    Tools_Mode_Waypointing::s_bShouldGenCoverpoints        = true;
float					Tools_Mode_Waypointing::s_fMinCoverpointRadius		   = 0.0f;
float                   Tools_Mode_Waypointing::s_fMaxCoverpointRadius         = 0.75f;
bool                    Tools_Mode_Waypointing::s_bShouldCalcZoningData        = true;
float					Tools_Mode_Waypointing::s_fMaxCoverPointDistanceSq	   = 75.0f * 75.0f;
int						Tools_Mode_Waypointing::s_iMaxCoverpointsPerWaypoint   = 2000;
float					Tools_Mode_Waypointing::s_fWaypointToCoverpointMaxDist = 0.0f;
bool                    Tools_Mode_Waypointing::s_bCalculateCoverpointExits    = true;

bool					Tools_Mode_Waypointing::s_bGravityFollowsSurfaceNormals					= false;
float					Tools_Mode_Waypointing::s_fMaxDotProductBetweenWaypointPolygonNormals	= -1.000f;	// 180 degrees difference - should never reject.
float					Tools_Mode_Waypointing::s_fMaxDotProductForLinkBetweenWaypointNormals	= -1.000f;	// 180 degrees difference - should never reject.

bool					Tools_Mode_Waypointing::s_bDataInNavSystem = false;

int						Tools_Mode_Waypointing::s_iAWCurrentModuleIndex		   = -1;
Asura_Vector_3			Tools_Mode_Waypointing::s_xAWCurrentNormal			   = Asura_Maths::ZeroVector3;
int						Tools_Mode_Waypointing::s_iAWCurrentWPFlags			   = 0;
float					Tools_Mode_Waypointing::s_fAWCurrentMinSubdivideSizeSq = s_fMinSubdivideSizeSq;
bool					Tools_Mode_Waypointing::s_bAWDoubleSidedPoly		   = false;

int                     Tools_Mode_Waypointing::s_iBinTestIndex = 0;
u_int                   Tools_Mode_Waypointing::s_uTestIndex    = 0;

const u_short			Tools_Mode_Waypointing::s_usWhiteZoneID = 0xffff;
const u_short			Tools_Mode_Waypointing::s_usGrayZoneID  = 0xfffe;
const u_short			Tools_Mode_Waypointing::s_usBlackZoneID = 0xfffd;

bool					Tools_Mode_Waypointing::s_bToolsEntitiesCreated = false;
bool					Tools_Mode_Waypointing::s_bStaticObjectsVisible = false;
bool					Tools_Mode_Waypointing::s_bShowStaticObjects = false;
bool					Tools_Mode_Waypointing::s_bWaypointingSettingsModified = false;

const char* Tools_Mode_Waypointing::s_pszWaypointingModeDataFilename = "WaypointingMode.dat";

Asura_Tabs_Dialog*		Tools_Mode_Waypointing::s_pxWaypointingTabs	= NULL;

int 					Tools_Mode_Waypointing::s_iInitialRenderMode;

int						Tools_Mode_Waypointing::s_iGenerateIncrementalWaypointPhase	= -1;

bool					Tools_Mode_Waypointing::s_bShowMessageLogOnCompletion = false;

//////////////////////////////////////////////////////////////////////////////
//
// 
//
//////////////////////////////////////////////////////////////////////////////

void Tools_Mode_Waypointing::Update()
{
    Tools_Mode_Flags::UpdateExMode( !Tools_Object_System::FindObjectUnderCursor(),
		( Tools_Waypoint_Selection::IsEnabled() ? PolygonSelectionUpdate_NoSelection : PolygonSelectionUpdate_Standard ),
		( Tools_Waypoint_Selection::IsEnabled() ? VertexSelectionUpdate_NoSelection : VertexSelectionUpdate_Standard ) );

    Tools_Waypoint_Selection::Update();

    HandleKeyboard();
	UpdateStaticObjects();

    s_pxWaypointAnalysis->Update();

	// have waypointing settings been modified?
	if ( s_bWaypointingSettingsModified )
	{
		Tools_SaveAsura::SetWaypointingModified( true );
		s_bWaypointingSettingsModified = false;
	}
}

void Tools_Mode_Waypointing::RenderWorld_PostToneMap()
{
    if ( s_bShowFloorFlag )
	{
		Tools_Mode_Flags::HighlightPolyFlags();
	}
    if ( s_bShowWaypoints )
    {
        RenderWaypoints();
    }
    if ( s_bShowWaypointNormals )
    {
        RenderWaypointNormals();
    }
    if ( s_bShowLinks )
    {
        RenderLinks();
    }
    if ( s_bShowCoverpoints )
    {
        RenderCoverpoints();
    }
    if ( s_bShowIntermediateWaypoints )
    {
        RenderIntermediateWaypoints();
    }
    if ( s_bShowIntermediateLinks )
    {
        RenderIntermediateLinks();
    }
    if ( s_bShowTestWaypoints )
    {
        RenderTestWaypoints();
    }
    if ( s_bShowTestLinks )
    {
        RenderTestLinks();
    }
	if ( s_bShowWaypointBlockerVolumes )
	{
		RenderWaypointBlockerVolumes();
	}

    Tools_Polygon_Selection::Render();
    Tools_Waypoint_Selection::Render();
    s_pxWaypointAnalysis->RenderWorld();
}

void Tools_Mode_Waypointing::RenderUI()
{
    Tools_Polygon_Selection::RenderUI();

    float fPrintX = 1.0f;
    float fPrintY = 7.0f;

    // Write a brief outline of the waypoint data stats.
    Asura_Text_Output::Print( fPrintX, fPrintY, 0xffffffff, 0, "Number of waypoints: %d", Asura_AI_Navigation_System::GetNumberOfWaypoints() );
    fPrintY += 1.0f;
    Asura_Text_Output::Print( fPrintX, fPrintY, 0xffffffff, 0, "Number of links: %d", Asura_AI_Navigation_System::GetNumberOfConnections() );
    fPrintY += 1.0f;
    Asura_Text_Output::Print(
        fPrintX, fPrintY, 0xffffffff, 0,
        "Number of coverpoints: %d (%.02f references per waypoint)",
        Asura_AI_Navigation_System::GetNumberOfCoverpoints(),
        s_fCoverpointReferencesPerWaypoint );
    fPrintY += 1.0f;
    Asura_Text_Output::Print( fPrintX, fPrintY, 0xffffffff, 0, "Number of waypoint zones: %d", s_iNumZones );
    fPrintY += 1.0f;
    if ( s_bShowOnlyInZone )
    {
        Asura_Text_Output::Print( fPrintX, fPrintY, 0xffffffff, 0, "Number of waypoints in current zone: %u", s_usNumWaypointsInZone[ s_iZoneIDToRender ] );
        fPrintY += 1.0f;
    }
    if ( Tools_Waypoint_Selection::IsEnabled() )
    {
        Asura_Text_Output::Print( fPrintX, fPrintY, 0xffffffff, 0, "Number of waypoints currently selected: %d", Tools_Waypoint_Selection::GetNumSelectedWaypoints() );
        fPrintY += 1.0f;
    }

    // Write help text for links, if necessary.
    if ( s_bShowLinks )
    {
        fPrintY += 1.0f;
        RenderLinksHelpText( fPrintX, fPrintY );
    }

    // Write help text for flags and flag info, if necessary.
    if ( s_bShowFloorFlag )
    {
		fPrintY += 1.0f;
        Tools_Mode_Flags::RenderPolyFlagText(fPrintX,fPrintY); 
    }

	s_pxWaypointAnalysis->RenderText( fPrintX, fPrintY );

    if ( Tools_Waypoint_Selection::IsEnabled() )
    {
        if ( Tools_Waypoint_Selection::GetHighlitWaypointIndex() != -1 )
        {
            fPrintY += 1.0f;
            RenderWaypointInfo( Tools_Waypoint_Selection::GetHighlitWaypointIndex(), fPrintX, fPrintY );
        }
    }

    if ( s_bShowWaypointInfoUnderCursor )
    {
        fPrintY += 1.0f;
        RenderIntermediateWaypointInfo( fPrintX, fPrintY );
    }
}

bool Tools_Mode_Waypointing::IsModified()
{
	return ( Tools_SaveAsura::IsWaypointingModified() ||
             Tools_SaveAsura::IsMainFileModified()    ||
             Tools_SaveAsura::IsEntityModified() );
}

void Tools_Mode_Waypointing::QuickSave()
{
    Tools_SaveAsura::QuickSaveFile();
	Tools_SaveAsura::SaveWaypointing();

    // entities may need saving if blocked waypoints for doors have been changed
    if ( Tools_SaveAsura::IsEntityModified() )
    {
        Tools_SaveAsura::SaveEntities();
    }
}

//////////////////////////////////////////////////////////////////////////////
//
// 
//
//////////////////////////////////////////////////////////////////////////////

void Tools_Mode_Waypointing::SetTestCollisionBox( const Asura_Collision_Box& xCollisionBox )
{
    s_xTestCollisionBox.ChangeBB( xCollisionBox.GetBoundingBox() );
}

void Tools_Mode_Waypointing::SetTestCollisionBox( const Asura_Bounding_Box& xBoundingBox )
{
    s_xTestCollisionBox.ChangeBB( xBoundingBox );
}

void Tools_Mode_Waypointing::SetCrouchCollisionBox( const Asura_Collision_Box& xCollisionBox )
{
    s_xCrouchCollisionBox.ChangeBB( xCollisionBox.GetBoundingBox() );
}

void Tools_Mode_Waypointing::SetCrouchCollisionBox( const Asura_Bounding_Box& xBoundingBox )
{
    s_xCrouchCollisionBox.ChangeBB( xBoundingBox );
}

void Tools_Mode_Waypointing::SetProneCollisionBox( const Asura_Collision_Box& xCollisionBox )
{
    s_xProneCollisionBox.ChangeBB( xCollisionBox.GetBoundingBox() );
}

void Tools_Mode_Waypointing::SetProneCollisionBox( const Asura_Bounding_Box& xBoundingBox )
{
    s_xProneCollisionBox.ChangeBB( xBoundingBox );
}

bool Tools_Mode_Waypointing::PlaceObjectAt( const Asura_Vector_3& xPosition, const Asura_Vector_3& xMovementVector, const float fAllowedOffsetSq,
                                            const Asura_Collision_Object& xCollisionObject, const u_short usFlagsOfPolysToIgnore,
                                            const Asura_Matrix_3x3& xOrientMatrix, Asura_Vector_3& xFinalPosition,
											Asura_Physics_Object::COLLISION_PROXY eCollisionProxy/* = Asura_Physics_Object::COLLISION_PROXY_OBB*/ )
{
    // Always add one increment on, as otherwise the box can get stuck when trying to be MovedToDestination.
    // PHYSICSTODO - This compensates for the condition where we have a box starting exactly on a poly and moving parallel to it, which can lead to it getting stuck.
	Asura_Assert(eCollisionProxy != Asura_Physics_Object::COLLISION_PROXY_POLYGONAL, "Can't use a polygonal object for waypointing. Using an OBB.");
    Asura_Vector_3 xTestPosition = xPosition + xMovementVector;

    bool bMovementLeft = true;
    bool bPlacedObject = false;
    do 
    {
		if ((eCollisionProxy == Asura_Physics_Object::COLLISION_PROXY_SPHERESTACK) || (eCollisionProxy == Asura_Physics_Object::COLLISION_PROXY_SPHERE))
		{
			bPlacedObject = Asura_Physics_System::CanPlaceSphereStackAt(xCollisionObject.GetBoundingBox(), xTestPosition, usFlagsOfPolysToIgnore);
		}
		else
		{
			bPlacedObject = Asura_Physics_System::CanPlaceObjectAt( xTestPosition, xOrientMatrix, xCollisionObject, usFlagsOfPolysToIgnore );
		}

        // If we haven't placed the object then move the test position, if allowed.
        if ( !bPlacedObject )
        {
            xTestPosition += xMovementVector;
            bMovementLeft = ( ( xTestPosition - xPosition ).MagnitudeSquared() < fAllowedOffsetSq );
        }
    } while ( !bPlacedObject && bMovementLeft );

    xFinalPosition = xTestPosition;

    return bPlacedObject;
}

bool Tools_Mode_Waypointing::GetSurfaceNormalAtPosition( const Asura_Vector_3& xPosition, Asura_Vector_3& xSurfaceNormal, u_int uLOSFlags /*= AsuraPolygonFlag_NoCollisions*/ )
{
    const Asura_LOS_InputData xInputData( xPosition + Asura_Vector_3( 0.0f, -0.5f, 0.0f ),
										  Asura_Vector_3( 0.0f, 1.0f, 0.0f ),
										  ASURA_GUID_UNREGISTERED,
										  1.0f,
										  false,
										  uLOSFlags );
    const Asura_LOS_OutputData* pxOutputData = Asura_LOS::TestAgainstEnvironment( &xInputData );
    xSurfaceNormal = pxOutputData->m_xNormal;
    return ( pxOutputData->m_bHitEnvironment );
}

bool Tools_Mode_Waypointing::GetSurfaceOrientAtPosition( const Asura_Vector_3& xPosition, Asura_Matrix_3x3& xOrientMatrix )
{
    // Use the surface normal as the y orientation.
    Asura_Vector_3 xSurfaceNormal;
    if ( !GetSurfaceNormalAtPosition( xPosition, xSurfaceNormal ) )
    {
        return false;
    }
    xSurfaceNormal *= -1.0f;
    // Fill in the orientation matrix.
    Asura_Vector_3 xRight  = Asura_Vector_3( 1.0f, 0.0f, 0.0f );    // maybe left - doesn't matter
    Asura_Vector_3 xFoward = xRight ^ xSurfaceNormal;
    Asura_Mem::Copy( xOrientMatrix.Element[ 0 ], &xRight,         sizeof( Asura_Vector_3 ) );
    Asura_Mem::Copy( xOrientMatrix.Element[ 1 ], &xSurfaceNormal, sizeof( Asura_Vector_3 ) );
    Asura_Mem::Copy( xOrientMatrix.Element[ 2 ], &xFoward,        sizeof( Asura_Vector_3 ) );
    return true;
}

bool Tools_Mode_Waypointing::MoveToDestination( const Asura_Vector_3& xStart, const Asura_Vector_3& xFinish, const Asura_Vector_3& xAllowedOffsetVector,
                                                Asura_Physics_Object* pxPhysicsObject, float& fMaxFallDistance,
                                                Asura_Vector_3* pxPositionArray /*= NULL*/, u_int* puPositionCount /*= NULL*/, const u_int uMaxPositions /*= 0*/ )
{
	const Asura_Matrix_3x3&	xOrientation	= pxPhysicsObject->GetOrientation();

	const Asura_Vector_3	xNormal(-xOrientation.e12, -xOrientation.e22, -xOrientation.e32);
	Asura_Bounding_Box		xDestinationBB;

    // We give ourselves an absolute max loop count of 2500, in case no other failure detection works.
    // This allows us 50 seconds of movement which should be enough to travel any realistic link.
    const u_int uMaxLoopCount = 2500;

	// Generate a bounding-box around the destination position...
	{
		Asura_Bounding_Box	xTempBB;

		xTempBB.Zero();
		xTempBB.Set(-xAllowedOffsetVector);
		xTempBB.Set(xAllowedOffsetVector);

		xDestinationBB.FromOBB(xTempBB, xOrientation);
		xDestinationBB.Displace(xFinish);
	}

    // In order to detect route failure we ensure that we are never staying still, or travelling away from our destination, for more than 10 physics frames.

    fMaxFallDistance = 0.0f;
    pxPhysicsObject->TeleportTo( xStart );
    pxPhysicsObject->SetLinVelocity( Asura_Maths::ZeroVector3 );
    if ( pxPositionArray && puPositionCount )
    {
        *pxPositionArray++ = pxPhysicsObject->GetPosition();
        *puPositionCount   = 1;
    }

    Asura_Vector_3 xPreFallPosition      = pxPhysicsObject->GetPosition();
    Asura_Vector_3 xPreviousPosition     = pxPhysicsObject->GetPosition();
    float          fPreviousDistanceSq   = ( xFinish - pxPhysicsObject->GetPosition() ).MagnitudeSquared();
    u_int          uNotMovingTowardCount = 0;
    bool           bAtDestination        = false;
    u_int          uLoopCount            = 0;
    do
    {
		Asura_Vector_3	xPosition(pxPhysicsObject->GetPosition());
        Asura_Vector_3	xVelocity(xFinish - xPosition);
		const float		fComponentAlongNormal	= xVelocity * xNormal;

		// Remove the movement in the normal direction...
        xVelocity	-= xNormal * fComponentAlongNormal;
        xVelocity.Normalise();

        if ( !s_bFallDirectlyDownwards || pxPhysicsObject->IsInContactWithFloor() )
        {
            pxPhysicsObject->SetLinVelocity( xVelocity * s_fLinkTraversalSpeed );
        }
        else
        {
            // Fall directly downwards is a misnomer.
            // We should fall mostly downwards, keeping an amount of movement towards our destination so we don't catch on a wall.
            pxPhysicsObject->SetLinVelocity( xVelocity * 0.5f );
        }

        Asura_Physics_System::UpdateForWaypointing();

		xPosition	= pxPhysicsObject->GetPosition();

        // Keep the maximum distance fallen up to date.
        if ( pxPhysicsObject->IsInContactWithFloor() )
        {
            xPreFallPosition = xPosition;
        }
        else
        {
			const Asura_Vector_3 xDifference( xPosition - xPreFallPosition );
			const float			 fNewComponentAlongNormal = xDifference * xNormal;

            // We're only interested in movement in the opposite direction from the normal, which we assume is up.
            // In other words we're interested in the largest negative value of fNewComponentAlongNormal.
            fMaxFallDistance = Asura_Maths::Max( fMaxFallDistance, -fNewComponentAlongNormal );
        }

        // Output the position data if required.
        if ( pxPositionArray && puPositionCount && *puPositionCount < uMaxPositions )
        {
            *pxPositionArray++ = xPosition;
            ++( *puPositionCount );
        }

        // Make sure we are still moving towards our destination.
        const float fDistanceSq = ( xFinish - xPosition ).MagnitudeSquared();
        if ( xPosition.ApproximatelyEquals( xPreviousPosition ) || fDistanceSq > fPreviousDistanceSq )
        {
            if ( ++uNotMovingTowardCount > 10 )
            {
				return(false);
            }
        }
        else
        {
            uNotMovingTowardCount = 0;
        }
        xPreviousPosition   = xPosition;
        fPreviousDistanceSq = fDistanceSq;

		// Test for environmental collisions.
		const Asura_Physics_EnvironmentCollisionReport* pxEnvCollisionReport = pxPhysicsObject->GetEnvironmentCollisionReportList();
		while ( pxEnvCollisionReport )
		{
			const u_int uPolyFlags = pxEnvCollisionReport->Tools_GetPolyFlags();
			if ( !( uPolyFlags & AsuraPolygonFlag_Floor || uPolyFlags & AsuraPolygonFlag_Step || uPolyFlags & AsuraPolygonFlag_DenseWaypoints ) )
			{
				// We've hit a poly that is not marked as floor/steps or dense - cull the link.
				return false;
			}

			pxEnvCollisionReport = pxEnvCollisionReport->GetNextReport();
		}

        // Test for object collisions.
        if ( pxPhysicsObject->GetObjectCollisionReportList() )
        {
            // If we hit any objects then we cull the link.
            return false;
        }

        bAtDestination	= xDestinationBB.Contains(xPosition);

    } while ( !bAtDestination && ++uLoopCount < uMaxLoopCount );

	return bAtDestination;
}

//////////////////////////////////////////////////////////////////////////////
//
// 
//
//////////////////////////////////////////////////////////////////////////////

void Tools_Mode_Waypointing::InitialiseMode()
{
    s_pxWaypointAnalysis = CreateWaypointAnalysis();
    s_pxWaypointEditing  = CreateWaypointEditing();

    if ( !s_bIsBatchWaypointing )
    {
		s_iInitialRenderMode = Tools_RenderCollisionMesh_System::GetRenderMode();

        TransferFileData( TOOLSMODEDATAFILE_LOAD );

        // Initialise selection helpers if necessary.
        Tools_Polygon_Selection::Activate();
        Tools_Waypoint_Selection::Activate();

        Tools_Mode_Flags::InitialiseExMode(AsuraPolygonFlag_Floor);

        //create the menubar
        s_iMenuHandle = Asura_Menubar_System::CreateMenuSection();

        Asura_Menubar_Title* xTitle;
        xTitle = Asura_Menubar_System::AddTitle( s_iMenuHandle, "Overlays" );
        xTitle->AddItem( "Find",        Tools_Texturing_Find::ToggleFindOverlay,          Tools_Texturing_Find::FindOverlayIsOn );
		xTitle->AddItem( "Flags",       Tools_Mode_Flags::ToggleFlagsOverlay,             Tools_Mode_Flags::FlagsOverlayIsOn );
        xTitle->AddItem( "Selection",   Tools_Polygon_Selection::ToggleSelectionOverlay,  Tools_Polygon_Selection::SelectionOverlayIsOn );
        xTitle->AddItem( "Waypointing", Tools_Mode_Waypointing::ToggleWaypointingOverlay, Tools_Mode_Waypointing::WaypointingOverlayIsOn );

        //create overlays
        CreateWaypointingOverlay();

        //add help
        Tools_Help::AddHelpTopic( "Waypointing Mode", WaypointingModeHelpStrings );

        CalculateWaypointDataStatistics();

        // create the necessary tools objects
        CreateUserWaypointToolsObjects();
#ifdef ASURA_USE_STREAMING
		Tools_InterZoneWaypointObject::CreateAll();
#endif
        Tools_Object_System::EnableAllControls( true );
    }

    s_pxPhysicsPositions = new Asura_Vector_3[ MAX_STORED_PHYSICS_POSITIONS ];
}

void Tools_Mode_Waypointing::ShutdownMode()
{
    delete [] s_pxPhysicsPositions;
    s_pxPhysicsPositions = NULL;
    s_pxWaypointEditing->Shutdown();
    delete s_pxWaypointEditing;
    s_pxWaypointEditing = NULL;
    s_pxWaypointAnalysis->Shutdown();
    delete s_pxWaypointAnalysis;
    s_pxWaypointAnalysis = NULL;

    if ( !s_bIsBatchWaypointing )
    {
        Tools_Help::RemoveHelpTopic( "Waypointing Mode" );

        Tools_Mode_Flags::ShutdownExMode();

        Tools_Waypoint_Selection::Deactivate();
        Tools_Polygon_Selection::Deactivate();

        // shutdown the menu
        if ( s_iMenuHandle != ASURA_INVALID_HANDLE )
        {
            Asura_Menubar_System::DestroyMenuSection( s_iMenuHandle );
            s_iMenuHandle = ASURA_INVALID_HANDLE;
        }

        // close overlays
        Asura_Overlay_System::CloseOverlays( TOOLS_OVERLAY_MODE );

        // release the test waypoint storage, if it's allocated
        DeleteTestWaypoints();

        // release the rest of the waypoint storage, if it's allocated
        ReleaseStorageAndReset( false );

        // destroy the necessary tools objects
        Tools_Object_System::DeselectAllObjects();
		DestroyUserWaypointToolsObjects();
#ifdef ASURA_USE_STREAMING
		Tools_InterZoneWaypointObject::DeleteAll();
#endif
        Tools_Object_System::EnableAllControls( false );

        TransferFileData( TOOLSMODEDATAFILE_SAVE );

		Tools_RenderCollisionMesh_System::SetRenderMode((Tools_CollRenderMode)s_iInitialRenderMode);
		
    }

	//clearup tools entities if they have been created
    Tools_Object_System::DeleteAllModeSpecific();
	s_bToolsEntitiesCreated = false;
	s_bStaticObjectsVisible = false;
}

Tools_Waypoint_Object* Tools_Mode_Waypointing::CreateUserWaypointToolsObject( Asura_AI_Navigation_System::UserDefinedWaypoint* pxUDW )
{
    return new Tools_Waypoint_Object( pxUDW );
}

void Tools_Mode_Waypointing::RenderLinks() const
{
    for ( int i = 0; i < Asura_AI_Navigation_System::GetNumberOfWaypoints(); ++i )
    {
        const Asura_AI_NavVertex* pxBaseWP = Asura_AI_Navigation_System::GetWaypoint( i );

        bool bShouldRenderWPLinks = true;
        if ( s_bShowOnlyInZone )
        {
			bShouldRenderWPLinks &= ( pxBaseWP->m_usZone == s_iZoneIDToRender );
        }
        if ( s_bShowOnlyInRange )
        {
            bShouldRenderWPLinks &= ( ( pxBaseWP->m_xPosition - Asura_Camera::GetCameraData( 0 ).m_xPosition ).MagnitudeSquared() < s_fRangeToRender * s_fRangeToRender );
        }

        if ( !bShouldRenderWPLinks )
        {
            continue;
        }

        for ( int c = 0; c < pxBaseWP->m_ucNumConnections; ++c )
        {
            int j = pxBaseWP->m_pxConnections[ c ].m_usIndex;

            const Asura_Vector_3& xP1 = pxBaseWP->m_xPosition;
            const Asura_Vector_3& xP2 = Asura_AI_Navigation_System::GetWaypoint( j )->m_xPosition;
            
            bool bIsTwoWayLink = false;
            for ( int k = 0; k < Asura_AI_Navigation_System::GetWaypoint( j )->m_ucNumConnections; ++k )
            {
                if ( Asura_AI_Navigation_System::GetWaypoint( j )->m_pxConnections[ k ].m_usIndex == i )
                {
                    bIsTwoWayLink = true;
                    break;
                }
            }

            const unsigned short usLinkFlags = pxBaseWP->m_pxConnections[ c ].m_usFlags;
            if ( usLinkFlags == 0 && bIsTwoWayLink )
            {
                // Standard link colour - white.
                Asura_Render::RenderLine( xP1, xP2, 0xffffffff );
            }
            else
            {
                Asura_Vector_3 xBeginColour, xEndColour;
                GetLinkColour( usLinkFlags, bIsTwoWayLink, xBeginColour, xEndColour );

                if ( xBeginColour == xEndColour )
                {
					unsigned int uLinkColour = 0xff000000;
					uLinkColour |= Asura_Maths::FloatToInt( xBeginColour.x * 255.0f ) << 16;
					uLinkColour |= Asura_Maths::FloatToInt( xBeginColour.y * 255.0f ) << 8;
					uLinkColour |= Asura_Maths::FloatToInt( xBeginColour.z * 255.0f );
					Asura_Render::RenderLine( xP1, xP2, uLinkColour );
                }
                else
                {
                    RenderLine( xP1, xP2, xBeginColour, xEndColour );
                }
            }
        }
    }
}

void Tools_Mode_Waypointing::RenderLinksHelpText( float& fX, float& fY ) const
{
    Asura_Text_Output::Print( fX, fY++, 0xffffffff, 0, "Link Colours:" );
    Asura_Text_Output::Print( fX, fY++, 0xffffffff, 0, "White       - standard link" );
    Asura_Text_Output::Print( fX, fY++, 0xffffffff, 0, "Blue        - restrictive link" );
    Asura_Text_Output::Print( fX, fY++, 0xffffffff, 0, "Green       - start of one way link" );
    Asura_Text_Output::Print( fX, fY++, 0xffffffff, 0, "Yellow      - start of fall link" );
    Asura_Text_Output::Print( fX, fY++, 0xffffffff, 0, "Red         - start of long fall link" );
	Asura_Text_Output::Print( fX, fY++, 0xffffffff, 0, "Cyan        - start of jump link" );
	Asura_Text_Output::Print( fX, fY++, 0xffffffff, 0, "Purple      - start of crouch link" );
	Asura_Text_Output::Print( fX, fY++, 0xffffffff, 0, "Dark Purple - start of prone link" );
}

void Tools_Mode_Waypointing::GenerateWaypoints( const bool bTestWaypointsOnly )
{
    s_bGenerateTestWaypointsOnly = bTestWaypointsOnly;

    PreGenerateWaypoints();
    GenerateWaypointsMain();
    PostGenerateWaypoints();

    s_bGenerateTestWaypointsOnly = false;
}

void Tools_Mode_Waypointing::PreGenerateWaypoints()
{
	Tools_MessageLog::DeleteMessages( Tools_MessageLog::WAYPOINTING, "" );
	Tools_MessageLog::AddMessage( Tools_MessageLog::WAYPOINTING, "Info", "Starting waypoint generation..." );
    UpdateProgress( 1, 2, "Starting waypoint generation..." );

    // need to redo any collision meshes with DontExport polygons
    for ( int i = 0; i < Asura_Module_System::GetNumberOfModules(); ++i )
    {
        Asura_Mesh* pxMesh = Asura_Module_System::GetMeshForModule( i );
        if ( !pxMesh->PolygonFlagDataPtr ) { continue; }

        bool bNeedToGenerate = false;
        for ( int j = 0; j < pxMesh->NumberOfPolygons; ++j )
        {
            if ( pxMesh->PolygonFlagDataPtr[ j ].Flags & AsuraPolygonFlag_DontExport )
            {
                bNeedToGenerate = true;
                break;
            }
        }
        if ( bNeedToGenerate )
        {
            Asura_Module_System::InvalidateCollisionMesh( i );
        }
    }

    // create collision meshes that include the don't export polys
    Asura_CollisionMesh::SetIncludeDontExportPolys( true );
    Asura_Module_System::UpdateInvalidCollisionMeshes();
    Asura_CollisionMesh::SetIncludeDontExportPolys( false );

    Tools_Mode_Flags::RegenerateStepFlags();

	s_bShowMessageLogOnCompletion = false;
}

void Tools_Mode_Waypointing::GenerateWaypointsMain()
{
	//ASURA_CPU_PROFILE_SCOPED_TIMER("Tools_Mode_Waypointing::GenerateWaypointsMain");

	const float fStartTime = Asura_Timers::GetUncappedPhase();

    int  iPhase    = 0;
    bool bFinished = false;
    bool bInError  = false;
    do
    {
        bInError = !GenerateWaypointsSingleStep( iPhase, bFinished );
		++iPhase;
    } while ( !bFinished && !bInError );

    if ( bInError )
    {
		Tools_MessageLog::AddMessage( Tools_MessageLog::WAYPOINTING, "Error", "Waypoint generation aborted." );
        if ( s_bDataInNavSystem )
        {
			Tools_MessageLog::AddMessage( Tools_MessageLog::WAYPOINTING, "Error", "Data has already been copied to the navigation system." );
			Tools_MessageLog::AddMessage( Tools_MessageLog::WAYPOINTING, "Error", "This data must now be deleted." );
            Asura_AI_Navigation_System::ResetForWaypointGeneration();
        }
		else if ( s_iNumWaypoints > USHRT_MAX )
		{
			Tools_MessageLog::AddMessage( Tools_MessageLog::WAYPOINTING, "Error", "Too much data generated - aborting!" );
		}

		s_bShowMessageLogOnCompletion = true;
    }
    else
    {
        const float fTimeTaken = Asura_Timers::GetUncappedPhase() - fStartTime;
		Tools_MessageLog::AddMessage( Tools_MessageLog::WAYPOINTING, "Info", "Time taken: %8.1f secs", fTimeTaken );
    }

    ReleaseStorageAndReset();
    CalculateWaypointDataStatistics();
}

void Tools_Mode_Waypointing::FindNextIncrementalWaypointPhase()
{
	const char*	szPhaseName = NULL;
	do
	{
		szPhaseName	= s_pxThis->GetPhaseName( ++s_iGenerateIncrementalWaypointPhase );
	} while ( szPhaseName == NULL );

	Tools_Timed_Message_System::AddMessage( Tools_Timed_Message_System::COLOUR_DEFAULT, "Next phase '%s'...", szPhaseName );
}

void Tools_Mode_Waypointing::GenerateWaypointsMainIncremental()
{
	bool	bFinished	= false;
    bool	bInError	= false;

	// Handle this phase...
	bInError = !GenerateWaypointsSingleStep( s_iGenerateIncrementalWaypointPhase, bFinished );

	// Check for errors...
    if ( bInError )
    {
        Tools_MessageLog::AddMessage( Tools_MessageLog::WAYPOINTING, "Error", "Waypoint generation aborted." );
        if ( s_bDataInNavSystem )
        {
			Tools_MessageLog::AddMessage( Tools_MessageLog::WAYPOINTING, "Error", "Data has already been copied to the navigation system." );
			Tools_MessageLog::AddMessage( Tools_MessageLog::WAYPOINTING, "Error", "This data must now be deleted." );
            Asura_AI_Navigation_System::ResetForWaypointGeneration();
        }

		s_bShowMessageLogOnCompletion = true;
    }

    if ( bFinished )
    {
        Tools_MessageLog::AddMessage( Tools_MessageLog::WAYPOINTING, "Info", "Waypoint generation complete." );
		s_iGenerateIncrementalWaypointPhase	= INT_MAX;
    }

    if ( bFinished || bInError )
    {
        ReleaseStorageAndReset();
        CalculateWaypointDataStatistics();
    }
	else
	{
		// Find the next valid phase...
		FindNextIncrementalWaypointPhase();
	}
}

bool Tools_Mode_Waypointing::GenerateWaypointsSingleStep( int iPhase, bool& bFinished )
{
	//ASURA_CPU_PROFILE_SCOPED_TIMER("Tools_Mode_Waypointing::GenerateWaypointsSingleStep");

	if ( GetPhaseName( iPhase ) )
	{
		Tools_MessageLog::AddMessage( Tools_MessageLog::WAYPOINTING, "Info", "Phase '%s'.", GetPhaseName( iPhase ) );
	}

	bool bInError = false;
	bFinished = false;

	switch ( iPhase )
	{
		// Initialisation.
        case   0:
        {
            if ( !VerifyWaypointingParameters() )
            {
                // Offer the user the chance to cancel the waypointing.
                YesNo_Overlay xYNOverlay( "Some waypointing parameters are invalid. Abort waypointing?" );
                if ( xYNOverlay.DoModal() )
                {
                    bInError = true;
                }
            }
            break;
        }
		case  10: bInError = ( InitialiseAndAllocateStorage()        == false ); break;
        case  20: bInError = ( InitialisePhysicsDataForWaypointing() == false ); break;

        // Waypointing.
        case  30: bInError = ( AddNewWaypoints()                     == false ); break;
        case  40: bInError = ( ReleaseHashTableStorage()             == false ); break;
        case  50: bInError = ( FindPotentialCoverpoints()            == false ); break;
        case  60: bInError = ( AllocateCoverpointStorage()           == false ); break;
        case  70: bInError = ( GenerateCoverpointData()              == false ); break;
        case  80: bInError = ( CullWaypointsUnderObjectsAndDoors()   == false ); break;
		case  85: bInError = ( CullWaypointsUnderContainers()		 == false ); break;
        case  90: bInError = ( CalculateWaypointVolumes()            == false ); break;
        case 100: bInError = ( CullWaypointsBySize()                 == false ); break;
        case 110: bInError = ( SortWaypointsPerBin()                 == false ); break;
        case 120: bInError = ( CullRedundantWaypoints()              == false ); break;
        case 130: bInError = ( AllocateReducedSizeWaypointStorage()  == false ); break;
        case 140: bInError = ( CopyWaypointsIntoReducedStorage()     == false ); break;
        case 150: bInError = ( ReleaseInitialWaypointStorage()       == false ); break;
        case 160: bInError = ( CullPhysicallyInvalidWaypoints()      == false ); break;

        // Linking.
        case 170: bInError = ( GenerateWaypointLinks()       == false ); break;
        case 180: bInError = ( CullRedundantLinks()          == false ); break;
        case 190: bInError = ( MakeAllLinksTwoWay()          == false ); break;
        case 200: bInError = ( CullAndFlagLinksWithPhysics() == false ); break;
        case 210:
        {
            if ( s_bGenerateTestWaypointsOnly )
            {
                bInError = ( CullTestWaypointsWithNoLinks() == false );
            }
            break;
        }
		case 220: bInError = ( AddInterZoneWaypointLinks() == false ); break;

        // Coverpointing.
        case 230: bInError = ( CheckCoverpointsAreNearWaypoints() == false ); break;
        case 240: bInError = ( CullRedundantCoverpoints()         == false ); break;
        case 250: bInError = ( AssignCoverpointsToWaypoints()     == false ); break;
        case 260: bInError = ( FindCoverpointExits()              == false ); break;

        // Setting up navigation system.
        case 270:
        {
            if ( s_bGenerateTestWaypointsOnly )
            {
                bInError  = ( CopyDataToTestWaypointStorage() == false );
                bFinished = true;
            }
            else
            {
                bInError           = ( CopyDataToNavigationSystem() == false );
                s_bDataInNavSystem = !bInError;
            }
        } break;

        // Zoning.
        case 280: bInError = ( CullOneWayLinksWithNoReturn() == false ); break;
        case 290: bInError = ( CalculateWaypointZoneIDs()    == false ); break;

        // Final steps to creating new nav data.
        case 300: bInError = ( CullWaypointsWithNoLinks()              == false ); break;
        case 310: bInError = ( CreateNavigationSystemWaypointTree()    == false ); break;
		case 320: bInError = ( ResetPhysicsDataAfterWaypointing()	   == false ); break;
        case 330: bInError = ( CalculatePhysicalObjectWaypointBlocks() == false ); break;
        case 340: bFinished = true; break;

        default: bInError = false; break;
	}

    return !bInError;
}

void Tools_Mode_Waypointing::PostGenerateWaypoints()
{
    // put back the collision meshes to their normal state
    for ( int i = 0; i < Asura_Module_System::GetNumberOfModules(); ++i )
    {
        Asura_Mesh* pxMesh = Asura_Module_System::GetMeshForModule( i );
        if ( !pxMesh->PolygonFlagDataPtr ) { continue; }

        bool bNeedToGenerate = false;
        for ( int j = 0; j < pxMesh->NumberOfPolygons; ++j )
        {
            if ( pxMesh->PolygonFlagDataPtr[ j ].Flags & AsuraPolygonFlag_DontExport )
            {
                bNeedToGenerate = true;
                break;
            }
        }
        
        if ( bNeedToGenerate )
        {
            Asura_Module_System::InvalidateCollisionMesh( i );
        }
    }
    Asura_Module_System::UpdateInvalidCollisionMeshes();

	Tools_MessageLog::AddMessage( Tools_MessageLog::WAYPOINTING, "Info", "Waypoint generation finished." );
	if ( s_bShowMessageLogOnCompletion )
	{
		Tools_MessageLog::ShowOverlay( Tools_MessageLog::WAYPOINTING, "" );
		s_bShowMessageLogOnCompletion = false;
	}
}

void Tools_Mode_Waypointing::VerifyWaypointingData()
{
    VerifyWaypointingParameters();

    // Check that there are no waypointing flags on non-collision mesh. Warn if there are.
    WarnAboutNonCollisionMeshWaypointingFlags();

    // Check for areas of mesh flagged as 'create waypoints' that don't have waypoint coverage.

    // Check for links passing through areas that don't have waypoint coverage.

	// Check physical objects pos and ori.
	WarnAboutBadPhysicalObjectPosOri();

	// Check that physical objects have their waypointing flags set correctly.
	WarnAboutBadPhysicalObjectWPFlags();
}

bool Tools_Mode_Waypointing::VerifyWaypointingParameters()
{
	bool bOk = true;

    if ( s_fMaxLinkLength < s_fMaxWaypointRadius * 2.0f )
    {
        Tools_MessageLog::AddMessage( Tools_MessageLog::WAYPOINTING, "Verification",
            "The maximum link length must be at least twice the maximum waypoint radius. Suggested value is %.2f.", s_fMaxWaypointRadius * 2.1f );
		bOk = false;
    }
    if ( s_fMaxLinkLength > s_fMaxWaypointRadius * 2.25f )
    {
        Tools_MessageLog::AddMessage( Tools_MessageLog::WAYPOINTING, "Verification",
            "The maximum link length is larger than twice the maximum waypoint radius. Suggested value is %.2f.", s_fMaxWaypointRadius * 2.1f );
		bOk = false;
    }

	// When packing into the hash table we make some assumptions about the size and number of the waypoint bins. Check those here.
	if ( NUM_WAYPOINTS_PER_BIN > ( 1 << 24 ) )
	{
		Tools_MessageLog::AddMessage( Tools_MessageLog::WAYPOINTING, "Verification",
			"NUM_WAYPOINTS_PER_BIN too large for hash table calculations." );
		bOk = false;
	}
	if ( NUM_WAYPOINT_BINS > ( 1 << 8 ) )
	{
		Tools_MessageLog::AddMessage( Tools_MessageLog::WAYPOINTING, "Verification",
			"NUM_WAYPOINT_BINS too large for hash table calculations" );
		bOk = false;
	}

	// Verify UDWs.
	for ( Asura_Hashed_List_It< Tools_Waypoint_Object > xObjIter( Tools_Waypoint_Object::GetList() ); !xObjIter.Done(); xObjIter.Next() )
	{
		bOk &= xObjIter.GetCurrent()->WaypointingVerify();
	}

    return bOk;
}

bool Tools_Mode_Waypointing::InitialiseAndAllocateStorage()
{
	//ASURA_CPU_PROFILE_SCOPED_TIMER("Tools_Mode_Waypointing::InitialiseAndAllocateStorage");
	
    // allocate the waypoint storage
    for ( int i = 0; i < NUM_WAYPOINT_BINS; ++i )
    {
		UpdateProgress( i, NUM_WAYPOINT_BINS, "Allocating waypoint storage..." );

        s_aiNumWaypoints[ i ]     = 0;
        s_apxWaypointStorage[ i ] = new Tools_Nav_Vertex[ NUM_WAYPOINTS_PER_BIN ];
        if ( !s_apxWaypointStorage[ i ] )
        {
            Asura_Assertf( false, ( "Tools_Mode_Waypointing::InitialiseAndAllocateStorage - could not allocate waypoint storage bin %d", i ) );
            return false;
        }
    }

    // set up the waypoint position hash table
    s_ppiHashTable = new int*[ NUM_HASH_BINS ];
    if ( !s_ppiHashTable )
    {
        Asura_Assert( false, "Tools_Mode_Waypointing::InitialiseAndAllocateStorage - could not allocate hash table storage" );
        return false;
    }
    s_piNumHashEntries = new int[ NUM_HASH_BINS ];
    if ( !s_piNumHashEntries )
    {
        Asura_Assert( false, "Tools_Mode_Waypointing::InitialiseAndAllocateStorage - could not allocate hash table count storage" );
        return false;
    }

    for ( int i = 0; i < NUM_HASH_BINS; ++i )
    {
        UpdateProgress( i, NUM_HASH_BINS, "Allocating waypoint storage..." );

        s_piNumHashEntries[ i ] = 0;
        s_ppiHashTable[ i ]     = new int[ NUM_HASH_BIN_ENTRIES ];
        if ( !s_ppiHashTable[ i ] )
        {
            Asura_Assertf( false, ( "Tools_Mode_Waypointing::InitialiseAndAllocateStorage - could not allocate hash table bin %d", i ) );
            return false;
        }
    }

    s_bHashStorageAllocated = true;
	s_bDataInNavSystem      = false;

	Tools_Waypoint_Selection::ClearSelection();

    return true;
}

bool Tools_Mode_Waypointing::InitialisePhysicsDataForWaypointing()
{
	//ASURA_CPU_PROFILE_SCOPED_TIMER("Tools_Mode_Waypointing::InitialisePhysicsDataForWaypointing");
	
    s_bPhysicsDataInitialised = true;

	RememberPhysicsObjectsStatus();
	SetRequiredPhysicsObjectsAsIgnored();
	return CreateWaypointingPhysicsObjects();
}

bool Tools_Mode_Waypointing::AddNewWaypoints()
{
	//ASURA_CPU_PROFILE_SCOPED_TIMER("Tools_Mode_Waypointing::AddNewWaypoints");
	
    Asura_Vector_3 axVtxTmp[ 4 ];

    const bool bUseCurrentlySelectedPolys = s_bGenerateTestWaypointsOnly;

    if ( bUseCurrentlySelectedPolys )
    {
		// Add our user defined waypoints.
		for ( Asura_Hashed_List_ConstIt< Tools_Waypoint_Object > xUDWIter( Tools_Waypoint_Object::GetList() ); !xUDWIter.Done(); xUDWIter.Next() )
		{
			AddUserDefinedNewWaypoint( xUDWIter.GetCurrent() );
		}

		// Add waypoints from all currently selected polys with the waypoint generating flags on them.
        Tools_Polygon_Selection::StartIteration();
        PolygonID xPolygonID;
        while ( Tools_Polygon_Selection::GetNextSelected( &xPolygonID ) )
        {
            if ( UpdateProgress( 1, 2, "Adding waypoints from currently selected polys (no progress bar)..." ) )
            {
                // Quit requested.
                return false;
            }

            Asura_Mesh* pxMesh = Asura_Module_System::GetMeshForModule( xPolygonID.ModuleIndex );
            if ( pxMesh == NULL )
            {
                continue;
            }
            if ( !pxMesh->AreNormalsValid() )
            {
                pxMesh->CreateNormals();
            }

            Asura_Vector_3 xModulePos;
            Asura_Module_System::GetPositionOfModule( xPolygonID.ModuleIndex, xModulePos );

			const int			  iPolyFlags  = pxMesh->PolygonFlagDataPtr ? pxMesh->PolygonFlagDataPtr[ xPolygonID.PolyIndex ].Flags : 0;
			const Asura_Vector_3& xFaceNormal = pxMesh->NormalsDataPtr[ xPolygonID.PolyIndex ].FaceNormal;

			// Skip if this isn't a floor, step or dense flagged poly.
			if ( !( iPolyFlags & AsuraPolygonFlag_Floor ) &&
				!( iPolyFlags & AsuraPolygonFlag_Step ) &&
				!( iPolyFlags & AsuraPolygonFlag_DenseWaypoints ) )
			{
				continue;
			}

			const int iNumVerts = pxMesh->PolygonDataPtr[ xPolygonID.PolyIndex ].NumberOfVertices();

			// Form some waypoints from this poly.
            for ( int i = 0; i < iNumVerts; ++i )
            {
                axVtxTmp[ i ] = pxMesh->VertexDataPtr[ pxMesh->PolygonDataPtr[ xPolygonID.PolyIndex ].VertexID[ i ] ].Position + xModulePos;
            }

			// determine necessary waypoint flags
            const int iWaypointFlags = GetFlagsForNewWaypoint( iPolyFlags, xFaceNormal );
            for ( int i = 0; i < iNumVerts; ++i )
            {
                if ( !AddNewWaypoint( xPolygonID.ModuleIndex, axVtxTmp[ i ], xFaceNormal, iWaypointFlags ) )
                {
                    return false;
                }
				if ( ( iPolyFlags & AsuraPolygonFlag_DoubleSided ) && !AddNewWaypoint( xPolygonID.ModuleIndex, axVtxTmp[ i ], -xFaceNormal, iWaypointFlags ) )
				{
					return false;
				}
            }

			s_iAWCurrentModuleIndex			= xPolygonID.ModuleIndex;
			s_xAWCurrentNormal				= xFaceNormal;
            s_iAWCurrentWPFlags				= iWaypointFlags;
			s_fAWCurrentMinSubdivideSizeSq	= ( iPolyFlags & AsuraPolygonFlag_DenseWaypoints ) ? s_fMinDenseSubdivideSizeSq : s_fMinSubdivideSizeSq;
			s_bAWDoubleSidedPoly			= ( iPolyFlags & AsuraPolygonFlag_DoubleSided );

            // add additional waypoints for large polys.
            if ( iNumVerts == 4 )
            {
                if ( !SubdivideAndAddWaypoints( axVtxTmp[ 0 ], axVtxTmp[ 1 ], axVtxTmp[ 2 ], axVtxTmp[ 3 ], s_iMaxSubdivideRecursionLevel ) )
                {
                    return false;
                }
            }
            else
            {
                if ( !SubdivideAndAddWaypoints( axVtxTmp[ 0 ], axVtxTmp[ 1 ], axVtxTmp[ 2 ], s_iMaxSubdivideRecursionLevel ) )
                {
                    return false;
                }
            }
        }
    }
    else
    {
		// Add our user defined waypoints.
		for ( Asura_Hashed_List_ConstIt< Tools_Waypoint_Object > xUDWIter( Tools_Waypoint_Object::GetList() ); !xUDWIter.Done(); xUDWIter.Next() )
		{
			AddUserDefinedNewWaypoint( xUDWIter.GetCurrent() );
		}

		// Add waypoints from all polys with the waypoint generating flags on them.
        for ( int m = 0; m < Asura_Module_System::GetNumberOfModules(); ++m )
        {
            if ( UpdateProgress( m, Asura_Module_System::GetNumberOfModules(), "Adding waypoints from floor polys..." ) )
            {
				// Quit requested.
                return false;
            }

            Asura_Mesh* pxMesh = Asura_Module_System::GetMeshForModule( m );
            if ( pxMesh == NULL )
            {
                continue;
            }
            if ( !pxMesh->AreNormalsValid() )
            {
                pxMesh->CreateNormals();
            }

            Asura_Vector_3 xModulePos;
            Asura_Module_System::GetPositionOfModule( m, xModulePos );

            for ( int p = 0; p < pxMesh->NumberOfPolygons; ++p )
            {
				const int				iPolyFlags	= pxMesh->PolygonFlagDataPtr ? pxMesh->PolygonFlagDataPtr[ p ].Flags : 0;
				const Asura_Vector_3&	xFaceNormal	= pxMesh->NormalsDataPtr[ p ].FaceNormal;

                // Skip if this isn't a floor, step or dense flagged poly.
                if ( !( iPolyFlags & AsuraPolygonFlag_Floor ) &&
					 !( iPolyFlags & AsuraPolygonFlag_Step ) &&
					 !( iPolyFlags & AsuraPolygonFlag_DenseWaypoints ) )
                {
                    continue;
                }

                const int iNumVerts = pxMesh->PolygonDataPtr[ p ].NumberOfVertices();

                // Form some waypoints from this poly.
                for ( int i = 0; i < iNumVerts; ++i )
                {
                    axVtxTmp[ i ] = pxMesh->VertexDataPtr[ pxMesh->PolygonDataPtr[ p ].VertexID[ i ] ].Position + xModulePos;
                }

                // determine necessary waypoint flags
				const int iWaypointFlags = GetFlagsForNewWaypoint( iPolyFlags, xFaceNormal );
				for ( int i = 0; i < iNumVerts; ++i )
				{
					if ( !AddNewWaypoint( m, axVtxTmp[ i ], xFaceNormal, iWaypointFlags ) )
					{
						return false;
					}
					if ( ( iPolyFlags & AsuraPolygonFlag_DoubleSided ) && !AddNewWaypoint( m, axVtxTmp[ i ], -xFaceNormal, iWaypointFlags ) )
					{
						return false;
					}
				}

				s_iAWCurrentModuleIndex			= m;
				s_xAWCurrentNormal				= xFaceNormal;
				s_iAWCurrentWPFlags				= iWaypointFlags;
				s_fAWCurrentMinSubdivideSizeSq	= ( iPolyFlags & AsuraPolygonFlag_DenseWaypoints ) ? s_fMinDenseSubdivideSizeSq : s_fMinSubdivideSizeSq;
				s_bAWDoubleSidedPoly			= ( iPolyFlags & AsuraPolygonFlag_DoubleSided );

                // add additional waypoints for large polys.
                if ( iNumVerts == 4 )
                {
                    if ( !SubdivideAndAddWaypoints( axVtxTmp[ 0 ], axVtxTmp[ 1 ], axVtxTmp[ 2 ], axVtxTmp[ 3 ], s_iMaxSubdivideRecursionLevel ) )
                    {
                        return false;
                    }
                }
                else
                {
                    if ( !SubdivideAndAddWaypoints( axVtxTmp[ 0 ], axVtxTmp[ 1 ], axVtxTmp[ 2 ], s_iMaxSubdivideRecursionLevel ) )
                    {
                        return false;
                    }
                }
            }
        }

#ifdef ASURA_USE_STREAMING		
		// add user defined waypoint pairs for connecting zones
        for ( u_int u = 0; u < Asura_AI_Navigation_System::GetNumInterZoneWaypointPairs(); ++u )
        {
			const Tools_AI_InterZoneWaypointPair& xPair = Asura_AI_Navigation_System::GetInterZoneWaypointPair(u);
            AddInterZoneNewWaypoint( xPair.m_xThisZonePosition, (int)u,true);
            AddInterZoneNewWaypoint( xPair.m_xOtherZonePosition, (int)u,false);
        }
#endif
    }

    return SomeBinWaypointsExist();
}

bool Tools_Mode_Waypointing::ReleaseHashTableStorage()
{
    UpdateProgress( 1, 2, "Cleaning up unused memory..." );

//  // go through and dump the stats about hash table usage
//  float fAverage = 0.0f;
//  int i;
//  int iZeroCount = 0;
//  for ( i = 0; i < NUM_HASH_BINS; ++i )
//  {
//      if ( s_piNumHashEntries[ i ] == 0 )
//      {
//          ++iZeroCount;
//      }
//      else
//      {
//          fAverage += s_piNumHashEntries[ i ];
//      }
//  }
//  fAverage /= NUM_HASH_BINS;
//
//  float fStdDev = 0.0f;
//  for ( i = 0; i < NUM_HASH_BINS; ++i )
//  {
//      fStdDev += ( s_piNumHashEntries[ i ] - fAverage ) * ( s_piNumHashEntries[ i ] - fAverage );
//  }
//  fStdDev /= NUM_HASH_BINS;
//  fStdDev = Asura_Maths::Sqrt( fStdDev );
//  Tools_Timed_Message_System::AddMessage( 0xffffffff, "average: %f", fAverage );
//  Tools_Timed_Message_System::AddMessage( 0xffffffff, "std dev: %f", fStdDev );
//  Tools_Timed_Message_System::AddMessage( 0xffffffff, "zero count: %d", iZeroCount );

    if ( s_bHashStorageAllocated )
    {
        s_bHashStorageAllocated = false;

        for ( int i = 0; i < NUM_HASH_BINS; ++i )
        {
            delete [] s_ppiHashTable[ i ];
            s_ppiHashTable[ i ] = NULL;
        }
        delete [] s_ppiHashTable;
        s_ppiHashTable = NULL;

        delete [] s_piNumHashEntries;
        s_piNumHashEntries = NULL;
    }

    return true;
}

bool Tools_Mode_Waypointing::FindPotentialCoverpoints()
{
    if ( s_bShouldGenCoverpoints == false || s_bGenerateTestWaypointsOnly )
    {
        return true;
    }

    // Consider all potential waypoints as potential coverpoints.
    s_iNumCoverpoints = GetTotalBinWaypointCount();

    return true;
}

bool Tools_Mode_Waypointing::AllocateCoverpointStorage()
{
    if ( s_bShouldGenCoverpoints == false || s_bGenerateTestWaypointsOnly )
    {
        return true;
    }

    UpdateProgress( 1, 2, "Allocating coverpoint storage..." );

    s_pxCoverpointStorage = new Tools_Nav_Coverpoint[ s_iNumCoverpoints ];
    return ( s_pxCoverpointStorage != NULL );
}

bool Tools_Mode_Waypointing::GenerateCoverpointData()
{
    if ( s_bShouldGenCoverpoints == false || s_bGenerateTestWaypointsOnly )
    {
        return true;
    }

    //ASURA_CPU_PROFILE_SCOPED_TIMER("Tools_Mode_Waypointing::GenerateCoverpointData");

    const int ciNumRayTests = 8;
    Asura_Vector_3 xDeltaPos[ciNumRayTests];
    Asura_Vector_3 xXVector[ciNumRayTests];
    for(int iRay = 0; iRay < ciNumRayTests; ++iRay)
    {
        float fAngle = 3.141f * 2.0f * float(iRay) / float(ciNumRayTests);
        xDeltaPos[iRay] = Asura_Vector_3(sinf(fAngle), 0.0f, cosf(fAngle)) * 1.0f;
        xXVector[iRay]  = Asura_Vector_3(cosf(fAngle), 0.0f, -sinf(fAngle));
    }

    Asura_Char acProgressString[ 128 ];

    int iStorageIndex = 0;
    for ( int iBin = 0; iBin < NUM_WAYPOINT_BINS; ++iBin )
    {
        Asura_PrintfToBfr( acProgressString, "Calculating coverpoint data ( %d of %d )...", 128, true, iBin, NUM_WAYPOINT_BINS );

        for(int iWaypoint = 0; iWaypoint < s_aiNumWaypoints[ iBin ]; ++iWaypoint)
        {
            if ( UpdateProgress( iWaypoint, s_aiNumWaypoints[ iBin ], acProgressString ) )
            {
                return false;
            }

            //Do ray tests outwards to find geometry which may offer cover
            Asura_Vector_3 xTestPos0 = s_apxWaypointStorage[ iBin ][iWaypoint].m_xPosition;

            xTestPos0.y += -0.625f;
            for(int iRay = 0; iRay < ciNumRayTests; ++iRay)
            {
                Asura_Vector_3 xTestPos1 = xTestPos0 + xDeltaPos[iRay];

                if(LOSTestHit(xTestPos0, xTestPos1))
                {
                    //Found geometry - see if it is a corner or low wall
                    Asura_Vector_3 xTestPos2[3];
                    xTestPos0.y += -0.25f;
                    xTestPos2[0] = xTestPos0 + xXVector[iRay] * -0.5f;
                    xTestPos2[1] = xTestPos0 + xXVector[iRay] *  0.5f;
                    xTestPos2[2] = xTestPos0;
                    for(int iRay2 = 0; iRay2 < 3; ++iRay2)
                    {
                        Asura_Vector_3 xTestPos3 = xTestPos2[iRay2] + xDeltaPos[iRay] * 2.0f;
                        if(!LOSTestHit(xTestPos2[iRay2], xTestPos3))
                        {
                            //Check for overlapping coverpoints - don't add if overlapping
                            bool bCull = false;
                            for(int iCover = 0; iCover < iStorageIndex; ++iCover)
                            {
                                Asura_Vector_3 xDisp = s_pxCoverpointStorage[iCover].m_xCentre - s_apxWaypointStorage[ iBin ][iWaypoint].m_xPosition;
                                if(xDisp.MagnitudeSquared() < 1.0f * 1.0f)
                                {
                                    bCull = true;
                                    break;
                                }
                            }

                            if(!bCull)
                            {
                                s_pxCoverpointStorage[iStorageIndex].m_xCentre = s_apxWaypointStorage[ iBin ][iWaypoint].m_xPosition;
                                s_pxCoverpointStorage[iStorageIndex].m_fRadius = s_apxWaypointStorage[ iBin ][iWaypoint].m_fRadius;
                                s_pxCoverpointStorage[iStorageIndex].m_usFlags = 0;
                                ++iStorageIndex;
                            }

                            iRay = 1000000;
                            break;
                        }
                    }
                }
            }
        }
    }

    s_iNumCoverpoints = iStorageIndex;

    return true;
}

bool Tools_Mode_Waypointing::CullWaypointsUnderObjectsAndDoors()
{
	//ASURA_CPU_PROFILE_SCOPED_TIMER("Tools_Mode_Waypointing::CullWaypointsUnderObjectsAndDoors");
	
    Asura_Hashed_List_It< Asura_ServerEntity_PhysicalObject > xObjIter( Asura_ServerEntity_PhysicalObject::GetList() );
    
    // first count the number of objects so we can give a progress reading
    int iNumObjects = 0;
    while ( !xObjIter.Done() )
    {
        Asura_ServerEntity_PhysicalObject* pxObj = xObjIter.GetCurrent();
        xObjIter.Next();

        if ( pxObj != NULL )
        {
            ++iNumObjects;
        }
    }

    // now go through and remove waypoints
    int iObjectIndex = 0;
    xObjIter.Restart();
    while ( !xObjIter.Done() )
    {
        if ( UpdateProgress( iObjectIndex, iNumObjects, "Removing waypoints under objects..." ) )
        {
            return false;
        }

        const Asura_ServerEntity_PhysicalObject* pxObject = xObjIter.GetCurrent();
        xObjIter.Next();

        if ( pxObject == NULL )
        {
            continue;
        }

		if ( !pxObject->HasBlockNavDataGenerationFlag() )
		{
			continue;
		}

		// Get collision data from the physical object.
        const Asura_Collision_Object* pxCollisionData = pxObject->GetCollisionObject();
        RemoveWaypointsBlockedByCollisionObject(pxCollisionData, pxObject->GetPosition(), pxObject->GetOrientation());

        ++iObjectIndex;
    }

    return SomeBinWaypointsExist();
}


void Tools_Mode_Waypointing::RemoveWaypointsBlockedByCollisionObject(const Asura_Collision_Object* pxCollisionData,
																	 const Asura_Vector_3& xPosition,
																	 const Asura_Matrix_3x3& xOrientation)
{
	if ( pxCollisionData == NULL ) return;

	 // Obstacle BB in object space.
    const Asura_Bounding_Box& xObjectBB = pxCollisionData->GetBoundingBox();
    Asura_Matrix_3x3 xInverseOrient = xOrientation;

	// we want to transform waypoints into the object space, so we need to transpose (invert) the orientation matrix
	xInverseOrient.Transpose();

    // Exclude waypoints that are within s_fMinWaypointClearanceRadius of the static object.
	int iStartWP = Asura_AI_Navigation_System::GetNumUserDefinedWaypoints();
    for ( int iBin = 0; iBin < NUM_WAYPOINT_BINS; ++iBin )
    {
        if ( iStartWP < s_aiNumWaypoints[ iBin ] )
        {
            for ( int i = iStartWP; i < s_aiNumWaypoints[ iBin ]; )
            {
                // Transform waypoint to object space
                Asura_Vector_3 xSeparationVector = s_apxWaypointStorage[ iBin ][ i ].m_xPosition - xPosition;
                xSeparationVector = xSeparationVector * xInverseOrient;

                // Now do a test of this sphere against the object space BB.
                if ( xObjectBB.IntersectsSphere( xSeparationVector, s_fMinWaypointClearanceRadius ) )
                {
                    // Waypoint is blocked by the object, remove it and adjust all other waypoints.
                    s_apxWaypointStorage[ iBin ][ i ] = s_apxWaypointStorage[ iBin ][ --s_aiNumWaypoints[ iBin ] ];
                }
                else
                {
                    ++i;
                }
            }

            iStartWP = 0;
        }
        else
        {
            iStartWP -= s_aiNumWaypoints[ iBin ];
        }
    }
}


bool Tools_Mode_Waypointing::CullWaypointsUnderContainers()
{
#ifdef ASURA_USE_CONTAINER_SYSTEM
	// Make sure that we have created the container tools entities,
	// which are needed for determining which waypoints are blocked.
	CreateToolsEntities();

	Asura_Hashed_List_It< Tools_Entity_Container > xObjIter( Tools_Entity_Container::GetList() );
    
    // First count the number of container entities so we can give a progress reading
	int iNumObjects = 0;
	for ( ; !xObjIter.Done() ; xObjIter.Next() )
	{
		++iNumObjects;
	}

    // Now go through and remove waypoints
	int iObjectIndex = 0;
	for ( xObjIter.Restart(); !xObjIter.Done() ; xObjIter.Next(), ++iObjectIndex )
	{
		if ( UpdateProgress( iObjectIndex, iNumObjects, "Removing waypoints under containers..." ) )
        {
            return false;
        }

		const Tools_Entity_Container* const pxObject = xObjIter.GetCurrent();
		if (!pxObject) continue;
    
		pxObject->RemoveBlockedWaypoints();
	}

	return SomeBinWaypointsExist();
#else
	return true;
#endif
}

void Tools_Mode_Waypointing::CalcWPVolumesInBin_ThreadFunction( const void* pParams )
{
    CalcWPVolumesInBin_ThreadParams* pxParams = const_cast< CalcWPVolumesInBin_ThreadParams* >( static_cast< const CalcWPVolumesInBin_ThreadParams* >( pParams ) );

    // Find our first bin.
    pxParams->m_xCurrentBinMutex.Lock();
    u_int uBin = pxParams->m_uCurrentBin++;
    pxParams->m_xCurrentBinMutex.Unlock();
    while ( uBin < NUM_WAYPOINT_BINS && !pxParams->m_xCancelEvent.IsSignalled() )
    {
        for ( int iWaypointIndex = 0; iWaypointIndex < s_aiNumWaypoints[ uBin ]; ++iWaypointIndex )
        {
            // Initialise the waypoint size to the maximum.
            if ( s_apxWaypointStorage[ uBin ][ iWaypointIndex ].IsDense() )
            {
                s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_fRadius = s_fMaxDenseWaypointRadius;
            }
            else
            {
                s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_fRadius = s_fMaxWaypointRadius;
            }

            const Asura_Vector_3& xWaypointNormal	= s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_xNormal;
            const Asura_Vector_3& xWaypointPosition = s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_xPosition;

            for ( int iModule = -1; iModule < Asura_Module_System::GetNumberOfModules(); ++iModule )
            {
                // this stuff ensures we check the module this waypoint is in first (see end of 'for' also)
                if ( iModule == s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_iModuleIndex ) { continue; }
                if ( iModule == -1 ) { iModule = s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_iModuleIndex; }

                // If we've already shrunk this waypoint below the min radius then stop.
                if ( RadiusIsLessThanMinRadius( s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_iFlags, s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_fRadius ) )
                {
                    break;
                }

                // If min distance to module is greater than the current waypoint
                // radius then we can ignore it.
                {
                const Asura_Vector_3 xToModuleCentre = Asura_Module_System::GetCentreOfModule( iModule ) - xWaypointPosition;
                const float fT = ( s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_fRadius + Asura_Module_System::GetRadiusOfModule( iModule ) ) * 1.01f;
                if ( xToModuleCentre * xToModuleCentre > fT * fT )
                {
                    continue;
                }
                }
        
                const Asura_Mesh* pxMesh = Asura_Module_System::GetMeshForModule( iModule );
                if ( !pxMesh->AreNormalsValid() )
                {
                    continue;
                }

                Asura_Vector_3 xMeshPos;
                Asura_Module_System::GetPositionOfModule( iModule, xMeshPos );
                
                for ( int iPoly = 0; iPoly < pxMesh->NumberOfPolygons; ++iPoly )
                {
                    const u_int uPolyFlags = static_cast< u_int >( pxMesh->PolygonFlagDataPtr ? pxMesh->PolygonFlagDataPtr[ iPoly ].Flags : 0 );

                    // Skip if explicitly flagged as ignorable.
                    if ( uPolyFlags & AsuraPolygonFlag_IgnoreWhenWaypointing )
                    {
                        continue;
                    }
                    // Skip if any of the standard physics object 'flags of polys to ignore' match.
                    if ( uPolyFlags & static_cast< u_int >( s_pxStandardPhysicsObject->GetFlagsOfPolysToIgnore() ) )
                    {
                        continue;
                    }
                    
                    // Check whether there's any possibility of this poly blocking the waypoint's expansion.
                    bool bMayBlock = false;
				    // If it is not a step and not floor and not dense then it might block.
				    if ( !( uPolyFlags & AsuraPolygonFlag_Step ) &&
					    !( uPolyFlags & AsuraPolygonFlag_Floor ) &&
					    !( uPolyFlags & AsuraPolygonFlag_DenseWaypoints ) )
				    {
					    bMayBlock = true;
				    }
				    else
				    {
                        const Asura_Vector_3& xFaceNormal = pxMesh->NormalsDataPtr[iPoly].FaceNormal;
					    const float	          fDotProduct = xFaceNormal * xWaypointNormal;
					    if ( fDotProduct < s_fMaxDotProductBetweenWaypointPolygonNormals )
					    {
						    bMayBlock = true;
					    }
				    }

                    // if we don't think anything's going to block this waypoint then continue
                    if ( bMayBlock == false )
                    {
                        continue;
                    }

                    // Find min distance from waypoint to poly.
                    {
                    const Asura_AVector3 v0( pxMesh->VertexDataPtr[ pxMesh->PolygonDataPtr[ iPoly ].VertexID[ 0 ] ].Position );
                    const Asura_AVector3 v1( pxMesh->VertexDataPtr[ pxMesh->PolygonDataPtr[ iPoly ].VertexID[ 1 ] ].Position );
                    const Asura_AVector3 v2( pxMesh->VertexDataPtr[ pxMesh->PolygonDataPtr[ iPoly ].VertexID[ 2 ] ].Position );
                    const Asura_AVector3 xWaypointPositionA( xWaypointPosition - xMeshPos );
                    Asura_AVector3 xNearestPoint;
                    Asura_Physics_CCD::FindClosestPointOnTriangle( xWaypointPositionA, v0, v1, v2, xNearestPoint );
                    float fDistanceSq = ( xWaypointPositionA - xNearestPoint ).MagnitudeSquared();
                    if ( fDistanceSq < s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_fRadius * s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_fRadius )
                    {
                        s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_fRadius = Asura_Maths::Sqrt( fDistanceSq );
                    }

                    if ( pxMesh->PolygonDataPtr[ iPoly ].IsQuad() )
                    {
                        const Asura_AVector3 v3( pxMesh->VertexDataPtr[ pxMesh->PolygonDataPtr[ iPoly ].VertexID[ 3 ] ].Position );
                        Asura_Physics_CCD::FindClosestPointOnTriangle( xWaypointPositionA, v0, v2, v3, xNearestPoint );
                        float fDistanceSq = ( xWaypointPositionA - xNearestPoint ).MagnitudeSquared();
                        if ( fDistanceSq < s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_fRadius * s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_fRadius )
                        {
                            s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_fRadius = Asura_Maths::Sqrt( fDistanceSq );
                        }
                    }
                    }

					if ( RadiusIsLessThanMinRadius( s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_iFlags,
													s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_fRadius ) )
					{
						break;
					}
				} // end for all module polys

                if ( iModule == s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_iModuleIndex ) { iModule = -1; }
            } // end for modules

#ifdef ASURA_USE_BOTANICALS
		    // Check waypoints against botanicals
		    const Asura_LinkList< Asura_Botanicals_Instance* >& p_xBotanicalsList = Asura_Botanicals_InstanceManager::GetInstanceList();
		    for ( Asura_ListElt< Asura_Botanicals_Instance* >* pxElt = p_xBotanicalsList.Front(); pxElt; pxElt = pxElt->Next() )
		    {
			    // Give up if the waypoint size is now below the minimum.
                if ( RadiusIsLessThanMinRadius( s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_iFlags,
                                                s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_fRadius ) )
                {
                    break;
                }

			    Asura_Botanicals_Instance	*pxInstance = pxElt->Data();
			    Asura_Botanicals_Template	*pxTemplate = Asura_Botanicals_TemplateManager::GetTemplate( pxInstance->iTemplateIdx );

			    if ((pxTemplate) && (pxTemplate->bMergeCollisionMeshWithEnv))
			    {
				    const Asura_Vector_3 xToInstance = pxInstance->xPosition - xWaypointPosition;
				    const Asura_Vector_3 xMinCorner( -1.f, -4.f, -1.f );
				    const Asura_Vector_3 xMaxCorner( 1.f, 0.f, 1.f );
				    Asura_Vector_3 xDistance( 0.0f, 0.0f, 0.0f );

				    // Find min distance from point to BB.
				    for ( int i = 0; i < 3; ++i )
				    {
					    if ( xToInstance[ i ] < xMinCorner[ i ] )
					    {
						    xDistance[ i ] = xMinCorner[ i ] - xToInstance[ i ];
					    }
					    else if ( xToInstance[ i ] > xMaxCorner[ i ] )
					    {
						    xDistance[ i ] = xToInstance[ i ] - xMaxCorner[ i ];
					    }
				    }

				    const float fDistanceSq = xDistance.MagnitudeSquared();
				    if ( fDistanceSq < s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_fRadius * s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_fRadius )
				    {
                        s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_fRadius = Asura_Maths::Sqrt( fDistanceSq );
				    }

                    // Give up if the waypoint size is now below the minimum.
                    if ( RadiusIsLessThanMinRadius( s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_iFlags,
                                                    s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_fRadius ) )
                    {
                        break;
                    }
			    }
		    }
#endif

            // Check waypoints against static objects and doors as well.
            {
            pxParams->m_xIteratorMutex.Lock();
            Asura_Hashed_List_ConstIt< Asura_ServerEntity_PhysicalObject > xObjIter( Asura_ServerEntity_PhysicalObject::GetList() );
            pxParams->m_xIteratorMutex.Unlock();
            while ( !xObjIter.Done() )
            {
                // Give up if the waypoint size is now below the minimum.
                if ( RadiusIsLessThanMinRadius( s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_iFlags,
                                                s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_fRadius ) )
                {
                    break;
                }

                const Asura_ServerEntity_PhysicalObject* pxObject = xObjIter.GetCurrent();
                xObjIter.Next();

                if ( pxObject == NULL )
                {
                    continue;
                }

				// (TI - 01.03.2007) - We now have a flag to define if this object should block waypoints
				if ( !pxObject->HasBlockNavDataGenerationFlag() )
				{
					continue;
				}

                const Asura_Collision_Object* pxCollisionData = pxObject->GetCollisionObject();
                if ( pxCollisionData == NULL )
                {
                    continue;
                }

                // Transform waypoint position to object space.
                const Asura_Matrix_3x3& xOrient = pxObject->GetOrientation();
                Asura_Vector_3 xAlpha = xWaypointPosition - pxObject->GetPosition();
                xAlpha = xOrient.ApplyTranspose( xAlpha );

                const float fDistanceSq = pxCollisionData->GetBoundingBox().GetSqdMinDistFromPoint( xAlpha );
                if ( fDistanceSq < s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_fRadius * s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_fRadius )
                {
                    s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_fRadius = Asura_Maths::Sqrt( fDistanceSq );

                    // Give up if the waypoint size is now below the minimum.
                    if ( RadiusIsLessThanMinRadius( s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_iFlags,
                                                    s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_fRadius ) )
                    {
                        break;
                    }
                }
            } // end while iterating over static objects and doors
            pxParams->m_xIteratorMutex.Lock();
            } // iterator destruction
            pxParams->m_xIteratorMutex.Unlock();
 
	
#ifdef ASURA_USE_CONTAINER_SYSTEM
			// Check waypoints against containers as well.
            {
            pxParams->m_xContainerIteratorMutex.Lock();
            Asura_Hashed_List_ConstIt< Tools_Entity_Container > xObjIter( Tools_Entity_Container::GetList() );
            pxParams->m_xContainerIteratorMutex.Unlock();
            	
			for ( ; !xObjIter.Done() ; xObjIter.Next() )
            {
                // Give up if the waypoint size is now below the minimum.
                if ( RadiusIsLessThanMinRadius( s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_iFlags,
                                                s_apxWaypointStorage[ uBin ][ iWaypointIndex ].m_fRadius ) )
                {
                    break;
                }

                const Tools_Entity_Container* pxObject = xObjIter.GetCurrent();

				// MGH 18/11/2008: Check flag (cached in earlier step) which determines whether the entity blocks waypoints.
				if ( pxObject && pxObject->ShouldBlockWaypointGeneration() )
				{
					// Reduce waypoint radius as necessary.
					pxObject->ReduceWaypointRadius( &s_apxWaypointStorage[ uBin ][ iWaypointIndex ] );
				}

            } // end while iterating over containers

            pxParams->m_xContainerIteratorMutex.Lock();
            } // iterator destruction
            pxParams->m_xContainerIteratorMutex.Unlock();
#endif //#ifdef ASURA_USE_CONTAINER_SYSTEM

		} // end for each waypoint in the current bin

        // Move on to the next available bin.
        pxParams->m_xFinishedWaypointCountMutex.Lock();
        pxParams->m_uFinishedWaypointCount += s_aiNumWaypoints[ uBin ];
        pxParams->m_xFinishedWaypointCountMutex.Unlock();

        pxParams->m_xCurrentBinMutex.Lock();
        uBin = pxParams->m_uCurrentBin++;
        pxParams->m_xCurrentBinMutex.Unlock();
    };

    // If we are the last thread to finish then set the 'finished' event.
    pxParams->m_xThreadFinishedCountMutex.Lock();
    if ( ++pxParams->m_uThreadFinishedCount == pxParams->m_uThreadCount )
    {
        pxParams->m_xFinishedEvent.SetSignal();
    }
    pxParams->m_xThreadFinishedCountMutex.Unlock();
}

bool Tools_Mode_Waypointing::CalculateWaypointVolumes()
{
	//ASURA_CPU_PROFILE_SCOPED_TIMER("Tools_Mode_Waypointing::CalculateWaypointVolumes");

    const int iTotalWaypoints = GetTotalBinWaypointCount();
    if ( UpdateProgress( 0, iTotalWaypoints, "Calculating waypoint volumes..." ) )
    {
        return false;
    }

    const u_int uNumThreadsToCreate = Asura_Thread_System::GetNumHardwareThreads();
    CalcWPVolumesInBin_ThreadParams xThreadParams( uNumThreadsToCreate );

    // Create the threads to do the work.
    for ( u_int uThread = 0; uThread < uNumThreadsToCreate; ++uThread )
    {
        Asura_Thread_System::CreateThread( "Calc WP Volumes Thread", s_pfnWaypointCalcThreadFn, static_cast< const void* >( &xThreadParams ) );
    }

    // Keep the progress display up to date.
    bool bCancelled = false;
    do
    {
        // We must put a delay in here, otherwise we'll use loads of processor resources just checking to see if we've finished.
        Asura_Thread_System::DelayCurrentThread( 0.5f, false );

        if ( !bCancelled )
        {
            if ( UpdateProgress( static_cast< int >( xThreadParams.m_uFinishedWaypointCount ), iTotalWaypoints, "Calculating waypoint volumes..." ) )
            {
                xThreadParams.m_xCancelEvent.SetSignal();
                bCancelled = true;
            }
        }
        else
        {
            UpdateProgress( 0, iTotalWaypoints, "Cancelling calculating waypoint volumes..." );
        }
    } while ( !xThreadParams.m_xFinishedEvent.IsSignalled() );

    return !bCancelled;
}

bool Tools_Mode_Waypointing::CullWaypointsBySize()
{
	int iStartWP = Asura_AI_Navigation_System::GetNumUserDefinedWaypoints();
    for ( int iBin = 0; iBin < NUM_WAYPOINT_BINS; ++iBin )
    {
        if ( iStartWP < s_aiNumWaypoints[ iBin ] )
        {
            for ( int iWaypointIndex = iStartWP; iWaypointIndex < s_aiNumWaypoints[ iBin ]; )
            {
                // cull if the waypoint is now too small
                if ( RadiusIsLessThanMinRadius( s_apxWaypointStorage[ iBin ][ iWaypointIndex ].m_iFlags,
                                                s_apxWaypointStorage[ iBin ][ iWaypointIndex ].m_fRadius ) )
		        {
                    s_apxWaypointStorage[ iBin ][ iWaypointIndex ] = s_apxWaypointStorage[ iBin ][ --s_aiNumWaypoints[ iBin ] ];
		        }
		        else
		        {
			        ++iWaypointIndex;
		        }
            }

            iStartWP = 0;
        }
        else
        {
            iStartWP -= s_aiNumWaypoints[ iBin ];
        }
    }

    return SomeBinWaypointsExist();
}

static int NavVertexSorter( const void* cmp1, const void* cmp2 )
{
    if ( ( ( const Tools_Nav_Vertex* )cmp1 )->m_fRadius < ( ( const Tools_Nav_Vertex* )cmp2 )->m_fRadius )
    {
        return  1;
    }
    if ( ( ( const Tools_Nav_Vertex* )cmp1 )->m_fRadius > ( ( const Tools_Nav_Vertex* )cmp2 )->m_fRadius )
    {
        return -1;
    }
    return 0;
}

bool Tools_Mode_Waypointing::SortWaypointsPerBin()
{
    // Sort the waypoints so they are largest first (not including the user defined waypoints).

    Asura_Char acProgressString[ 128 ];

    int iStartWP = Asura_AI_Navigation_System::GetNumUserDefinedWaypoints();
    for ( int iBin = 0; iBin < NUM_WAYPOINT_BINS; ++iBin )
    {
        Asura_PrintfToBfr( acProgressString, "Sorting waypoints ( %d / %d )...", 128, true, iBin, NUM_WAYPOINT_BINS );
        UpdateProgress( iBin, NUM_WAYPOINT_BINS, acProgressString );

        if ( iStartWP < s_aiNumWaypoints[ iBin ] )
        {
            qsort( s_apxWaypointStorage[ iBin ] + iStartWP, s_aiNumWaypoints[ iBin ] - iStartWP, sizeof( Tools_Nav_Vertex ), NavVertexSorter );
            iStartWP = 0;
        }
        else
        {
            iStartWP -= s_aiNumWaypoints[ iBin ];
        }
    }

    return true;
}

bool Tools_Mode_Waypointing::CullRedundantWaypoints()
{
	//ASURA_CPU_PROFILE_SCOPED_TIMER("Tools_Mode_Waypointing::CullRedundantWaypoints");

    // Create an AABB tree with all of the waypoint bounding boxes.
    // Note that these don't have to be inserted in order, as the tree create function will sort them.
    Tools_Large_AABB_Tree* pxTree;
    {
    const int iTotalWaypointCount = GetTotalBinWaypointCount();
    Asura_Bounding_Box* pxBoundingBoxes = new Asura_Bounding_Box[ iTotalWaypointCount ];
    if ( !pxBoundingBoxes ) { return false; }

    int iBoxIndex = 0;
    for ( int iBin = 0; iBin < NUM_WAYPOINT_BINS; ++iBin )
    {
        if ( UpdateProgress( iBoxIndex, iTotalWaypointCount, "Preparing to cull redundant waypoints..." ) )
        {
            delete [] pxBoundingBoxes;
            return false;
        }

        for ( int i = 0; i < s_aiNumWaypoints[ iBin ]; ++i, ++iBoxIndex )
        {
            pxBoundingBoxes[ iBoxIndex ].Reset();
            pxBoundingBoxes[ iBoxIndex ].Set( s_apxWaypointStorage[ iBin ][ i ].m_xPosition );
        }
    }

    pxTree = Tools_Large_AABB_Tree_Creator::Create( iTotalWaypointCount, pxBoundingBoxes );
    delete [] pxBoundingBoxes;
    if ( !pxTree ) { return false; }
    }

    // Test which waypoint positions are inside this waypoint's bounding box.
    // We need to do this in order:
    // Firstly - user defined waypoints.
    {
    const int   iTotalWaypointCount      = GetTotalBinWaypointCount();
    const u_int uNumUserDefinedWaypoints = Asura_AI_Navigation_System::GetNumUserDefinedWaypoints();
    for ( u_int u = 0; u < uNumUserDefinedWaypoints; ++u )
    {
        if ( UpdateProgress( u, iTotalWaypointCount, "Culling redundant waypoints..." ) )
        {
            delete pxTree;
            return false;
        }

        int iBinIndex      = 0;
        int iIntraBinIndex = 0;
        if ( !CalculateBinAndIntraBinIndex( u, iBinIndex, iIntraBinIndex ) )
        {
            continue;
        }

        Tools_Nav_Vertex& xTestWaypoint = s_apxWaypointStorage[ iBinIndex ][ iIntraBinIndex ];
        if ( xTestWaypoint.m_fRadius == 0.0f )
        {
            continue;
        }

        Asura_Bounding_Box xTestBB;
        {
            Asura_Vector_3       xWaypointPosition = xTestWaypoint.m_xPosition;
            const float          fWaypointRadius   = xTestWaypoint.m_fRadius * ( 1.0f - s_fWaypointOverlapFraction );
            const Asura_Vector_3 xMin( -fWaypointRadius, -fWaypointRadius, -fWaypointRadius );
            const Asura_Vector_3 xMax(  fWaypointRadius,  2.0f,             fWaypointRadius );

            Asura_Bounding_Box xTempBB;
            xTempBB.Reset();
            xTempBB.Set( xMin );
            xTempBB.Set( xMax );

            Asura_Matrix_3x3 xOrientation;
            Asura_Maths::OrientationFromNormal( xOrientation, xTestWaypoint.m_xNormal );

            xTestBB.FromOBB( xTempBB, xOrientation );
            xTestBB.Displace( xWaypointPosition );
        }

        s_iBinTestIndex = iBinIndex;
        s_uTestIndex    = iIntraBinIndex;
        pxTree->TestTree( xTestBB, AABBTreePerBinSizeTestCallback );

        xTestWaypoint.m_iFlags |= TOOLS_NAVVERTEX_FLAG_ALREADY_VISITED;
    }
    }

    // Next - largest to smallest across all bins simultaneously.
    {
    Tools_Nav_Vertex* apxBinPointers[ NUM_WAYPOINT_BINS ];
    for ( int iBin = 0; iBin < NUM_WAYPOINT_BINS; ++iBin )
    {
        apxBinPointers[ iBin ] = s_apxWaypointStorage[ iBin ];
    }

    int               iRunningIndex       = Asura_AI_Navigation_System::GetNumUserDefinedWaypoints();
    const int         iTotalWaypointCount = GetTotalBinWaypointCount();
    Tools_Nav_Vertex* pxLargestWaypoint   = NULL;
    int               iBinIndex           = 0;
    int               iIntraBinIndex      = 0;
    // NOTE: We mustn't use these bin pointers once GetNextLargestWaypoint returns false, as they will be invalid.
    while ( GetNextLargestWaypoint( apxBinPointers, pxLargestWaypoint, iBinIndex, iIntraBinIndex ) )
    {
        if ( UpdateProgress( iRunningIndex, iTotalWaypointCount, "Culling redundant waypoints..." ) )
        {
            delete pxTree;
            return false;
        }

        if ( pxLargestWaypoint->m_fRadius == 0.0f )
        {
            continue;
        }

        Asura_Bounding_Box xTestBB;
        {
        Asura_Vector_3       xWaypointPosition = pxLargestWaypoint->m_xPosition;
        const float          fWaypointRadius   = pxLargestWaypoint->m_fRadius * ( 1.0f - s_fWaypointOverlapFraction );
        const Asura_Vector_3 xMin( -fWaypointRadius, -fWaypointRadius, -fWaypointRadius );
        const Asura_Vector_3 xMax(  fWaypointRadius,  2.0f,             fWaypointRadius );

        Asura_Bounding_Box xTempBB;
        xTempBB.Reset();
        xTempBB.Set( xMin );
        xTempBB.Set( xMax );

        Asura_Matrix_3x3 xOrientation;
        Asura_Maths::OrientationFromNormal( xOrientation, pxLargestWaypoint->m_xNormal );

        xTestBB.FromOBB( xTempBB, xOrientation );
        xTestBB.Displace( xWaypointPosition );
        }

        s_iBinTestIndex = iBinIndex;
        s_uTestIndex    = iIntraBinIndex;
        pxTree->TestTree( xTestBB, AABBTreePerBinSizeTestCallback );

        pxLargestWaypoint->m_iFlags |= TOOLS_NAVVERTEX_FLAG_ALREADY_VISITED;

        ++iRunningIndex;
    }

    delete pxTree;
    }

    // Remove waypoints that have been flagged for deletion by the previous test callback function.
    for ( int iBin = 0; iBin < NUM_WAYPOINT_BINS; ++iBin )
    {
        for ( int i = 0; i < s_aiNumWaypoints[ iBin ];    )
        {
            if ( s_apxWaypointStorage[ iBin ][ i ].m_fRadius == 0.0f )
            {
                s_apxWaypointStorage[ iBin ][ i ] = s_apxWaypointStorage[ iBin ][ --s_aiNumWaypoints[ iBin ] ];
            }
            else
            {
                ++i;
            }
        }
    }

    return SomeBinWaypointsExist();
}

bool Tools_Mode_Waypointing::AllocateReducedSizeWaypointStorage()
{
    // We should have dramatically less waypoints now, so we should be able to store them all in one large array.
    s_iNumWaypoints     = GetTotalBinWaypointCount();
    s_pxWaypointStorage = new Tools_Nav_Vertex[ s_iNumWaypoints ];
    if ( !s_pxWaypointStorage )
    {
        Asura_Assert( false, "Tools_Mode_Waypointing::AllocateReducedSizeWaypointStorage - could not allocate reduced size waypoint array" );
        return false;
    }
    else
    {
        return true;
    }
}

bool Tools_Mode_Waypointing::CopyWaypointsIntoReducedStorage()
{
    int iWriteIndex = 0;
    for ( int iBin = 0; iBin < NUM_WAYPOINT_BINS; ++iBin )
    {
		if ( UpdateProgress( iBin, NUM_WAYPOINT_BINS, "Copying waypoints to reduced storage..." ) )
		{
			return false;
		}
		else
		{
			for ( int i = 0; i < s_aiNumWaypoints[ iBin ]; ++i )
			{
				s_pxWaypointStorage[ iWriteIndex + i ] = s_apxWaypointStorage[ iBin ][ i ];
			}

            iWriteIndex += s_aiNumWaypoints[ iBin ];
        }
    }

    return true;
}

bool Tools_Mode_Waypointing::ReleaseInitialWaypointStorage()
{
    for ( int iBin = 0; iBin < NUM_WAYPOINT_BINS; ++iBin )
    {
		if ( UpdateProgress( iBin, NUM_WAYPOINT_BINS, "Releasing initial waypoint storage..." ) )
		{
			return false;
		}
		else
		{
            s_aiNumWaypoints[ iBin ] = 0;
            delete [] s_apxWaypointStorage[ iBin ];
            s_apxWaypointStorage[ iBin ] = NULL;
        }
    }

    return true;
}

bool Tools_Mode_Waypointing::CullPhysicallyInvalidWaypoints()
{
	//ASURA_CPU_PROFILE_SCOPED_TIMER("Tools_Mode_Waypointing::CullPhysicallyInvalidWaypoints");

    // Remove waypoints that are not in physically valid positions.
    const int iStartWP = Asura_AI_Navigation_System::GetNumUserDefinedWaypoints();
    for ( int i = iStartWP; i < s_iNumWaypoints;   )
    {
        if ( UpdateProgress( i, s_iNumWaypoints, "Culling physically invalid waypoints..." ) )
        {
            return false;
        }

		if ( !AllowWaypointCull( s_pxWaypointStorage[ i ].m_iFlags ) )
        {
            ++i;
            continue;
        }

		static const Asura_Matrix_3x3	s_xMaximumRotationMatrix(0.707f, 0.000f, 0.707f,  0.000f, 1.000f, 0.000f,  -0.707f, 0.000f, 0.707f);
		const Asura_Bounding_Box&		xCollisionBB	= s_pxStandardPhysicsObject->GetCollisionObject()->GetBoundingBox();
		Asura_Bounding_Box				xMaximumRotationBB;

		xMaximumRotationBB.FromOBB(xCollisionBB, s_xMaximumRotationMatrix);

		Asura_Collision_Box		xCollisionObjectForMaximumRotation(xMaximumRotationBB);

		const Asura_Vector_3&	xNormal	= s_pxWaypointStorage[i].m_xNormal;

        Asura_Vector_3 xFinalPosition;
		Asura_Vector_3			xUpMovement;
		Asura_Matrix_3x3		xOrientation;

        Asura_Maths::OrientationFromNormal(xOrientation, xNormal);

		xUpMovement[0]	= xOrientation.e12 * -s_fPlacedObjectMovementScale;
		xUpMovement[1]	= xOrientation.e22 * -s_fPlacedObjectMovementScale;
		xUpMovement[2]	= xOrientation.e32 * -s_fPlacedObjectMovementScale;

		Asura_Physics_Object	*pxPhysicsObject;

		if ( s_bProneEnabled )
			pxPhysicsObject = s_pxPronePhysicsObject;
		else if ( s_bCrouchEnabled )
			pxPhysicsObject = s_pxCrouchPhysicsObject;
		else
			pxPhysicsObject = s_pxStandardPhysicsObject;

        const bool bOk = PlaceObjectAt( s_pxWaypointStorage[ i ].m_xPosition, xUpMovement,
                                        s_fPlacedObjectAllowedMovement * s_fPlacedObjectAllowedMovement,
                                        xCollisionObjectForMaximumRotation, pxPhysicsObject->GetFlagsOfPolysToIgnore(),
                                        xOrientation, xFinalPosition );

		if ( !bOk )
        {
#ifdef ASURA_USE_STREAMING
            if ( s_pxWaypointStorage[ i ].m_iInterZonePairIndex >= 0 )
            {
                Tools_Timed_Message_System::AddMessage( Tools_Timed_Message_System::COLOUR_ERROR,
                    "Interzone waypoint %d is being deleted for being in a physically invalid location.",
                    s_pxWaypointStorage[ i ].m_iInterZonePairIndex );
            }
#endif
            s_pxWaypointStorage[ i ] = s_pxWaypointStorage[ --s_iNumWaypoints ];
        }
        else
        {
            ++i;
        }
    }

	if ( s_bGenerateTestWaypointsOnly )
	{
		return ( s_iNumWaypoints > 0 );
	}
	else
	{
		// (TI - 14.07.2008) - Added size check here.  As no more waypoint culling is done between here and
		// copying the data into the nav system we may as well return false here to save having to needlessly go through
		// the time consuming job of creating, culling and flagging links.
		return ( s_iNumWaypoints > 0 && s_iNumWaypoints <= USHRT_MAX );
	}
}

// The connections are sorted such that the longest comes *last*.
static int NavLinkSorter( const void* cmp1, const void* cmp2 )
{
    if ( ( ( const Tools_Nav_Link* )cmp1 )->m_fLength < ( ( const Tools_Nav_Link* )cmp2 )->m_fLength )
    {
        return -1;
    }
    if ( ( ( const Tools_Nav_Link* )cmp1 )->m_fLength > ( ( const Tools_Nav_Link* )cmp2 )->m_fLength )
    {
        return  1;
    }
    return 0;
}

bool Tools_Mode_Waypointing::GenerateWaypointLinks()
{
	//ASURA_CPU_PROFILE_SCOPED_TIMER("Tools_Mode_Waypointing::GenerateWaypointLinks");

    for ( int i = 0; i < s_iNumWaypoints; ++i )
    {
        if ( UpdateProgress( i, s_iNumWaypoints, "Creating waypoint links..." ) )
        {
            return false;
        }

        s_pxWaypointStorage[ i ].m_xLinks.SetSize( 0 );

#ifdef ASURA_USE_STREAMING
		if(!s_pxWaypointStorage[i].GetBelongsToThisEnv()) continue;
#endif
   
        for ( int j = 0; j < s_iNumWaypoints; ++j )
        {
            if ( i == j )
            {
                continue;
            }

#ifdef ASURA_USE_STREAMING
			if(!s_pxWaypointStorage[j].GetBelongsToThisEnv()) continue;
#endif

            // Ignore this link if it doesn't meet our length criteria.
            const Asura_Vector_3 xDisp    = s_pxWaypointStorage[ i ].m_xPosition - s_pxWaypointStorage[ j ].m_xPosition;
            const float          fRangeSq = xDisp.MagnitudeSquared();
            if ( fRangeSq < s_fMinLinkLength * s_fMinLinkLength )
            {
                continue;
            }
            if ( fRangeSq > s_fMaxLinkLength * s_fMaxLinkLength )
            {
                continue;
            }

            // Ignore this link if it doesn't meet our waypoint normal angle criteria.
            const float fNormalDot = s_pxWaypointStorage[ i ].m_xNormal * s_pxWaypointStorage[ j ].m_xNormal;
            if ( fNormalDot < s_fMaxDotProductForLinkBetweenWaypointNormals )
            {
                continue;
            }

			Tools_Nav_Link xNavLink;
			xNavLink.m_iIndex  = j;
			xNavLink.m_fLength = Asura_Maths::Sqrt( fRangeSq );
			s_pxWaypointStorage[ i ].m_xLinks += xNavLink;
        }

        // Sort the connections so that furthest is last.
		qsort( s_pxWaypointStorage[ i ].m_xLinks.GetArray(), s_pxWaypointStorage[ i ].m_xLinks.GetSize(), sizeof( Tools_Nav_Link ), NavLinkSorter );
	}
    
    return true;
}

bool Tools_Mode_Waypointing::CullRedundantLinks()
{
	//ASURA_CPU_PROFILE_SCOPED_TIMER("Tools_Mode_Waypointing::CullRedundantLinks");

    for ( int i = 0; i < s_iNumWaypoints; ++i )
    {   
        if ( UpdateProgress( i, s_iNumWaypoints, "Culling redundant links..." ) )
        {
            return false;
        }

        // Test each waypoint connection against all closer waypoint connections, to see if any of them
        // are in the way - if that is the case don't consider this link as a valid connection.
        for ( int iLinkIndex = s_pxWaypointStorage[ i ].GetNumLinks() - 1; iLinkIndex >= 0; --iLinkIndex )
        {
			const int iTargetWaypointIndex = s_pxWaypointStorage[ i ].m_xLinks[ iLinkIndex ].m_iIndex;

            bool bCullLink = false;

			const Asura_Vector_3 xAlpha = s_pxWaypointStorage[ i ].m_xPosition;
			Asura_Vector_3 xBeta = s_pxWaypointStorage[ iTargetWaypointIndex ].m_xPosition - xAlpha;
			xBeta.Normalise();

			for ( int iTestIndex = 0; iTestIndex < iLinkIndex; ++iTestIndex )
			{
				const int iTestWaypointIndex = s_pxWaypointStorage[ i ].m_xLinks[ iTestIndex ].m_iIndex;

				// We adjust the test waypoint radius in order to include links that only overlap by a small amount.
				float fTestRadius = s_pxWaypointStorage[ iTestWaypointIndex ].m_fRadius * ( 1.0f - s_fLinkOverlapFraction );
				if ( fTestRadius < 0.01f )
				{
					fTestRadius = 0.01f;
				}

				if ( Asura_Maths::RayIntersectsSphere( xAlpha, xBeta,
													   s_pxWaypointStorage[ iTestWaypointIndex ].m_xPosition,
													   fTestRadius ) )
				{
					bCullLink = true;
					break;
				}
			}

            if ( bCullLink )
            {
				s_pxWaypointStorage[ i ].m_xLinks.Remove( iLinkIndex, 1 );
            }
        }
    }

    return true;
}

bool Tools_Mode_Waypointing::MakeAllLinksTwoWay()
{
	//ASURA_CPU_PROFILE_SCOPED_TIMER("Tools_Mode_Waypointing::MakeAllLinksTwoWay");

    for ( int i = 0; i < s_iNumWaypoints; ++i )
    {   
        if ( UpdateProgress( i, s_iNumWaypoints, "Making sure initial links are two way..." ) )
        {
            return false;
        }

        for ( int iLinkIndex = 0; iLinkIndex < s_pxWaypointStorage[ i ].GetNumLinks(); ++iLinkIndex )
        {
            bool bTwoWay = false;

            // check to see if the link is one way
            const int iNextWaypointIndex = s_pxWaypointStorage[ i ].m_xLinks[ iLinkIndex ].m_iIndex;
            for ( int iLinkBackIndex = 0; iLinkBackIndex < s_pxWaypointStorage[ iNextWaypointIndex ].GetNumLinks(); ++iLinkBackIndex )
            {
                if ( s_pxWaypointStorage[ iNextWaypointIndex ].m_xLinks[ iLinkBackIndex ].m_iIndex == i )
                {
                    bTwoWay = true;
                    break;
                }
            }

            if ( bTwoWay == false )
            {
				// add a return connection
				Tools_Nav_Link xNavLink;
				xNavLink.m_iIndex  = i;
				xNavLink.m_fLength = s_pxWaypointStorage[ i ].m_xLinks[ iLinkIndex ].m_fLength;
				s_pxWaypointStorage[ iNextWaypointIndex ].m_xLinks += xNavLink;
            }
        }
    }

    return true;
}

bool Tools_Mode_Waypointing::CullAndFlagLinksWithPhysics()
{
	//ASURA_CPU_PROFILE_SCOPED_TIMER("Tools_Mode_Waypointing::CullAndFlagLinksWithPhysics");
	
	// Set the gravity direction...
	if(s_pxStandardPhysicsObject != NULL)
	{
		s_pxStandardPhysicsObject->SetUseOwnGravityDirection(s_bGravityFollowsSurfaceNormals);
	}
	if(s_pxCrouchPhysicsObject != NULL)
	{
		s_pxCrouchPhysicsObject->SetUseOwnGravityDirection(s_bGravityFollowsSurfaceNormals);
	}
	if(s_pxPronePhysicsObject != NULL)
	{
		s_pxPronePhysicsObject->SetUseOwnGravityDirection(s_bGravityFollowsSurfaceNormals);
	}

#ifdef ASURA_USE_CONTAINER_SYSTEM
	Tools_Entity_Container::BeginCullWaypointLinks();
#endif //ASURA_USE_CONTAINER_SYSTEM

    // If we cannot move our physics object along a link then cull it.
    // If a link is a fall or a long fall then flag it as such.
    for ( int i = 0; i < s_iNumWaypoints; ++i )
    {
        if ( UpdateProgress( i, s_iNumWaypoints, "Culling links with physics..." ) )
        {
            return false;
        }

        const Asura_Vector_3& xSourcePosition = s_pxWaypointStorage[ i ].m_xPosition;
		const Asura_Vector_3& xSourceNormal   = s_pxWaypointStorage[ i ].m_xNormal;

        for ( int iLinkIndex = 0; iLinkIndex < s_pxWaypointStorage[ i ].GetNumLinks(); )
        {
            const int             iNextWaypointIndex = s_pxWaypointStorage[ i ].m_xLinks[ iLinkIndex ].m_iIndex;
            const Asura_Vector_3& xTargetPosition    = s_pxWaypointStorage[ iNextWaypointIndex ].m_xPosition;
			const Asura_Vector_3& xTargetNormal	     = s_pxWaypointStorage[ iNextWaypointIndex ].m_xNormal;

            const bool bLinkOk = CullAndFlagLinkWithPhysics( s_pxWaypointStorage[ i ].m_xLinks[ iLinkIndex ].m_uFlags,
                xSourcePosition, xSourceNormal, i, xTargetPosition, xTargetNormal, iNextWaypointIndex );
            if ( bLinkOk )
            {
                ++iLinkIndex;
            }
            else
            {
				s_pxWaypointStorage[ i ].m_xLinks.Remove( iLinkIndex, 1 );
            }
        }
    }

#ifdef ASURA_USE_CONTAINER_SYSTEM
	Tools_Entity_Container::EndCullWaypointLinks();
#endif //ASURA_USE_CONTAINER_SYSTEM

    return true;
}

bool Tools_Mode_Waypointing::CullTestWaypointsWithNoLinks()
{
    if ( !s_bGenerateTestWaypointsOnly )
    {
        return true;
    }

    if ( s_iNumWaypoints == 1 )
    {
        return true;
    }

    for ( int i = 0; i < s_iNumWaypoints; )
    {
        if ( UpdateProgress( i, s_iNumWaypoints, "Culling waypoints with no links..." ) )
        {
            return false;
        }

        if ( s_pxWaypointStorage[ i ].GetNumLinks() == 0 )
        {
			bool	bLinkedTo = false;

            // we have one way links so we need first to remove all connections to this waypoint
            int j;
            for ( j = 0; j < s_iNumWaypoints; ++j )
            {
                if ( j == i )
                {
                    continue;
                }
                
                // work through all the connections to see if any of them connect back to i
                for ( int c = 0; c < s_pxWaypointStorage[ j ].GetNumLinks(); ++c )
                {
                    if ( s_pxWaypointStorage[ j ].m_xLinks[ c ].m_iIndex == i )
                    {
						if ( s_bDontCullLinkedToWaypoints )
						{
							bLinkedTo = true;
						}
						else
						{
							// found a connection into the waypoint we're trying to delete - delete it!
							s_pxWaypointStorage[ j ].m_xLinks.Remove( c, 1 );
						}

                        // this will be the only link to that waypoint from this, so we can end now
                        break;
                    }
                }
            }
            
			if ( !bLinkedTo )
			{
				// update all the connection links
				// (i.e.) remove the ith waypoint from the waypoint list by copying over it with the last
				// waypoint, updating each waypoint's connections as we go
				for ( j = 0; j < s_iNumWaypoints; ++j )
				{
					for ( int c = 0; c < s_pxWaypointStorage[ j ].GetNumLinks(); ++c )
					{
						if ( s_pxWaypointStorage[ j ].m_xLinks[ c ].m_iIndex == s_iNumWaypoints - 1 )
						{
							s_pxWaypointStorage[ j ].m_xLinks[ c ].m_iIndex = i;
						}
					}
				}
				s_pxWaypointStorage[ i ] = s_pxWaypointStorage[ --s_iNumWaypoints ];
	            
				// We now might have a waypoint that just had its one and only connection stripped!
				// Start again!
				i = 0;
			}
			else
			{
				++i;
			}
        }
        else
        {
            ++i;
        }
    }

    return ( s_iNumWaypoints > 0 );
}

bool Tools_Mode_Waypointing::AddInterZoneWaypointLinks()
{
#ifdef ASURA_USE_STREAMING
	//find the waypoints for each interzone waypoint pair , and create a link between them
	for ( u_int u = 0; u < Asura_AI_Navigation_System::GetNumInterZoneWaypointPairs(); ++u )
	{
		int iWaypoint1 = -1;
		int iWaypoint2 = -1;
	    for ( int i = 0; i < s_iNumWaypoints;i++ )
		{
			if(s_pxWaypointStorage[i].m_iInterZonePairIndex == (int) u)
			{
				if(s_pxWaypointStorage[i].GetBelongsToThisEnv())
				{
					iWaypoint1 = i;
				}
				else
				{
					iWaypoint2 = i;
				}
				if(iWaypoint1>=0 && iWaypoint2>=0) break;
			}
		}
	
		if(iWaypoint1>=0 && iWaypoint2>=0)
		{
			Tools_Nav_Vertex* pxWaypoint1 = &s_pxWaypointStorage[iWaypoint1];
			Tools_Nav_Vertex* pxWaypoint2 = &s_pxWaypointStorage[iWaypoint2];

			Asura_Assert(pxWaypoint1->m_xLinks,"Links not allocated");
			Asura_Assert(pxWaypoint2->m_xLinks,"Links not allocated");
			//add links if they don't already exist
			//waypoint1->waypoint2
			bool bFoundLink = false;
			for(int i=0;i<pxWaypoint1->GetNumLinks();i++)
			{
				if(pxWaypoint1->m_xLinks[i].m_iIndex == iWaypoint2)
				{
					bFoundLink = true;
					break;
				}
			}
			if(!bFoundLink)
			{
				Tools_Nav_Link xNavLink;
				xNavLink.m_iIndex  = iWaypoint2;
				xNavLink.m_fLength = (pxWaypoint1->m_xPosition-pxWaypoint2->m_xPosition).Magnitude();
				pxWaypoint1->m_xLinks += xNavLink;
			}
			//waypoint2->waypoint1
			bFoundLink = false;
			for(int i=0;i<pxWaypoint2->GetNumLinks();i++)
			{
				if(pxWaypoint2->m_xLinks[i].m_iIndex == iWaypoint1)
				{
					bFoundLink = true;
					break;
				}
			}
			if(!bFoundLink)
			{
				Tools_Nav_Link xNavLink;
				xNavLink.m_iIndex  = iWaypoint1;
				xNavLink.m_fLength = (pxWaypoint1->m_xPosition-pxWaypoint2->m_xPosition).Magnitude();
				pxWaypoint1->m_xLinks += xNavLink;
			}
		}
		else
		{
       		Tools_Timed_Message_System::AddMessage( Tools_Timed_Message_System::COLOUR_ERROR,
                "Failed to find waypoints for interzone waypoint %d. They may have been culled...", u );
		}
	}
#endif //ASURA_USE_STREAMING

	return true;
}

bool Tools_Mode_Waypointing::CheckCoverpointsAreNearWaypoints()
{
    if ( s_bShouldGenCoverpoints == false || s_bGenerateTestWaypointsOnly )
    {
        return true;
    }

    for ( int i = 0; i < s_iNumCoverpoints; ++i )
    {
        if ( UpdateProgress( i, s_iNumCoverpoints, "Checking coverpoints are near waypoints..." ) )
        {
            return false;
        }

        for ( int j = 0; j < s_iNumWaypoints; ++j )
        {
            // for safety's sake ignore waypoints with no links as these could be culled later
            if ( s_pxWaypointStorage[ j ].GetNumLinks() == 0 )
            {
                continue;
            }

            Asura_Vector_3 xDisp =
                s_pxWaypointStorage[ j ].m_xPosition - s_pxCoverpointStorage[ i ].m_xCentre;
            if ( Asura_Maths::Abs( xDisp.y ) < 0.11f )
            {
                xDisp.y = 0.0f;
                if ( xDisp.Magnitude() < s_pxWaypointStorage[ j ].m_fRadius + s_fWaypointToCoverpointMaxDist )
                {
                    s_pxCoverpointStorage[ i ].m_iNearestWaypoint = j;
                    break;
                }
            }
        }
    }

    return true;
}

bool Tools_Mode_Waypointing::CullRedundantCoverpoints()
{
    if ( s_bShouldGenCoverpoints == false || s_bGenerateTestWaypointsOnly )
    {
        return true;
    }

    for ( int i = 0; i < s_iNumCoverpoints;  )
    {
        if ( UpdateProgress( i, s_iNumCoverpoints, "Culling redundant coverpoints..." ) )
        {
            return false;
        }

        bool bCullWaypoint = false;

        // cull coverpoints that aren't inside a waypoint
        if ( s_pxCoverpointStorage[ i ].m_iNearestWaypoint == -1 )
        {
            bCullWaypoint = true;
        }

        if ( bCullWaypoint )
        {
            s_pxCoverpointStorage[ i ] = s_pxCoverpointStorage[ --s_iNumCoverpoints ];
        }
        else
        {
            ++i;
        }
    }

    return true;
}

bool Tools_Mode_Waypointing::AssignCoverpointsToWaypoints()
{
    if ( s_bShouldGenCoverpoints == false || s_bGenerateTestWaypointsOnly )
    {
        return true;
    }

    // Kevin Lea - 24/06/2002 - this is a potentially awkward problem, so we'll take it in stages...
    //
    // 1. Make bounding box roughly covering waypoint so we can test which modules the waypoint overlaps.
    // 2. Use module visibility data to get list of all visible modules from waypoint.
    // 3. Get all cover points contained in these modules. (Perhaps allow a bit of give here, e.g. if 
    //    radius of cover point intersects a visible module, that's valid too.)
    // 4. LOS test all these cover points to test visibility of original waypoint. Ouch. Likely to be
    //    a slow process. Need to test against multiple points in waypoint to check full coverage, e.g. 
    //    check points along a diameter of the waypoint perpendicular to the line from the cover point
    //    to the waypoint's centre. Reject anything which doesn't get 100% coverage... this allows a get
    //    out quick clause when processing, but limits us from making 'this is slight cover, better than
    //    nothing' decisions in game.
    // 5. Ideally prune the remaining Coverpoints further, but that may not be necessary. One could find
    //    groupings of cover points (e.g. a 'line' of points whose radii physically overlap) and keep only
    //    the nearest/farthest to/from the waypoint. For now we'll just see how much memory we take up 
    //    without an extra pruning stage.

	//ASURA_CPU_PROFILE_SCOPED_TIMER("Tools_Mode_Waypointing::AssignCoverpointsToWaypoints");

    s_bVisibleModules = new bool[ Asura_Module_System::GetNumberOfModules() ];
    if ( s_bVisibleModules == NULL )
    {
        return false;
    }

    for ( int i = 0; i < s_iNumWaypoints; ++i )
    {
        if ( UpdateProgress( i, s_iNumWaypoints, "Assigning coverpoints to waypoints..." ) )
        {
            delete [] s_bVisibleModules;
            return false;
        }

        // for safety's sake ignore waypoints with no links as these could be culled later
        if ( s_pxWaypointStorage[ i ].GetNumLinks() == 0 )
        {
            continue;
        }

        // stage 1
        Asura_Bounding_Box xBB;
        xBB.Reset();

        Asura_Vector_3 xWaypointPosition = s_pxWaypointStorage[ i ].m_xPosition;
        const float    fWaypointRadius   = s_pxWaypointStorage[ i ].m_fRadius;

        xWaypointPosition.x -= fWaypointRadius;
        xWaypointPosition.z -= fWaypointRadius;
        xBB.Set( xWaypointPosition );

        xWaypointPosition.y -= 2.0f;
        xBB.Set( xWaypointPosition );

        xWaypointPosition.x += fWaypointRadius * 2.0f;
        xWaypointPosition.z += fWaypointRadius * 2.0f;
        xBB.Set( xWaypointPosition );

        for ( int j = 0; j < Asura_Module_System::GetNumberOfModules(); ++j )
        {
            s_bVisibleModules[ j ] = false;
        }
		s_bBBInNonExteriorModule = false;

        Asura_Module_System::TestModuleTree( xBB, WaypointVisibilityCheckCallback );

        // stages 2 and 3
        int j;
        for ( j = 0; j < s_iNumCoverpoints; ++j )
        {
            s_pxCoverpointStorage[ j ].m_bValid = true;
            s_uTestIndex = j;
			s_bBBInNonExteriorModule = false;

            xBB.Reset();
            xBB.Set( s_pxCoverpointStorage[ j ].m_xCentre );

            Asura_Module_System::TestModuleTree( xBB, CoverpointVisibilityCheckCallback );
        }

        // stage 4
        int iNumValidCoverpoints = 0;
        for ( j = 0; j < s_iNumCoverpoints; ++j )
        {
            if ( iNumValidCoverpoints >= s_iMaxCoverpointsPerWaypoint )
            {
                for ( int k = j; k < s_iNumCoverpoints; ++k )
                {
                    s_pxCoverpointStorage[ k ].m_bValid = false;
                }
                break;
            }

            if ( s_pxCoverpointStorage[ j ].m_bValid )
            {
                if ( UpdateProgress( i, s_iNumWaypoints, "Assigning coverpoints to waypoints..." ) )
                {
                    delete [] s_bVisibleModules;
                    return false;
                }

                Asura_Vector_3 xOffset =
                    s_pxWaypointStorage[ i ].m_xPosition - s_pxCoverpointStorage[ j ].m_xCentre;
                const float    fDistanceSq = xOffset.MagnitudeSquared();

                if ( fDistanceSq < s_fMaxCoverPointDistanceSq )
                {
                    unsigned short  usLOSResult = LOSExistsFromCoverpointToWaypoint( j, i );

                    if ( usLOSResult == 0 )
                    {
                        s_pxCoverpointStorage[ j ].m_bValid = false;
                    }
                    else
                    {
                        s_pxCoverpointStorage[ j ].m_usFlags |= usLOSResult;
                        ++iNumValidCoverpoints;
                    }
                }
                else
                {
                    s_pxCoverpointStorage[ j ].m_bValid = false;
                }
            }
        }

        if ( iNumValidCoverpoints > 0 )
        {
            s_pxWaypointStorage[ i ].m_iNumCoverpointIndices = iNumValidCoverpoints;
            s_pxWaypointStorage[ i ].m_pxCoverpointIndices   = new unsigned short[ iNumValidCoverpoints ];

            int iCoverpointIndex = 0;
            for ( int j = 0; j < s_iNumCoverpoints; ++j )
            {
                if ( s_pxCoverpointStorage[ j ].m_bValid )
                {
                    s_pxWaypointStorage[ i ].m_pxCoverpointIndices[ iCoverpointIndex++ ] = static_cast< unsigned short >( j );
                }
            }
        }
    }

    delete [] s_bVisibleModules;

    return true;
}

bool Tools_Mode_Waypointing::FindCoverpointExits()
{
    if ( s_bShouldGenCoverpoints == false || s_bGenerateTestWaypointsOnly )
    {
        return true;
    }

    for ( int i = 0; i < s_iNumCoverpoints; ++i )
    {
        if ( UpdateProgress( i, s_iNumCoverpoints, "Finding coverpoint exits..." ) )
        {
            break;      
        }

        // if this is a valid coverpoint find which exit directions are valid
        unsigned char ucShortExits = 0;
        unsigned char ucLongExits  = 0;

		if (s_bCalculateCoverpointExits)
	        FindCoverpointExits( i, ucShortExits, ucLongExits );

        s_pxCoverpointStorage[ i ].m_ucShortExits = ucShortExits;
        s_pxCoverpointStorage[ i ].m_ucLongExits  = ucLongExits;   
    }

    return true;
}

bool Tools_Mode_Waypointing::CopyDataToTestWaypointStorage()
{
    UpdateProgress( 1, 2, "Copying data to test storage..." );

    if ( s_pxTestWaypointStorage != NULL )
    {
        delete [] s_pxTestWaypointStorage;
    }

    s_pxTestWaypointStorage = new Tools_Nav_Vertex[ s_iNumWaypoints ];
    s_iNumTestWaypoints     = s_iNumWaypoints;

    if ( s_pxTestWaypointStorage == NULL )
    {
        s_iNumTestWaypoints = 0;
        return false;
    }

    for ( int i = 0; i < s_iNumWaypoints; ++i )
    {
        s_pxTestWaypointStorage[ i ] = s_pxWaypointStorage[ i ];
    }

    return true;
}

bool Tools_Mode_Waypointing::CopyDataToNavigationSystem()
{
	//ASURA_CPU_PROFILE_SCOPED_TIMER("Tools_Mode_Waypointing::CopyDataToNavigationSystem");

    UpdateProgress( 1, 2, "Copying data to navigation system..." );

    // verify the data will fit before messing with the navigation system
    bool bTooMuchData = false;
    if ( s_iNumWaypoints > USHRT_MAX )
    {
        bTooMuchData = true;
    }
    int i;
    for ( i = 0; i < s_iNumWaypoints && bTooMuchData == false; ++i )
    {
        if ( s_pxWaypointStorage[ i ].GetNumLinks() > UCHAR_MAX )
        {
            bTooMuchData = true;
        }
    }
    if ( bTooMuchData )
    {
		Tools_MessageLog::AddMessage( Tools_MessageLog::WAYPOINTING, "Error", "Too much data generated - aborting!" );
        return false;
    }

	Asura_ResourcePushType(ASURA_RESOURCE_WAYPOINT);
	{
		// data is ok - copy across
		Asura_AI_Navigation_System::ResetForWaypointGeneration();

		Asura_ResourceAllocateArray(Asura_AI_Navigation_System::s_pxWaypointStorage, s_iNumWaypoints);
		Asura_AI_Navigation_System::s_usNumWaypoints     = static_cast< unsigned short >( s_iNumWaypoints );
		Asura_ResourceAllocateArray(Asura_AI_Navigation_System::s_pucWaypointWeights, s_iNumWaypoints);
		memset( Asura_AI_Navigation_System::s_pucWaypointWeights, 0, s_iNumWaypoints * sizeof( unsigned char ) );

		if ( Asura_AI_Navigation_System::s_pxWaypointStorage == NULL )
		{
			Asura_AI_Navigation_System::s_usNumWaypoints = 0;
			Tools_MessageLog::AddMessage( Tools_MessageLog::WAYPOINTING, "Error", "Could not allocate storage - aborting!" );
			Asura_ResourcePopType();
			return false;
		}

		// first count the total number of connections and coverpoint indices
		unsigned int uNumConnections       = 0;
		unsigned int uNumCoverpointIndices = 0;
		for ( i = 0; i < s_iNumWaypoints; ++i )
		{
			uNumConnections       += s_pxWaypointStorage[ i ].GetNumLinks();
			uNumCoverpointIndices += s_pxWaypointStorage[ i ].m_iNumCoverpointIndices;
		}

		// allocate the connection and coverpoint storage
		Asura_ResourceAllocateArray(Asura_AI_Navigation_System::s_pxNavConnectionStorage, uNumConnections);
		Asura_AI_Navigation_System::s_uNumConnections = uNumConnections;

		const bool bSetupCoverpointData = ( s_iNumCoverpoints > 0 );

		if ( bSetupCoverpointData )
		{
			Asura_ResourceAllocateArray(Asura_AI_Navigation_System::s_pxPerVertexCoverpointData, s_iNumWaypoints);

			Asura_ResourceAllocateArray(Asura_AI_Navigation_System::s_pusCoverpointIndexStorage, uNumCoverpointIndices);
			Asura_AI_Navigation_System::s_uNumCoverpointIndices = uNumCoverpointIndices;

			if ( Asura_AI_Navigation_System::s_pxPerVertexCoverpointData == NULL ||
				 ((Asura_AI_Navigation_System::s_pusCoverpointIndexStorage == NULL) && (uNumCoverpointIndices>0)) )
			{
				Tools_MessageLog::AddMessage( Tools_MessageLog::WAYPOINTING, "Error", "Could not allocate storage - aborting!" );
				Asura_ResourcePopType();
				return false;
			}
		}

		// add the waypoints to the system and fix up the connection and coverpoint indices pointers
		Asura_AI_NavConnection* pxConnectionListPtr = Asura_AI_Navigation_System::s_pxNavConnectionStorage;
		unsigned short*         pusCoverpointIdxPtr = Asura_AI_Navigation_System::s_pusCoverpointIndexStorage;
		for ( i = 0; i < s_iNumWaypoints; ++i )
		{
			Asura_AI_NavVertex&               xDstWP   = Asura_AI_Navigation_System::s_pxWaypointStorage[ i ];
			Asura_AI_PerVertexCoverpointData& xDstPVCD = Asura_AI_Navigation_System::s_pxPerVertexCoverpointData[ i ];

			const Tools_Nav_Vertex& xSrc = s_pxWaypointStorage[ i ];

            xDstWP.m_xPosition        = xSrc.m_xPosition;
			xDstWP.SetRadius( xSrc.m_fRadius );
			xDstWP.m_ucNumConnections = static_cast< unsigned char >( xSrc.GetNumLinks() );
            xDstWP.m_usZone           = 0;
			xDstWP.m_usFlags          = static_cast< unsigned short >( xSrc.m_iFlags );
            xDstWP.SetNormal( xSrc.m_xNormal );

			if ( xSrc.GetNumLinks() )
			{
				xDstWP.m_pxConnections = pxConnectionListPtr;

				for ( int c = 0; c < xSrc.GetNumLinks(); ++c )
				{
					pxConnectionListPtr->m_usIndex = static_cast< unsigned short >( xSrc.m_xLinks[ c ].m_iIndex );
					pxConnectionListPtr->SetLength( xSrc.m_xLinks[ c ].m_fLength );
					pxConnectionListPtr->m_usFlags = static_cast< unsigned short >( xSrc.m_xLinks[ c ].m_uFlags );

					++pxConnectionListPtr;
				}
			}
			else
			{
				xDstWP.m_pxConnections = NULL;
			}

			if ( bSetupCoverpointData )
			{
				xDstPVCD.m_usNumCoverpoints = static_cast< unsigned short >( xSrc.m_iNumCoverpointIndices );
				if ( xSrc.m_iNumCoverpointIndices )
				{
					xDstPVCD.m_pusCoverpoints = pusCoverpointIdxPtr;

					for ( int c = 0; c < xSrc.m_iNumCoverpointIndices; ++c )
					{
						*pusCoverpointIdxPtr++ = xSrc.m_pxCoverpointIndices[ c ];
					}
				}
				else
				{
					xDstPVCD.m_pusCoverpoints = NULL;
				}
			}
		}

		if ( bSetupCoverpointData )
		{
			// now copy over the coverpoints
			Asura_ResourceAllocateArray(Asura_AI_Navigation_System::s_pxCoverpointStorage, s_iNumCoverpoints);
			Asura_AI_Navigation_System::s_iNumCoverpoints     = s_iNumCoverpoints;
			for ( i = 0; i < s_iNumCoverpoints; ++i )
			{
				Asura_AI_Coverpoint&        xDst = Asura_AI_Navigation_System::s_pxCoverpointStorage[ i ];
				const Tools_Nav_Coverpoint& xSrc = s_pxCoverpointStorage[ i ];

				xDst.m_xCentre      = xSrc.m_xCentre;
				xDst.m_usFlags      = xSrc.m_usFlags;
				xDst.m_ucShortExits = xSrc.m_ucShortExits;
				xDst.m_ucLongExits  = xSrc.m_ucLongExits;
			}
		}

#ifdef ASURA_USE_STREAMING
		FindInterZoneWaypointsInNavigationSystem();
#endif

		Asura_AI_Navigation_System::InitialiseAStarStorage();

		Asura_AI_Navigation_System::s_bWaypointsValid = true;

		Tools_SaveAsura::SetWaypointingModified();
	}
	Asura_ResourcePopType();

	Tools_Waypoint_Selection::OnWaypointDataChanged();

    return true;
}

#ifdef ASURA_USE_STREAMING
void Tools_Mode_Waypointing::FindInterZoneWaypointsInNavigationSystem()
{
	Asura_AI_Navigation_Handler* pxNavHandler = &Asura_AI_Navigation_System::GetCurrentHandler();

	u_int uNumPairs = pxNavHandler->GetNumInterZoneWaypointPairs();
	if(!uNumPairs) return;
	

	//allocate array in nav system
	Asura_ResourcePushType(ASURA_RESOURCE_WAYPOINT);
	{
		Asura_ResourceAllocateArray(pxNavHandler->m_pxInterZoneWaypoints, uNumPairs);
	}
	Asura_ResourcePopType();

	//Find the nav system waypoint that corresponds to the 'other zone' waypoint in each waypoint pair
	u_int uDestIndex = 0;

	for(u_int u=0;u<uNumPairs;u++)
	{
		const Tools_AI_InterZoneWaypointPair& xPair = Asura_AI_Navigation_System::GetInterZoneWaypointPair(u);

		//determine the destination zone
		Asura_Hash_ID uZoneHash = 0;
		for(Tools_Streaming_Iterator xIt;!xIt.Done();xIt.Next())
		{
			if(Tools_Streaming_System::IsSetToMainZone()) continue;

			if(Asura_Module_System::GetModuleIndexFromPosition(xPair.m_xOtherZonePosition) != -1)
			{
				//Point is within a module within this zone
				uZoneHash = xIt.GetHash();
				break;
			}

		}

		if(!uZoneHash)
		{
        	Tools_Timed_Message_System::AddMessage( Tools_Timed_Message_System::COLOUR_ERROR, "Interzone waypoint %d isn't inside a connected streaming zone.", u );
			continue;
		}
		

		//Find the index of the nav system waypoint
		int iWaypointIndex = -1;

	    for ( int i = 0; i < s_iNumWaypoints;i++ )
		{
			if(s_pxWaypointStorage[i].m_iInterZonePairIndex == (int)u)
			{
				if(!s_pxWaypointStorage[i].GetBelongsToThisEnv())
				{
					iWaypointIndex = i;
					break;
				}
			}
		}

		if(iWaypointIndex < 0) continue;

		pxNavHandler->m_pxWaypointStorage[iWaypointIndex].m_usFlags |= ASURA_AI_NAVVERTEX_FLAG_STREAMINGZONEEXIT;

		pxNavHandler->m_pxInterZoneWaypoints[uDestIndex].m_uWaypointIndex = iWaypointIndex;
		pxNavHandler->m_pxInterZoneWaypoints[uDestIndex].m_uZoneHash = uZoneHash;
		uDestIndex++;
	} 

	pxNavHandler->m_uNumInterZoneWaypoints = uDestIndex;
}
#endif //ASURA_USE_STREAMING

bool Tools_Mode_Waypointing::CullOneWayLinksWithNoReturn()
{
    if (( s_bShouldCalcZoningData == false ) ||
		( s_bShouldCullOneWayLinksWithNoReturn == false ))
    {
        return true;
    }

    for ( int i = 0; i < Asura_AI_Navigation_System::GetNumberOfWaypoints(); ++i )
    {   
        if ( UpdateProgress( 1, 2, "Culling one-way links with no return path (no progress display)..." ) )
        {
            return false;
        }

        for ( int c = 0; c < Asura_AI_Navigation_System::GetWaypoint( i )->m_ucNumConnections; )
        {
            bool bFoundReverseLink = false;

            int iOtherWaypoint = Asura_AI_Navigation_System::GetWaypoint( i )->m_pxConnections[ c ].m_usIndex;

            for ( int iOtherConnections = 0;
                  iOtherConnections < Asura_AI_Navigation_System::GetWaypoint( iOtherWaypoint )->m_ucNumConnections;
                  ++iOtherConnections )
            {
                if ( Asura_AI_Navigation_System::GetWaypoint( iOtherWaypoint )->m_pxConnections[ iOtherConnections ].m_usIndex == i )
                {
                    bFoundReverseLink = true;
                    break;
                }
            }
            
            if ( bFoundReverseLink == false )
            {
                enum { LONG_ROUTE_LENGTH = 65535 };
                unsigned short ausRoute[ LONG_ROUTE_LENGTH ];
                int            iRouteLength;

                Asura_AI_Navigation_System::FindRoute(
                    ausRoute, iRouteLength, LONG_ROUTE_LENGTH,
                    static_cast< unsigned short >( iOtherWaypoint ),
					static_cast< unsigned short >( i ),
                    0, NULL );

                if ( iRouteLength == 0 )
                {
                    // there is no route back - delete this connection
                    s_pxWaypointEditing->DeleteLink( i, iOtherWaypoint, false );

                    // because we've culled a link we may have created a link that has no return
                    // situation elsewhere - start again!
                    i = -1;
                    break;
                }
                else 
                {   
                    ++c;    
                }
            }
            else
            {
                ++c;
            }
        }
    }

    return true;
}


bool Tools_Mode_Waypointing::CalculateWaypointZoneIDs()
{
    // cf Introduction To Algorithms, Cormen, Leiserson, Rivest, Stein, pp 532->533

    if ( s_bShouldCalcZoningData == false )
    {
        return true;
    }

    UpdateProgress( 0, Asura_AI_Navigation_System::GetNumberOfWaypoints(), "Assigning zone IDs to waypoints..." );

    // first set all of the waypoints to have a white zone ID
    int i;
    for ( i = 0; i < Asura_AI_Navigation_System::GetNumberOfWaypoints(); ++i )
    {
        Asura_AI_Navigation_System::GetWaypoint( i )->m_usZone = s_usWhiteZoneID;
    }

    bool bShouldAbort = false;

    // use a breadth first search to flood fill the waypoint graph with a zone id,
    // starting at the first waypoint and continuing from the next unzoned waypoint
    unsigned char ucZone = 0;
    for ( i = 0; i < Asura_AI_Navigation_System::GetNumberOfWaypoints() && bShouldAbort == false; ++i )
    {
        if ( UpdateProgress( i, Asura_AI_Navigation_System::GetNumberOfWaypoints(), "Assigning zone IDs to waypoints..." ) )
        {
            bShouldAbort = true;
        }

        if ( Asura_AI_Navigation_System::GetWaypoint( i )->m_usZone == s_usWhiteZoneID )
        {
            if ( FloodFillWaypointGraph( i, ucZone++ ) == false )
            {
                bShouldAbort = true;
            }
        }
    }

    if ( bShouldAbort )
    {
        // set all the zone IDs to be the same value
        for ( i = 0; i < Asura_AI_Navigation_System::GetNumberOfWaypoints(); ++i )
        {
            Asura_AI_Navigation_System::GetWaypoint( i )->m_usZone = 0;
        }
    }

    return true;
}

bool Tools_Mode_Waypointing::CullWaypointsWithNoLinks()
{
    if ( Asura_AI_Navigation_System::GetNumberOfWaypoints() == 1 )
    {
        return true;
    }

    for ( int i = 0; i < Asura_AI_Navigation_System::GetNumberOfWaypoints(); )
    {
        UpdateProgress( i, Asura_AI_Navigation_System::GetNumberOfWaypoints(), "Culling waypoints with no links..." );

        // if the waypoint has enough links then continue
        if ( ( Asura_AI_Navigation_System::GetWaypoint( i )->m_ucNumConnections > 0 ) ||
			( !AllowWaypointCull( Asura_AI_Navigation_System::GetWaypoint( i )->m_usFlags ) ) )
        {
            ++i;
            continue;
        }

        // if we've calculated zoning information check to see if this waypoint is on its own in a zone
        // if it is on its own then continue
        if ( s_bShouldCalcZoningData )
        {
			// John Treece-Birch - 10/3/2006
			// This now culls any waypoints that are in their own zone and less than the specified minimum size
			if (Asura_AI_Navigation_System::GetWaypoint( i )->GetRadius() > s_fMinLoneWaypointRadius)
			{
				int iNumWPsInZone = 0;
				for ( int iWPIndex = 0; iWPIndex < Asura_AI_Navigation_System::GetNumberOfWaypoints(); ++iWPIndex )
				{
					if ( Asura_AI_Navigation_System::GetWaypoint( iWPIndex )->m_usZone ==
						Asura_AI_Navigation_System::GetWaypoint( i )->m_usZone )
					{
						++iNumWPsInZone;
					}
				}

				Asura_Assert( iNumWPsInZone > 0, "Tools_Mode_Waypointing::CullWaypointsWithNoLinks - waypoint information error" );
				if ( iNumWPsInZone == 1 )
				{
					++i;
					continue;
				}
			}
        }

        // we have one way links so we need first to remove all connections to this waypoint
        int j;
        for ( j = 0; j < Asura_AI_Navigation_System::GetNumberOfWaypoints(); ++j )
        {
            if ( j == i )
            {
                continue;
            }
                            
            // work through all the connections to see if any of them connect back to i
            for ( int c = 0; c < Asura_AI_Navigation_System::GetWaypoint( j )->m_ucNumConnections; ++c )
            {
                if ( Asura_AI_Navigation_System::GetWaypoint( j )->m_pxConnections[ c ].m_usIndex == i )
                {
                    // found a connection into the waypoint we're trying to delete - delete it!
                    s_pxWaypointEditing->DeleteLink( i, j, false );

                    // this will be the only link to that waypoint from this, so we can end now
                    break;
                }
            }
        }
        
        // delete the waypoint
        s_pxWaypointEditing->DeleteWaypointAndLinks( i, false );
        
        // We now might have a waypoint that just had its one and only connection stripped!
        // Start again!
        i = 0;
    }

    return ( Asura_AI_Navigation_System::GetNumberOfWaypoints() > 0 );
}

bool Tools_Mode_Waypointing::CreateNavigationSystemWaypointTree()
{
    UpdateProgress( 1, 2, "Creating navigation system waypoint tree..." );

    Asura_AI_Navigation_System::CreateWaypointTree();

    Tools_SaveAsura::SetWaypointingModified();

    return true;
}

#ifdef ASURA_USE_STREAMING
// If we are using streaming then we don't want the physical objects storing links to block.
// This is because the waypoint information is stored in the separate zones, not in missions where the entities are,
// and so whatever waypoint graph exists when the entities are present will not be the graph that will exist in game.
// In a streaming build UpdateWaypointLinkBlockPass should be getting called from the update loop,
// and this will calculate the links to block for physical objects as new zones are loaded.
bool Tools_Mode_Waypointing::CalculatePhysicalObjectWaypointBlocks()
{
    if ( Asura_AI_Navigation_System::s_usNumWaypoints == 1 )
    {
        return true;
    }

    UpdateProgress( 0, 1, "Calculating physical object waypoint blocks..." );

    // Count how many objects there are first, to keep the progress updated.
    Asura_Hashed_List_It< Asura_ServerEntity_PhysicalObject > xPhyObjectIter( Asura_ServerEntity_PhysicalObject::GetList() );
    u_int uNumObjects = 0;
    while ( !xPhyObjectIter.Done() )
    {
        ++uNumObjects;
        xPhyObjectIter.Next();
    }
    xPhyObjectIter.Restart();

    // Now iterate through all physical objects.
    bool  bLinkBlocksChanged = false;
    u_int uObjectIndex = 0;
    while ( !xPhyObjectIter.Done() )
    {
        if ( UpdateProgress( uObjectIndex, uNumObjects, "Calculating physical object waypoint blocks..." ) )
        {
            break;
        }
        // There's no need to remove any old link blocks as the navigation data is brand new.
        // Make sure the physical object is not storing any links.
        bLinkBlocksChanged |= xPhyObjectIter.GetCurrent()->DiscardWaypointLinkBlockInformation();
        xPhyObjectIter.Next();
    }

    if ( bLinkBlocksChanged )
    {
        Tools_SaveAsura::SetEntityModified();
    }

    return xPhyObjectIter.Done();
}

#else // if ASURA_USE_STREAMING is not defined

bool Tools_Mode_Waypointing::CalculatePhysicalObjectWaypointBlocks()
{
    // (TI - 11.10.2007) - This function now also handles waypoint blocker volumes

    if ( Asura_AI_Navigation_System::GetNumberOfWaypoints() == 1 )
    {
        return true;
    }

    UpdateProgress( 0, 1, "Calculating physical object waypoint blocks..." );

    // create the tools objects for calculating door blocking
	CreateToolsEntities();

    Asura_ServerEntity_PhysicalObject::InitialiseWaypointLinkBlockCalculations();
	Asura_ServerEntity_WaypointBlockerVolume::InitialiseWaypointLinkBlockCalculations();

    // Count how many objects there are first, to keep the progress updated.
    Asura_Hashed_List_It< Asura_ServerEntity_PhysicalObject > xPhyObjectIter( Asura_ServerEntity_PhysicalObject::GetList() );
	Asura_Hashed_List_It< Asura_ServerEntity_WaypointBlockerVolume > xWBVIter( Asura_ServerEntity_WaypointBlockerVolume::GetList() );
    u_int uNumObjects = 0;
    while ( !xPhyObjectIter.Done() )
    {
        ++uNumObjects;
        xPhyObjectIter.Next();
    }
    xPhyObjectIter.Restart();
	while ( !xWBVIter.Done() )
	{
		++uNumObjects;
		xWBVIter.Next();
	}
	xWBVIter.Restart();

    // Now iterate through all physical objects.
    bool  bLinkBlocksChanged = false;
    u_int uObjectIndex = 0;
    while ( !xPhyObjectIter.Done() )
    {
        if ( UpdateProgress( uObjectIndex, uNumObjects, "Calculating physical object waypoint blocks..." ) )
        {
            break;
        }
        // There's no need to remove any old link blocks as the navigation data is brand new.
        // Make sure the physical object is not storing any links.
        bLinkBlocksChanged |= xPhyObjectIter.GetCurrent()->DiscardWaypointLinkBlockInformation();
        // Calculate the new link blocks.
        bLinkBlocksChanged |= xPhyObjectIter.GetCurrent()->CalculateWaypointLinkBlocks();
        // Apply the new link blocks.
        xPhyObjectIter.GetCurrent()->UpdateWaypointLinkBlocks();
        xPhyObjectIter.Next();
		++uObjectIndex;
    }
	while ( !xWBVIter.Done() )
	{
		if ( UpdateProgress( uObjectIndex, uNumObjects, "Calculating waypoint blocker volumes waypoint blocks..." ) )
		{
			break;
		}
		// There's no need to remove any old link blocks as the navigation data is brand new.
		// Make sure the link blocking object is not storing any links.
		bLinkBlocksChanged |= xWBVIter.GetCurrent()->DiscardWaypointLinkBlockInformation();
		// Calculate the new link blocks.
		bLinkBlocksChanged |= xWBVIter.GetCurrent()->CalculateWaypointLinkBlocks();
		// Apply the new link blocks.
		xWBVIter.GetCurrent()->UpdateWaypointLinkBlocks();
		xWBVIter.Next();
		++uObjectIndex;
	}

    Asura_ServerEntity_PhysicalObject::ShutdownWaypointLinkBlockCalculations();
	Asura_ServerEntity_WaypointBlockerVolume::ShutdownWaypointLinkBlockCalculations();


#ifdef ASURA_USE_CONTAINER_SYSTEM
	Asura_ServerEntity_Container::InitialiseWaypointLinkBlockCalculations();
	Asura_Hashed_List_It< Asura_ServerEntity_Container > xContIter( Asura_ServerEntity_Container::GetList() );

	int iObjectIndex = 0;
    u_int uNumContainers = 0;
	for ( ; !xContIter.Done() ; xContIter.Next() )
    {
        ++uNumContainers;
    }

	for ( xContIter.Restart() ; !xContIter.Done() ; xContIter.Next(), ++iObjectIndex )
	{
		if ( UpdateProgress( iObjectIndex, uNumContainers, "Calculating container waypoint blocks..." ) )
		{
			break;
		}

		Asura_ServerEntity_Container* pxServerEntity = xContIter.GetCurrent();

		// There's no need to remove any old link blocks as the navigation data is brand new.
		// Make sure the link blocking object is not storing any links.
		bLinkBlocksChanged |= pxServerEntity->DiscardWaypointLinkBlockInformation();

		// Calculate the new link blocks.
		bLinkBlocksChanged |= pxServerEntity->CalculateWaypointLinkBlocks();

		// Apply the new link blocks.
		pxServerEntity->UpdateWaypointLinkBlocks();
	}
    Asura_ServerEntity_Container::ShutdownWaypointLinkBlockCalculations();
#endif //#ifdef ASURA_USE_CONTAINER_SYSTEM

    if ( bLinkBlocksChanged )
    {
        Tools_SaveAsura::SetEntityModified();
    }

    return xPhyObjectIter.Done();
}
#endif // ASURA_USE_STREAMING

bool Tools_Mode_Waypointing::ResetPhysicsDataAfterWaypointing()
{
    DestroyWaypointingPhysicsObjects();
	RestorePhysicsObjectsStatus();
	ForgetPhysicsObjectsStatus();
    s_bPhysicsDataInitialised = false;

    return true;
}

bool Tools_Mode_Waypointing::ReleaseStorageAndReset( const bool bRenderProgress /*=true*/ )
{
    if ( bRenderProgress )
    {
        UpdateProgress( 1, 2, "Cleaning up..." );
    }

    // release the waypoint storage (including the larger initial waypoint bins, if still allocated)
    for ( int iBin = 0; iBin < NUM_WAYPOINT_BINS; ++iBin )
    {
        delete [] s_apxWaypointStorage[ iBin ];
        s_apxWaypointStorage[ iBin ] = NULL;
        s_aiNumWaypoints[ iBin ]     = 0;
    }
    delete [] s_pxWaypointStorage;
    s_pxWaypointStorage = NULL;
    s_iNumWaypoints     = 0;

    // destroy the waypoint position hash table, if still allocated
    if ( s_bHashStorageAllocated )
    {
        s_bHashStorageAllocated = false;

        for ( int i = 0; i < NUM_HASH_BINS; ++i )
        {
            delete [] s_ppiHashTable[ i ];
            s_ppiHashTable[ i ] = NULL;
        }
        delete [] s_ppiHashTable;
        s_ppiHashTable = NULL;

        delete [] s_piNumHashEntries;
        s_piNumHashEntries = NULL;
    }

    // release the coverpoint storage, if allocated
    if ( s_bShouldGenCoverpoints )
    {
        delete [] s_pxCoverpointStorage;
        s_pxCoverpointStorage = NULL;
        s_iNumCoverpoints     = 0;
    }

    // Reset the physics data, if necessary.
    if ( s_bPhysicsDataInitialised )
    {
        ResetPhysicsDataAfterWaypointing();
    }

	s_bDataInNavSystem = false;

    return true;
}

Asura_Char* Tools_Mode_Waypointing::GetPhaseName( const int iPhase )
{
    switch( iPhase )
    {
        case   0: return "VerifyWaypointingParameters";
		case  10: return "InitialiseAndAllocateStorage";
        case  20: return "InitialisePhysicsDataForWaypointing";

        // Waypointing.
        case  30: return "AddNewWaypoints";
        case  40: return "ReleaseHashTableStorage";
        case  50: return "FindPotentialCoverpoints";
        case  60: return "AllocateCoverpointStorage";
        case  70: return "GenerateCoverpointData";
        case  80: return "CullWaypointsUnderObjectsAndDoors";
		case  85: return "CullWaypointsUnderContainers";
        case  90: return "CalculateWaypointVolumes";
        case 100: return "CullWaypointsBySize";
        case 110: return "SortWaypointsPerBin";
        case 120: return "CullRedundantWaypoints";
        case 130: return "AllocateReducedSizeWaypointStorage";
        case 140: return "CopyWaypointsIntoReducedStorage";
        case 150: return "ReleaseInitialWaypointStorage";
        case 160: return "CullPhysicallyInvalidWaypoints";

        // Linking.
        case 170: return "GenerateWaypointLinks";
        case 180: return "CullRedundantLinks";
        case 190: return "MakeAllLinksTwoWay";
        case 200: return "CullAndFlagLinksWithPhysics";
        case 210:
        {
            if ( s_bGenerateTestWaypointsOnly )
            {
                return "CullTestWaypointsWithNoLinks";
            }
            else
            {
                return NULL;
            }
        }
		case 220: return "AddInterZoneWaypointLinks";

        // Coverpointing.
        case 230: return "CheckCoverpointsAreNearWaypoints";
        case 240: return "CullRedundantCoverpoints";
        case 250: return "AssignCoverpointsToWaypoints";
        case 260: return "FindCoverpointExits";

        // Setting up navigation system.
        case 270:
        {
            if ( s_bGenerateTestWaypointsOnly )
            {
                return "CopyDataToTestWaypointStorage";
            }
            else
            {
                return "CopyDataToNavigationSystem";
            }
        }

        // Zoning.
        case 280: return "CullOneWayLinksWithNoReturn";
        case 290: return "CalculateWaypointZoneIDs";

        // Final steps to creating new nav data.
        case 300: return "CullWaypointsWithNoLinks";
        case 310: return "CreateNavigationSystemWaypointTree";
        case 320: return "ResetPhysicsDataAfterWaypointing";
		case 330: return "CalculatePhysicalObjectWaypointBlocks";

        case 340: return "finish phase";

        default: return(NULL);
    }
}

//////////////////////////////////////////////////////////////////////////////
//
// 
//
//////////////////////////////////////////////////////////////////////////////

void Tools_Mode_Waypointing::HandleKeyboard()
{
    if ( Asura_UserInput::DebouncedKeyPressed( ASURA_KEYID_F5 ) )
    {
        s_bShowWaypoints = !s_bShowWaypoints;
    }

    if ( Asura_UserInput::DebouncedKeyPressed( ASURA_KEYID_F3 ) )
    {
        s_bShowLinks = !s_bShowLinks;
    }
}

void Tools_Mode_Waypointing::ToggleWaypointingOverlay()
{
    s_bWaypointingOverlayOpen = !s_bWaypointingOverlayOpen;

    if ( s_bWaypointingOverlayOpen )
    {
        CreateWaypointingOverlay();
    }
    else
    {
        Asura_Overlay_System::CloseOverlays( TOOLS_OVERLAY_MODE );
    }
}

bool Tools_Mode_Waypointing::WaypointingOverlayIsOn()
{
    return s_bWaypointingOverlayOpen;
}

void Tools_Mode_Waypointing::CreateWaypointingOverlay()
{
    if ( !s_bWaypointingOverlayOpen ) { return; }

    Asura_Overlay* pxNewOverlay = new Asura_Overlay( "Waypointing Mode", false, &s_xWaypointingOverlayPosition );
    Asura_Vector_2 xOffset      = Asura_Vector_2( 1.0f, 3.0f );

    pxNewOverlay->m_uID = TOOLS_OVERLAY_MODE;

    s_pxThis->AddOverlayControls( pxNewOverlay, xOffset );

    Asura_Overlay_System::SubmitNewOverlay( pxNewOverlay, false );
}

void Tools_Mode_Waypointing::TransferFileData( const int iMode )
{
	// load/save *.dat file in the ToolsIniData directory.
	Tools_ModeDataFile xFileWaypointData( s_pszWaypointingModeDataFilename, iMode );
    TransferToolsModeData( iMode, xFileWaypointData );
}

void Tools_Mode_Waypointing::TransferToolsModeData( const int iMode, Tools_ModeDataFile& xFile )
{
    int iVersion = CURRENT_MODE_DATA_VERSION;
    xFile.Transfer( iVersion );

	if ( iVersion == CURRENT_MODE_DATA_VERSION )
	{
		xFile.Transfer( s_xWaypointingOverlayPosition );
		xFile.Transfer( s_bShowFloorFlag );
		xFile.Transfer( s_bShowWaypoints ); 
		xFile.Transfer( s_bShowWaypointNormals );
		xFile.Transfer( s_bShowLinks );
		xFile.Transfer( s_bShowOnlyInZone );
		xFile.Transfer( s_bShowOnlyInRange );
		xFile.Transfer( s_iZoneIDToRender );
		xFile.Transfer( s_fRangeToRender );
		xFile.Transfer( s_bShowCoverpoints );
		xFile.Transfer( s_bShowCoverpointExits );
		xFile.Transfer( s_bShowCoverpointFlags );
		xFile.Transfer( s_bShowCoverpointDistances );
		xFile.Transfer( s_bShowCoverpointGradientColour );
		xFile.Transfer( s_bShowUserDefinedWaypoints );
		xFile.Transfer( s_bShowWaypointBlockerVolumes );
		xFile.Transfer( s_fShowCoverpointDistanceScale );

		// Also, remember if we're rendering the collision mesh or not.
		int iRenderMode = static_cast< int >( Tools_RenderCollisionMesh_System::GetRenderMode() );
		xFile.Transfer( iRenderMode );
		Tools_RenderCollisionMesh_System::SetRenderMode( static_cast< Tools_CollRenderMode >( iRenderMode ) );

		Tools_Waypoint_Selection::TransferToolsModeData( iMode, xFile );
	}
	else if ( iVersion == 4 )
	{
		xFile.Transfer( s_xWaypointingOverlayPosition );
		xFile.Transfer( s_bShowFloorFlag );
		xFile.Transfer( s_bShowWaypoints ); 
		xFile.Transfer( s_bShowWaypointNormals );
		xFile.Transfer( s_bShowLinks );
		xFile.Transfer( s_bShowOnlyInZone );
		xFile.Transfer( s_bShowOnlyInRange );
		xFile.Transfer( s_iZoneIDToRender );
		xFile.Transfer( s_fRangeToRender );
		xFile.Transfer( s_bShowCoverpoints );
		xFile.Transfer( s_bShowCoverpointExits );
		xFile.Transfer( s_bShowCoverpointFlags );
		xFile.Transfer( s_bShowCoverpointDistances );
		xFile.Transfer( s_bShowCoverpointGradientColour );
		xFile.Transfer( s_bShowWaypointBlockerVolumes );
		xFile.Transfer( s_fShowCoverpointDistanceScale );

		// Also, remember if we're rendering the collision mesh or not.
		int iRenderMode = static_cast< int >( Tools_RenderCollisionMesh_System::GetRenderMode() );
		xFile.Transfer( iRenderMode );
		Tools_RenderCollisionMesh_System::SetRenderMode( static_cast< Tools_CollRenderMode >( iRenderMode ) );

		Tools_Waypoint_Selection::TransferToolsModeData( iMode, xFile );
	}
    else if ( iVersion == 3 )
    {
        xFile.Transfer( s_xWaypointingOverlayPosition );
        xFile.Transfer( s_bShowFloorFlag );
        xFile.Transfer( s_bShowWaypoints ); 
        xFile.Transfer( s_bShowWaypointNormals );
        xFile.Transfer( s_bShowLinks );
        xFile.Transfer( s_bShowOnlyInZone );
        xFile.Transfer( s_bShowOnlyInRange );
        xFile.Transfer( s_iZoneIDToRender );
        xFile.Transfer( s_fRangeToRender );
        xFile.Transfer( s_bShowCoverpoints );
        xFile.Transfer( s_bShowCoverpointExits );
        xFile.Transfer( s_bShowCoverpointFlags );
        xFile.Transfer( s_bShowCoverpointDistances );
        xFile.Transfer( s_bShowCoverpointGradientColour );
        xFile.Transfer( s_fShowCoverpointDistanceScale );

        // Also, remember if we're rendering the collision mesh or not.
        int iRenderMode = static_cast< int >( Tools_RenderCollisionMesh_System::GetRenderMode() );
        xFile.Transfer( iRenderMode );
        Tools_RenderCollisionMesh_System::SetRenderMode( static_cast< Tools_CollRenderMode >( iRenderMode ) );

        Tools_Waypoint_Selection::TransferToolsModeData( iMode, xFile );
    }
    else if ( iVersion == 2 )
    {
        xFile.Transfer( s_xWaypointingOverlayPosition );
        bool bDummyBoolean;
        xFile.Transfer( bDummyBoolean );
        xFile.Transfer( s_bShowFloorFlag );
        xFile.Transfer( s_bShowWaypoints ); 
        xFile.Transfer( s_bShowWaypointNormals );
        xFile.Transfer( s_bShowLinks );
        xFile.Transfer( s_bShowOnlyInZone );
        xFile.Transfer( s_bShowOnlyInRange );
        xFile.Transfer( s_iZoneIDToRender );
        xFile.Transfer( s_fRangeToRender );
        xFile.Transfer( s_bShowCoverpoints );
        xFile.Transfer( s_bShowCoverpointExits );
        xFile.Transfer( s_bShowCoverpointFlags );
        xFile.Transfer( s_bShowCoverpointDistances );
        xFile.Transfer( s_bShowCoverpointGradientColour );
        xFile.Transfer( s_fShowCoverpointDistanceScale );

        // Also, remember if we're rendering the collision mesh or not.
        int iRenderMode = static_cast< int >( Tools_RenderCollisionMesh_System::GetRenderMode() );
        xFile.Transfer( iRenderMode );
        Tools_RenderCollisionMesh_System::SetRenderMode( static_cast< Tools_CollRenderMode >( iRenderMode ) );
    }
    else if ( iVersion == 1 )
    {
        xFile.Transfer( s_xWaypointingOverlayPosition );
        bool bDummyBoolean;
        xFile.Transfer( bDummyBoolean );
        xFile.Transfer( s_bShowFloorFlag );
        xFile.Transfer( s_bShowWaypoints ); 
        xFile.Transfer( s_bShowWaypointNormals );
        xFile.Transfer( s_bShowLinks );
        xFile.Transfer( s_bShowOnlyInZone );
        xFile.Transfer( s_bShowOnlyInRange );
        xFile.Transfer( s_iZoneIDToRender );
        xFile.Transfer( s_fRangeToRender );
        xFile.Transfer( s_bShowCoverpoints );
        xFile.Transfer( s_bShowCoverpointExits );
        xFile.Transfer( s_bShowCoverpointFlags );
        xFile.Transfer( s_bShowCoverpointDistances );
        xFile.Transfer( s_bShowCoverpointGradientColour );
        xFile.Transfer( s_fShowCoverpointDistanceScale );
    }
    else if ( iVersion == 0 )
    {
        xFile.Transfer( s_xWaypointingOverlayPosition );
        bool bDummyBoolean;
        xFile.Transfer( bDummyBoolean );
    }
    else
    {
        Asura_Assertf( false, ( "Tools_Mode_Waypointing::TransferToolsModeData - unhandled version (%d)", iVersion ) );
    }
}

void EmptyFunction()
{
}

void Tools_Mode_Waypointing::AddOverlayControls( Asura_Overlay* pxNewOverlay, Asura_Vector_2& xOffset )
{
    Asura_Dialog* pxNewDialogue = NULL;

    pxNewDialogue = new Asura_Function_Dialog( "Generate Waypoints", xOffset, GenerateWaypointsStatic );
    pxNewOverlay->AddDialog( pxNewDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxNewDialogue = new Asura_Function_Dialog( "Verify Waypointing Data", xOffset, VerifyWaypointingDataStatic );
    pxNewOverlay->AddDialog( pxNewDialogue );
    xOffset.y += Asura_Dialog::LargeSpacing();

	Asura_Tabs_Dialog* pxTabsDialogue = s_pxWaypointingTabs = new Asura_Tabs_Dialog(xOffset, s_aszTabNames, sizeof(s_aszTabNames) / sizeof(s_aszTabNames[0]));
    pxTabsDialogue->m_iControlID = s_iTabDialogueControlID;
    pxNewOverlay->AddDialog(pxTabsDialogue);

    // Settings tab stuff.
    pxTabsDialogue->SelectTab( TAB_SETTINGS );
    xOffset.y = pxTabsDialogue->m_xMaxCoord.y + 0.5f;

    m_pxAssignCoverpointsTickbox = new Asura_Tickbox_Dialog( "Assign Coverpoints", xOffset, &s_bShouldGenCoverpoints, &s_bWaypointingSettingsModified );
    pxTabsDialogue->AddDialog( m_pxAssignCoverpointsTickbox );
    xOffset.y += Asura_Dialog::DefaultSpacing();

	pxNewDialogue = new Asura_Tickbox_Dialog( "Calculate Zone Information", xOffset, &s_bShouldCalcZoningData, &s_bWaypointingSettingsModified );
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.y += Asura_Dialog::LargeSpacing();

    pxNewDialogue = new Asura_Dialog( "Min/Max Waypoint Radii", xOffset );
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.x += 20.0f;

    pxNewDialogue = new Asura_Numerical_Dialog( "", xOffset, &s_fMinWaypointRadius, 0.1f, 1000.0f, 0.25f, "%.2f", &s_bWaypointingSettingsModified );
    pxNewDialogue->SetHelpText( "The minimum radius that waypoints are allowed to be." );
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.x += 12.0f;

    pxNewDialogue = new Asura_Numerical_Dialog( "", xOffset, &s_fMaxWaypointRadius, 0.1f, 1000.0f, 0.25f, "%.2f", &s_bWaypointingSettingsModified );
    pxNewDialogue->SetHelpText( "The maximum radius that waypoints are allowed to be." );
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.x -= 32.0f;
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxNewDialogue = new Asura_Dialog( "Min/Max Dense Waypoint Radii", xOffset );
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.x += 20.0f;

    pxNewDialogue = new Asura_Numerical_Dialog( "", xOffset, &s_fMinDenseWaypointRadius, 0.1f, 1000.0f, 0.1f, "%.2f", &s_bWaypointingSettingsModified );
    pxNewDialogue->SetHelpText( "The minimum radius that dense waypoints are allowed to be." );
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.x += 12.0f;

	pxNewDialogue = new Asura_Numerical_Dialog( "", xOffset, &s_fMaxDenseWaypointRadius, 0.1f, 1000.0f, 0.1f, "%.2f", &s_bWaypointingSettingsModified );
    pxNewDialogue->SetHelpText( "The maximum radius that dense waypoints are allowed to be." );
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.x -= 32.0f;
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxNewDialogue = new Asura_Dialog( "Min/Max Link Length", xOffset );
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.x += 20.0f;

    pxNewDialogue = new Asura_Numerical_Dialog( "", xOffset, &s_fMinLinkLength, 0.25f, 10.0f, 0.25f, "%.2f", &s_bWaypointingSettingsModified );
    pxNewDialogue->SetHelpText( "The minimum length of links allowed." );
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.x += 12.0f;

    pxNewDialogue = new Asura_Numerical_Dialog( "", xOffset, &s_fMaxLinkLength, 1.0f, ASURA_AI_MAX_LINK_LENGTH, 0.25f, "%.2f", &s_bWaypointingSettingsModified );
    pxNewDialogue->SetHelpText( "The maximum length of links allowed." );
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.x -= 32.0f;
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxNewDialogue = new Asura_Dialog( "Max Link Fall Distance", xOffset );
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.x += 20.0f;

    pxNewDialogue = new Asura_Numerical_Dialog( "", xOffset, &s_fMaxLinkFallDistance, 1.0f, ASURA_AI_MAX_LINK_LENGTH, 0.25f, "%.2f", &s_bWaypointingSettingsModified );
    pxNewDialogue->SetHelpText( "The maximum distance actors are allowed to fall when moving along links." );
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.x -= 20.0f;
    xOffset.y += Asura_Dialog::DefaultSpacing();

#ifdef TOOLS_ALLOW_WAYPOINT_DEBUGGING
    pxNewDialogue = new Asura_Dialog( "Max Poly Normal Dot", xOffset );
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.x += 20.0f;

    pxNewDialogue = new Asura_Numerical_Dialog( "", xOffset, &s_fMaxDotProductBetweenWaypointPolygonNormals, -1.0f, 1.0f, 0.100f, "%.3f" );
    pxNewDialogue->SetHelpText( "Polys whose dot-product is less than this will act to constrict waypoint volumes." );
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.x -= 20.0f;
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxNewDialogue = new Asura_Dialog( "Max Link Normal Dot", xOffset );
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.x += 20.0f;

    pxNewDialogue = new Asura_Numerical_Dialog( "", xOffset, &s_fMaxDotProductForLinkBetweenWaypointNormals, -1.0f, 1.0f, 0.100f, "%.3f" );
    pxNewDialogue->SetHelpText( "Links to a poly whose dot-product is less than this will be culled." );
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.x -= 20.0f;
    xOffset.y += Asura_Dialog::DefaultSpacing();
#endif


    // Rendering tab stuff.
    pxTabsDialogue->SelectTab( TAB_RENDERING );
    xOffset.y = pxTabsDialogue->m_xMaxCoord.y + 0.5f;

    pxNewDialogue = new Asura_Tickbox_Dialog( "Show Flag", xOffset, &s_bShowFloorFlag );
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxNewDialogue = new Asura_Tickbox_Dialog( "Show Static Objects, Containers And Doors", xOffset, &s_bShowStaticObjects );
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

	pxNewDialogue = new Asura_Tickbox_Dialog( "Show User Defined Waypoints", xOffset, &s_bShowUserDefinedWaypoints );
	static_cast< Asura_Tickbox_Dialog* >( pxNewDialogue )->SetClickedFunction( ShowUDWsChanged );
	pxTabsDialogue->AddDialog( pxNewDialogue );
	xOffset.y += Asura_Dialog::DefaultSpacing();

    pxNewDialogue = new Asura_Tickbox_Dialog( "Show Waypoint Blocker Volumes", xOffset, &s_bShowWaypointBlockerVolumes );
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.y += Asura_Dialog::LargeSpacing();

    pxNewDialogue = new Asura_Tickbox_Dialog( "Show Waypoints", xOffset, &s_bShowWaypoints );
    pxTabsDialogue->AddDialog( pxNewDialogue );
	xOffset.y += Asura_Dialog::DefaultSpacing();

    pxNewDialogue = new Asura_Tickbox_Dialog( "Show Links", xOffset, &s_bShowLinks );
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxNewDialogue = new Asura_Tickbox_Dialog( "Show Normals", xOffset, &s_bShowWaypointNormals );
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

    xOffset.x += 5.0f;
    pxNewDialogue = new Asura_Tickbox_Dialog( "In Zone", xOffset, &s_bShowOnlyInZone );
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.x += 15.0f;

    pxNewDialogue = new Asura_Numerical_Dialog( "", xOffset, &s_iZoneIDToRender, 0, ASURA_NAVVERTEX_MAX_ZONE_ID, 1, "%d" );
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.x -= 15.0f;
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxNewDialogue = new Asura_Tickbox_Dialog( "Within Range", xOffset, &s_bShowOnlyInRange );
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.x += 15.0f;

    pxNewDialogue = new Asura_Numerical_Dialog( "", xOffset, &s_fRangeToRender, 0.0f, 10000.0f, 1.0f, "%0.1f" );
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.x -= 15.0f;
    xOffset.y += Asura_Dialog::LargeSpacing();
    xOffset.x -= 5.0f;

    pxNewDialogue = new Asura_Tickbox_Dialog( "Show Coverpoints", xOffset, &s_bShowCoverpoints );
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxNewDialogue = new Asura_Tickbox_Dialog( "Show Coverpoint Exits", xOffset, &s_bShowCoverpointExits );
    pxNewDialogue->SetEnabled( false );
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxNewDialogue = new Asura_Tickbox_Dialog( "Show Coverpoint Flags", xOffset, &s_bShowCoverpointFlags );
    pxNewDialogue->SetEnabled( false );
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxNewDialogue = new Asura_Tickbox_Dialog( "Show Distances", xOffset, &s_bShowCoverpointDistances );
    pxNewDialogue->SetEnabled( false );
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.x += 20.0f;

    pxNewDialogue = new Asura_Tickbox_Dialog( "Use Gradient Colour", xOffset, &s_bShowCoverpointGradientColour );
    pxNewDialogue->SetEnabled( false );
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.x -= 20.0f;
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxNewDialogue = new Asura_Numerical_Dialog( "Distance Scale", xOffset, &s_fShowCoverpointDistanceScale, 1.0f, 512.0f, 1.0f, "%.1f" );
    pxNewDialogue->SetEnabled( false );
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();


    // Selection tab stuff.
    pxTabsDialogue->SelectTab( TAB_SELECTION );
    xOffset.y = pxTabsDialogue->m_xMaxCoord.y + 0.5f;

    Tools_Waypoint_Selection::AddOverlayItemsToTabsDialogue( pxTabsDialogue, xOffset );

    // Analysis tab stuff.
    pxTabsDialogue->SelectTab( TAB_ANALYSIS );
    xOffset.y = pxTabsDialogue->m_xMaxCoord.y + 0.5f;

    pxNewDialogue = new Asura_Function_Dialog( "Find Waypoint By Index", xOffset, FindWaypointByIndex );
    pxTabsDialogue->AddDialog(pxNewDialogue);
    xOffset.y += Asura_Dialog::DefaultSpacing();

    s_pxWaypointAnalysis->AddOverlayItemsToTabsDialogue( pxTabsDialogue, xOffset );


    // Editing tab stuff.
    pxTabsDialogue->SelectTab( TAB_EDITING );
    xOffset.y = pxTabsDialogue->m_xMaxCoord.y + 0.5f;

    pxNewDialogue = new Asura_Function_Dialog( "Create User Defined Waypoint", xOffset, CreateUserWaypoint );
    pxNewDialogue->SetHelpText( "This defines a position/orientation that will always try to have a waypoint generated at it.\n"
                                "Use this for getting decent links through tight doorways, etc." );
    pxTabsDialogue->AddDialog( pxNewDialogue );
	xOffset.y += Asura_Dialog::DefaultSpacing();

#ifdef ASURA_USE_STREAMING
    pxNewDialogue = new Asura_Function_Dialog( "Create Interzone Waypoint Pair", xOffset, Tools_InterZoneWaypointObject::CreateNew );
    pxNewDialogue->SetHelpText( "Creates a new pair of waypoints for linking this zone to a connected zone." );
    pxTabsDialogue->AddDialog( pxNewDialogue );
	xOffset.y += Asura_Dialog::DefaultSpacing();
#endif

    s_pxWaypointEditing->AddOverlayItemsToTabsDialogue( pxTabsDialogue, xOffset );


    // Testing tab stuff.
    pxTabsDialogue->SelectTab( TAB_TESTING );
    xOffset.y = pxTabsDialogue->m_xMaxCoord.y + 0.5f;

	pxNewDialogue = new Asura_Function_Dialog( "Generate Test Waypoints From Selected Polys", xOffset, GenerateTestWaypointsStatic );
	pxNewDialogue->SetIsEnabledFunction(CanGenerateTestWaypointsStatic);
	pxTabsDialogue->AddDialog( pxNewDialogue );
	xOffset.y += Asura_Dialog::DefaultSpacing();

	pxNewDialogue = new Asura_Function_Dialog( "Delete Test Waypoints", xOffset, DeleteTestWaypoints );
	pxNewDialogue->SetIsEnabledFunction(HasTestWaypointsStatic);
	pxTabsDialogue->AddDialog( pxNewDialogue );
	xOffset.y += Asura_Dialog::LargeSpacing();

	pxNewDialogue = new Asura_Tickbox_Dialog( "Show Test Waypoints", xOffset, &s_bShowTestWaypoints );
	pxNewDialogue->SetIsEnabledFunction(HasTestWaypointsStatic);
	pxTabsDialogue->AddDialog( pxNewDialogue );
	xOffset.y += Asura_Dialog::DefaultSpacing();

	pxNewDialogue = new Asura_Tickbox_Dialog( "Show Test Links", xOffset, &s_bShowTestLinks );
	pxNewDialogue->SetIsEnabledFunction(HasTestWaypointsStatic);
	pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.y += Asura_Dialog::LargeSpacing();


	pxNewDialogue = new Asura_Divider_Dialog( xOffset );
	pxTabsDialogue->AddDialog( pxNewDialogue );
	xOffset.y += Asura_Dialog::DefaultSpacing();

    pxNewDialogue = new Asura_Function_Dialog( "Pre Generate Waypoints", xOffset, PreGenerateWaypointsIncrementalStatic );
	pxNewDialogue->SetIsEnabledFunction(CanPreGenerateWaypointsIncrementalStatic);
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxNewDialogue = new Asura_Function_Dialog( "Generate Waypoints Incremental", xOffset, GenerateWaypointsIncrementalStatic );
	pxNewDialogue->SetIsEnabledFunction(CanGenerateWaypointsIncrementalStatic);
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxNewDialogue = new Asura_Function_Dialog( "Post Generate Waypoints", xOffset, PostGenerateWaypointsIncrementalStatic );
	pxNewDialogue->SetIsEnabledFunction(CanPostGenerateWaypointsIncrementalStatic);
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.y += Asura_Dialog::LargeSpacing();


    pxNewDialogue = new Asura_Tickbox_Dialog( "Show Intermediate Waypoints", xOffset, &s_bShowIntermediateWaypoints );
	pxNewDialogue->SetIsEnabledFunction(HasIntermediateWaypointsStatic);
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxNewDialogue = new Asura_Numerical_Dialog( "Num Waypoints In Batch", xOffset, &s_iNumIntermediateWaypointsInBatch, 0, 100000, 5, "%d" );
	pxNewDialogue->SetIsEnabledFunction(HasIntermediateWaypointsStatic);
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxNewDialogue = new Asura_Numerical_Dialog( "Waypoint Batch", xOffset, &s_iIntermediateWaypointBatchIndex, 0, 10000, 1, "%d" );
	pxNewDialogue->SetIsEnabledFunction(HasIntermediateWaypointsStatic);
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxNewDialogue = new Asura_Tickbox_Dialog( "Show Intermediate Links", xOffset, &s_bShowIntermediateLinks );
	pxNewDialogue->SetIsEnabledFunction(HasIntermediateWaypointsStatic);
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

	pxNewDialogue = new Asura_Tickbox_Dialog( "Show Waypoint Info Under Cursor", xOffset, &s_bShowWaypointInfoUnderCursor );
	pxNewDialogue->SetIsEnabledFunction(HasIntermediateWaypointsStatic);
    pxTabsDialogue->AddDialog( pxNewDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

	xOffset.y = pxTabsDialogue->GetMaxCoords().y;

    pxTabsDialogue->SelectTab( TAB_SETTINGS );
}

Tools_Waypoint_Analysis* Tools_Mode_Waypointing::CreateWaypointAnalysis()
{
    return new Tools_Waypoint_Analysis;
}

Tools_Waypoint_Editing* Tools_Mode_Waypointing::CreateWaypointEditing()
{
    return new Tools_Waypoint_Editing;
}

void Tools_Mode_Waypointing::RenderWaypoints() const
{
    extern void Render_Sphere( const Asura_Vector_3& xCentre, float fRadius, int iColour );

    for ( int i = 0; i < Asura_AI_Navigation_System::GetNumberOfWaypoints(); ++i )
    {
        const Asura_AI_NavVertex* pxWP = Asura_AI_Navigation_System::GetWaypoint( i );

        bool bShouldRenderWP = true;
        if ( s_bShowOnlyInZone )
        {
            bShouldRenderWP &= ( pxWP->m_usZone == s_iZoneIDToRender );
        }
        if ( s_bShowOnlyInRange )
        {
            bShouldRenderWP &= ( ( pxWP->m_xPosition - Asura_Camera::GetCameraData( 0 ).m_xPosition ).MagnitudeSquared() < s_fRangeToRender * s_fRangeToRender );
        }

        if ( bShouldRenderWP )
        {
			int iColour = 0x1fffffff;
			GetWaypointColour( pxWP->m_usFlags, iColour );
			Render_Sphere( pxWP->m_xPosition, pxWP->GetRadius(), iColour );
        }
    }
}

void Tools_Mode_Waypointing::RenderWaypointNormals() const
{
    for ( int i = 0; i < Asura_AI_Navigation_System::GetNumberOfWaypoints(); ++i )
    {
        const Asura_AI_NavVertex* pxWP = Asura_AI_Navigation_System::GetWaypoint( i );

        bool bShouldRenderWP = true;
        if ( s_bShowOnlyInZone )
        {
            bShouldRenderWP &= ( pxWP->m_usZone == s_iZoneIDToRender );
        }
        if ( s_bShowOnlyInRange )
        {
            bShouldRenderWP &= ( ( pxWP->m_xPosition - Asura_Camera::GetCameraData( 0 ).m_xPosition ).MagnitudeSquared() < s_fRangeToRender * s_fRangeToRender );
        }

        if ( bShouldRenderWP )
        {
            Asura_Render::RenderLine( pxWP->m_xPosition, pxWP->m_xPosition + pxWP->GetNormal(), 0xffffffff );
        }
    }
}

void Tools_Mode_Waypointing::RenderCoverpoints()
{
    extern void Render_Sphere( const Asura_Vector_3& xCentre, float fRadius, int iColour );

    for ( int i = 0; i < Asura_AI_Navigation_System::GetNumberOfCoverpoints(); ++i )
    {
        Render_Sphere( Asura_AI_Navigation_System::s_pxCoverpointStorage[ i ].m_xCentre, 0.3f, 0x6f00ffff );
    }

    //if (Asura_AI_Navigation_System::GetNumberOfCoverpoints())
    //{
    //    // (AW - 29.09.2003) - Loop through the coverpoints.
    //    for (int c=0;c<Asura_AI_Navigation_System::GetNumberOfCoverpoints();c++)
    //    {
    //        Asura_Vector_3 xWaypointPosition(0.0f, 0.0f, 0.0f);
    //        bool bFoundSelectedWaypointThatUsesThisCoverpoint = false;
    //        // (AW - 29.09.2003) - Find a waypoint that uses this coverpoint.
    //        if(!s_bRenderAllCoverpoints)
    //        {
    //            for (int w=0;w<Asura_AI_Navigation_System::GetNumberOfWaypoints();w++)
    //            {
    //                if ( !s_pxSelectionArray[ w ].m_bIsSelected ) continue;
    //                for (int c2=0;c2<Asura_AI_Navigation_System::GetNumCoverpointsForWaypoint(w);c2++)
    //                {
    //                    if (Asura_AI_Navigation_System::GetCoverpointData(w)->m_pusCoverpoints[c2] == c)
    //                    {
    //                        bFoundSelectedWaypointThatUsesThisCoverpoint = true;
    //                        xWaypointPosition = Asura_AI_Navigation_System::GetWaypoint(w)->m_xPosition;
    //                        break;
    //                    }
    //                }
    //                if (bFoundSelectedWaypointThatUsesThisCoverpoint) break;
    //            }
    //            if (!bFoundSelectedWaypointThatUsesThisCoverpoint) continue;
    //        }
    //
    //        int iCoverpointIndex = c;
    //        if (Asura_Camera::IsSphereInFrustum(Asura_AI_Navigation_System::GetCoverpoint(iCoverpointIndex)->m_xCentre, 1.0f))
    //        {
    //            const Asura_AI_Coverpoint* pxCoverpoint = Asura_AI_Navigation_System::GetCoverpoint(iCoverpointIndex);
    //            Asura_Vector_3 xCoverpointCentre = pxCoverpoint->m_xCentre;
    //            if(s_bRenderCoverpointFlags)
    //            {
    //                xCoverpointCentre.y -= 2.f;
    //                if(pxCoverpoint->m_usFlags & ASURA_AI_COVERPOINT_FULL)   Render_Sphere(xCoverpointCentre,0.3f, 0x6f00ffff);
    //                xCoverpointCentre.y += .5f;
    //                if(pxCoverpoint->m_usFlags & ASURA_AI_COVERPOINT_HIGH)   Render_Sphere(xCoverpointCentre,0.3f, 0x6f00ffb0);
    //                xCoverpointCentre.y += .5f;
    //                if(pxCoverpoint->m_usFlags & ASURA_AI_COVERPOINT_SEMI)   Render_Sphere(xCoverpointCentre,0.3f, 0x6f00ff80);
    //                xCoverpointCentre.y += .5f;
    //                if(pxCoverpoint->m_usFlags & ASURA_AI_COVERPOINT_LOW)    Render_Sphere(xCoverpointCentre,0.3f, 0x6f00ff40);
    //                xCoverpointCentre.y += .5f;
    //            }
    //            Render_Sphere(xCoverpointCentre,0.3f, 0x6f00ff00);
    //            if(s_bRenderCoverpointExits)
    //            {
    //                RenderCoverpointExits(iCoverpointIndex);
    //            }
    //            /*
    //            Asura_Vector_3 xDirection = xWaypointPosition - xCoverpointCentre;
    //            xDirection.y = 0.0f;
    //            xDirection.Normalise();
    //            Asura_Vector_3 xEndPoint = xCoverpointCentre;
    //            xEndPoint.y -= 0.01f;
    //            xCoverpointCentre.y -= 0.01f;*/
    //        }
    //    }
    //}
}

void Tools_Mode_Waypointing::RenderIntermediateWaypoints()
{
    extern void Render_Sphere( const Asura_Vector_3& position, float radius, const Asura_Vector_3* direction = 0, float angle = 0 );

    if ( SomeBinWaypointsExist() )
    {
        // The waypoint information is still in the waypoint bins.
        int iStartWP     = s_iIntermediateWaypointBatchIndex * s_iNumIntermediateWaypointsInBatch;
        int iRenderCount = 0;
        for ( int iBin = 0; iBin < NUM_WAYPOINT_BINS; ++iBin )
        {
            if ( iStartWP < s_aiNumWaypoints[ iBin ] )
            {
                for ( int i = iStartWP; i < s_aiNumWaypoints[ iBin ] && iRenderCount < s_iNumIntermediateWaypointsInBatch; ++i, ++iRenderCount )
                {
					const Tools_Nav_Vertex&	xWaypoint	= s_apxWaypointStorage[iBin][i];
					const float				fRadius		= xWaypoint.m_fRadius;

                    if ( fRadius > 0.0f )
                    {
						const Asura_Vector_3&	xPosition	= xWaypoint.m_xPosition;
						const Asura_Vector_3&	xNormal		= xWaypoint.m_xNormal;

                        Render_Sphere( xPosition, fRadius );
						Asura_Render::RenderLine(xPosition, (xPosition + xNormal), 0x80FFFFFF);
                    }
                }

                iStartWP = 0;
            }
            else
            {
                iStartWP -= s_aiNumWaypoints[ iBin ];
            }
        }
    }
    else
    {
        // We have copied the waypoint information over into the reduced storage.
        for ( int i = s_iIntermediateWaypointBatchIndex * s_iNumIntermediateWaypointsInBatch;
            i < ( s_iIntermediateWaypointBatchIndex + 1 ) * s_iNumIntermediateWaypointsInBatch && i < s_iNumWaypoints;
            ++i )
        {
			const Tools_Nav_Vertex&	xWaypoint	= s_pxWaypointStorage[i];
			const float				fRadius		= xWaypoint.m_fRadius;

			if ( fRadius > 0.0f )
            {
				const Asura_Vector_3&	xPosition	= xWaypoint.m_xPosition;
				const Asura_Vector_3&	xNormal		= xWaypoint.m_xNormal;

				Render_Sphere( xPosition, fRadius );
				Asura_Render::RenderLine(xPosition, (xPosition + xNormal), 0xFFFFFFFF);
            }
        }
    }
}

void Tools_Mode_Waypointing::RenderIntermediateLinks()
{
    Tools_Nav_Vertex* pxVert;

    for ( int i = 0; i < s_iNumWaypoints; ++i )
    {
        pxVert = &s_pxWaypointStorage[ i ];

        for ( int c = 0; c < pxVert->GetNumLinks(); ++c )
        {
            const int iNextWPIndex = pxVert->m_xLinks[ c ].m_iIndex;

            const Asura_Vector_3& xPos1 = pxVert->m_xPosition;
            const Asura_Vector_3& xPos2 = s_pxWaypointStorage[ iNextWPIndex ].m_xPosition;

            Asura_Render::RenderLine( xPos1, xPos2, 0xffffffff );
        }
    }
}

void Tools_Mode_Waypointing::RenderTestWaypoints() const
{
    extern void Render_Sphere( const Asura_Vector_3& xCentre, float fRadius, int iColour );

    for ( int i = 0; i < s_iNumTestWaypoints; ++i )
    {
		const Tools_Nav_Vertex* pxWP = &s_pxTestWaypointStorage[ i ];

		bool bShouldRenderWP = true;
		if ( s_bShowOnlyInRange )
		{
			bShouldRenderWP &= ( ( pxWP->m_xPosition - Asura_Camera::GetCameraData( 0 ).m_xPosition ).MagnitudeSquared() < s_fRangeToRender * s_fRangeToRender );
		}

		int iColour = 0x1fffffff;
		GetWaypointColour( static_cast< u_short >( pxWP->m_iFlags ), iColour );
        Render_Sphere( pxWP->m_xPosition, pxWP->m_fRadius, iColour );
    }
}

void Tools_Mode_Waypointing::RenderTestLinks() const
{
	for ( int i = 0; i < s_iNumTestWaypoints; ++i )
	{
		const Tools_Nav_Vertex* pxVert = &s_pxTestWaypointStorage[ i ];

		bool bShouldRenderWPLinks = true;
		if ( s_bShowOnlyInRange )
		{
			bShouldRenderWPLinks &= ( ( pxVert->m_xPosition - Asura_Camera::GetCameraData( 0 ).m_xPosition ).MagnitudeSquared() < s_fRangeToRender * s_fRangeToRender );
		}

		if ( !bShouldRenderWPLinks )
		{
			continue;
		}

		for ( int c = 0; c < pxVert->GetNumLinks(); ++c )
		{
			const int iNextWPIndex = pxVert->m_xLinks[ c ].m_iIndex;

			const Asura_Vector_3& xPos1 = pxVert->m_xPosition;
			const Asura_Vector_3& xPos2 = s_pxTestWaypointStorage[ iNextWPIndex ].m_xPosition;

			// check for a link back to this waypoint
			bool bTwoWayLink = false;
			for ( int iNextWPConIdx = 0;
				iNextWPConIdx < s_pxTestWaypointStorage[ iNextWPIndex ].GetNumLinks();
				++iNextWPConIdx )
			{
				if ( s_pxTestWaypointStorage[ iNextWPIndex ].m_xLinks[ iNextWPConIdx ].m_iIndex == i )
				{
					bTwoWayLink = true;
					break;
				}
			}

			const u_short usLinkFlags = static_cast< u_short >( pxVert->m_xLinks[ c ].m_uFlags );
			if ( usLinkFlags == 0 && bTwoWayLink )
			{
				// Standard link colour - white.
				Asura_Render::RenderLine( xPos1, xPos2, 0xffffffff );
			}
			else
			{
				Asura_Vector_3 xStartColour, xEndColour;
				GetLinkColour( pxVert->m_xLinks[ c ].m_uFlags, bTwoWayLink, xStartColour, xEndColour );

				if ( xStartColour == xEndColour )
				{
					unsigned int uLinkColour = 0xff000000;
					uLinkColour |= Asura_Maths::FloatToInt( xStartColour.x * 255.0f ) << 16;
					uLinkColour |= Asura_Maths::FloatToInt( xStartColour.y * 255.0f ) << 8;
					uLinkColour |= Asura_Maths::FloatToInt( xStartColour.z * 255.0f );
					Asura_Render::RenderLine( xPos1, xPos2, uLinkColour );
				}
				else
				{
					RenderLine( xPos1, xPos2, xStartColour, xEndColour );
				}
			}
		}
	}
}

void Tools_Mode_Waypointing::RenderWaypointBlockerVolumes()
{
	Asura_Hashed_List_It< Asura_ServerEntity_WaypointBlockerVolume > xWBVIter( Asura_ServerEntity_WaypointBlockerVolume::GetList() );
	while ( !xWBVIter.Done() )
	{
		Asura_ServerEntity_WaypointBlockerVolume* const pxWBV = xWBVIter.GetCurrent();

		if ( !pxWBV->IsUsingAdvancedVolumes() )
		{
			extern void Render_Sphere( const Asura_Vector_3& position, float radius, int iColour ); 

			// render the radius set using a sphere
			Render_Sphere( pxWBV->GetPosition(), pxWBV->GetSphericalVolumeRadius(), 0x6fff0000 );
		}
		else
		{
			for ( int i = 0; i < pxWBV->GetNumAdvancedVolumes(); i++ )
			{
				const Asura_OrientedVolume* const pxVolume = pxWBV->GetVolume(i);
				if ( !pxVolume )
				{
					continue;
				}

				Asura_Bounding_Box xRenderVolume(  pxVolume->m_xBox  );
				xRenderVolume.Displace( -pxVolume->CalculateCentre() );

				const Asura_Quat xOriQuat = pxVolume->m_xOrient;
				Asura_Matrix_3x3 xOriMat;
				xOriQuat.ToMatrix( xOriMat );

				Tools_Render::RenderCube( xRenderVolume, pxVolume->CalculateCentre(), xOriMat, 0x6fff0000, false );
			}
		}

		xWBVIter.Next();
	}
}

void Tools_Mode_Waypointing::CreateUserWaypoint()
{
    if ( !s_pxThis ) { return; }

	Asura_AI_Navigation_System::UserDefinedWaypoint* pxUDW = Asura_AI_Navigation_System::AddUserDefinedWaypoint();
	if ( !pxUDW ) { return; }

	pxUDW->m_xPosition = *Asura_Camera::GetPosition();
    Tools_Object_System::SelectObject( s_pxThis->CreateUserWaypointToolsObject( pxUDW ) );
    Tools_SaveAsura::SetWaypointingModified();
	Tools_Timed_Message_System::AddMessage( Tools_Timed_Message_System::COLOUR_SUCCESS, "Added user defined waypoint." );
}

void Tools_Mode_Waypointing::CreateUserWaypointToolsObjects()
{
    if ( !s_pxThis ) { return; }

	for ( Asura_LinkList_It< Asura_AI_Navigation_System::UserDefinedWaypoint* > xUDWIter( Asura_AI_Navigation_System::GetUserDefinedWaypointList() );
		!xUDWIter.Done();
		xUDWIter.Next() )
	{
		s_pxThis->CreateUserWaypointToolsObject( *xUDWIter.GetCurrent() );
	}

	ShowUDWsChanged();
}

void Tools_Mode_Waypointing::DestroyUserWaypointToolsObjects()
{
    for ( int i = 0; i < Tools_Object_System::ObjectList.size();    )
    {
        Tools_Object* pxObject = Tools_Object_System::ObjectList[ i ];
        if ( pxObject->Type == TOT_UserDefinedWaypointPosition )
        {
            delete pxObject;
        }
        else
        {
            ++i;
        }
    }
}

void Tools_Mode_Waypointing::CalculateWaypointDataStatistics()
{
    // Calculate the number of zones and coverpoint references in the waypoint data
    s_iNumZones                        = 0;
    s_fCoverpointReferencesPerWaypoint = 0;
    const u_int uNumElements = sizeof( s_usNumWaypointsInZone ) / sizeof( s_usNumWaypointsInZone[ 0 ] );
    Asura_Mem::Set( s_usNumWaypointsInZone, 0, uNumElements * sizeof( unsigned short ) );

    int iNumCoverpointReferences = 0;
    for ( int i = 0; i < Asura_AI_Navigation_System::GetNumberOfWaypoints(); ++i )
    {
        iNumCoverpointReferences += Asura_AI_Navigation_System::GetNumCoverpointsForWaypoint( i );

        const u_short usZone = Asura_AI_Navigation_System::GetWaypoint( i )->m_usZone;

		// Don't include waypoints with the max/invalid zone ID.
		if ( usZone == ASURA_NAVVERTEX_MAX_ZONE_ID )
		{
			continue;
		}

        ++s_usNumWaypointsInZone[ usZone ];
        if ( usZone + 1 > s_iNumZones )
        {
            s_iNumZones = usZone + 1;
        }
    }

    if ( Asura_AI_Navigation_System::GetNumberOfWaypoints() > 0 )
    {
        s_fCoverpointReferencesPerWaypoint = static_cast< float >( iNumCoverpointReferences ) / Asura_AI_Navigation_System::GetNumberOfWaypoints();
    }
}

void Tools_Mode_Waypointing::FindWaypointByIndex()
{
    GetIntNumber_Overlay xOverlay("Enter Index",0,0,USHRT_MAX);
    if (xOverlay.DoModal())
    {
        const int iIndex = xOverlay.GetNumber();
        if ( 0 <= iIndex && iIndex < Asura_AI_Navigation_System::GetNumberOfWaypoints() )
        {
            const Asura_Matrix_3x3* pxCameraOrientation = Asura_Camera::GetOrientation();

            Asura_Vector_3 xPosition = Asura_AI_Navigation_System::GetWaypoint( iIndex )->m_xPosition;
            xPosition.x -= pxCameraOrientation->e13*2.0f;
            xPosition.y -= pxCameraOrientation->e23*2.0f;
            xPosition.z -= pxCameraOrientation->e33*2.0f;

            Tools_Movement::TeleportCameraToPosition( &xPosition, pxCameraOrientation );
        }
    }
}

void Tools_Mode_Waypointing::GetLinkColour( const unsigned int uFlags, const bool bTwoWayLink, Asura_Vector_3& xStartColour, Asura_Vector_3& xEndColour) const
{
    if ( bTwoWayLink )
    {
        if ( uFlags & ASURA_AI_CONNECTION_FLAGS_RESTRICTIVE )
        {
            // 0xff7f7fff
            xStartColour.x = 0.5f;
            xStartColour.y = 0.5f;
            xStartColour.z = 1.0f;
        }
        else if ( uFlags & ASURA_AI_CONNECTION_FLAGS_JUMP )
        {
            // 0xff00ffff
            xStartColour.x = 0.0f;
            xStartColour.y = 1.0f;
            xStartColour.z = 1.0f;
        }
        else if ( uFlags & ASURA_AI_CONNECTION_FLAGS_CROUCH )
        {
            // 0xffff00ff
            xStartColour.x = 1.0f;
            xStartColour.y = 0.0f;
            xStartColour.z = 1.0f;
        }
        else if ( uFlags & ASURA_AI_CONNECTION_FLAGS_PRONE )
        {
            // 0xff770077
            xStartColour.x = 0.5f;
            xStartColour.y = 0.0f;
            xStartColour.z = 0.5f;
        }
        else
        {
            // 0xffffffff
            xStartColour.x = 1.0f;
            xStartColour.y = 1.0f;
            xStartColour.z = 1.0f;
        }

        xEndColour = xStartColour;
    }
    else
    {
        if ( uFlags & ASURA_AI_CONNECTION_FLAGS_FALL )
        {
            // 0xffd2b400
            xStartColour.x = 0.824f;
            xStartColour.y = 0.706f;
            xStartColour.z = 0.0f;
        }
        else if ( uFlags & ASURA_AI_CONNECTION_FLAGS_LONGFALL )
        {
            // 0xffff0000
            xStartColour.x = 1.0f;
            xStartColour.y = 0.0f;
            xStartColour.z = 0.0f;
        }
        else if ( uFlags & ASURA_AI_CONNECTION_FLAGS_JUMP )
        {
            // 0xff00ffff
            xStartColour.x = 0.0f;
            xStartColour.y = 1.0f;
            xStartColour.z = 1.0f;
        }
        else
        {
            // One way link colour.
            // 0xff00ff00
            xStartColour.x = 0.0f;
            xStartColour.y = 1.0f;
            xStartColour.z = 0.0f;
        }

        // If it's a restrictive link then colour it.
        if ( uFlags & ASURA_AI_CONNECTION_FLAGS_RESTRICTIVE )
        {
            // 0xff7f7fff
            xEndColour.x = 0.5f;
            xEndColour.y = 0.5f;
            xEndColour.z = 1.0f;
        }
        else
        {
            // 0xffffffff
            xEndColour.x = 1.0f;
            xEndColour.y = 1.0f;
            xEndColour.z = 1.0f;
        }
    }
}

void Tools_Mode_Waypointing::GetWaypointColour( const unsigned int uFlags, int& iColour ) const
{
    iColour = 0x1fffffff;
}

void Tools_Mode_Waypointing::RenderWaypointInfo( const int iWPIndex, float& fPrintX, float& fPrintY ) const
{
    fPrintY += 1.0f;
    Asura_Text_Output::Print(fPrintX, fPrintY, 0xffffffff, 0, "Highlighted Waypoint Info:");
    fPrintY += 1.0f;
    Asura_Text_Output::Print(fPrintX, fPrintY, 0xffffffff, 0, "Index : %d", iWPIndex);
    fPrintY += 1.0f;
    {
        Asura_Vector_3 xPosition = Asura_AI_Navigation_System::GetWaypoint( iWPIndex )->m_xPosition;
        Asura_Text_Output::Print(fPrintX, fPrintY, 0xffffffff, 0, "Position : %f %f %f", xPosition.x, xPosition.y, xPosition.z);
        fPrintY += 1.0f;
    }
    Asura_Text_Output::Print(fPrintX, fPrintY, 0xffffffff, 0, "Radius : %f", Asura_AI_Navigation_System::GetWaypoint(iWPIndex)->GetRadius());
    fPrintY += 1.0f;
	Asura_Text_Output::Print(fPrintX, fPrintY, 0xffffffff, 0, "Zone ID : %d", Asura_AI_Navigation_System::GetWaypoint(iWPIndex)->m_usZone);
    fPrintY += 1.0f;

    if (Asura_AI_Navigation_System::GetNumCoverpointsForWaypoint(iWPIndex)>0)
    {
        Asura_String xCPIString = "Number of Coverpoints : ";
        char acBuf[16];
        sprintf( acBuf, "%d:- ", Asura_AI_Navigation_System::GetCoverpointData(iWPIndex)->m_usNumCoverpoints);
        xCPIString += acBuf;
        for ( int i = 0; (xCPIString.GetLength()<200) && (i<Asura_AI_Navigation_System::GetCoverpointData(iWPIndex)->m_usNumCoverpoints); i++ )
        {
            sprintf( acBuf, "%d, ",Asura_AI_Navigation_System::GetCoverpointData(iWPIndex)->m_pusCoverpoints[i] );
            xCPIString += acBuf;
        }
        Asura_Text_Output::Print(fPrintX, fPrintY, 0xffffffff, 0, xCPIString.GetString() );
        fPrintY += 1.0f;
    }
    else
    {
        Asura_Text_Output::Print(fPrintX, fPrintY, 0xffffffff, 0, "No Coverpoints" );
        fPrintY += 1.0f;
    }
    Asura_Text_Output::Print(fPrintX, fPrintY, 0xffffffff, 0, "Number of Connections : %d", Asura_AI_Navigation_System::GetWaypoint(iWPIndex)->m_ucNumConnections);
    fPrintY += 1.0f;

    Asura_String xFlagsString;
	if (Asura_AI_Navigation_System::GetWaypoint(iWPIndex)->m_usFlags & (ASURA_AI_NAVVERTEX_FLAG_SLOPE | 
																		ASURA_AI_NAVVERTEX_FLAG_STEPS | 
																		ASURA_AI_NAVVERTEX_FLAG_CEILING |
																		ASURA_AI_NAVVERTEX_FLAG_WALL))
	{
        xFlagsString.MakeBlank();
        if (Asura_AI_Navigation_System::GetWaypoint(iWPIndex)->m_usFlags & ASURA_AI_NAVVERTEX_FLAG_SLOPE)
        {
            xFlagsString += "Slope. ";
        }
        if (Asura_AI_Navigation_System::GetWaypoint(iWPIndex)->m_usFlags & ASURA_AI_NAVVERTEX_FLAG_STEPS)
        {
            xFlagsString += "Steps. ";
        }
        if (Asura_AI_Navigation_System::GetWaypoint(iWPIndex)->m_usFlags & ASURA_AI_NAVVERTEX_FLAG_CEILING)
        {
            xFlagsString += "Ceiling. ";
        }
        if (Asura_AI_Navigation_System::GetWaypoint(iWPIndex)->m_usFlags & ASURA_AI_NAVVERTEX_FLAG_WALL)
        {
            xFlagsString += "Wall. ";
        }
    }
    else
    {
        xFlagsString = "None";
    }
    Asura_Text_Output::Print(fPrintX, fPrintY, 0xffffffff, 0, "Flags : %s", xFlagsString.GetString());
    fPrintY += 1.0f;
}

void Tools_Mode_Waypointing::RenderIntermediateWaypointInfo( float& fPrintX, float& fPrintY ) const
{
    // We only bother checking the waypoints that are within the batch, as defined by our intermediate batch index and the number of waypoints in a batch.
    int iStartWP = s_iIntermediateWaypointBatchIndex * s_iNumIntermediateWaypointsInBatch;

    if ( s_apxWaypointStorage[ 0 ] != NULL )
    {
        int aiWPIndex[ NUM_WAYPOINT_BINS ];
        for ( int iBin = 0; iBin < NUM_WAYPOINT_BINS; ++iBin ) { aiWPIndex[ iBin ] = -1; }

        int iBatchLength = s_iNumIntermediateWaypointsInBatch;

        // Find the best waypoint in the intermediate batch range. We may have to look in more than one bin.
        for ( int iBin = 0, iBinStartIndex = 0; iBin < NUM_WAYPOINT_BINS && iBatchLength > 0; ++iBin )
        {
            if ( iStartWP < s_aiNumWaypoints[ iBin ] )
            {
                const int iBinBatchLength = Asura_Maths::Min( iBatchLength, s_aiNumWaypoints[ iBin ] - iStartWP );
                iBatchLength -= iBinBatchLength;

                if ( GetWaypointIndexUnderMouse( aiWPIndex[ iBin ], &s_apxWaypointStorage[ iBin ][ iStartWP ], iBinBatchLength ) )
                {
                    aiWPIndex[ iBin ] += iBinStartIndex;
                }

                iStartWP = 0;
            }
            else
            {
                iStartWP -= s_aiNumWaypoints[ iBin ];
            }

            iBinStartIndex += s_aiNumWaypoints[ iBin ];
        }

        for ( int iBin = 0; iBin < NUM_WAYPOINT_BINS; ++iBin )
        {
            if ( aiWPIndex[ iBin ] != -1 )
            {
                Asura_Text_Output::Print( fPrintX, fPrintY++, 0xffffffff, 0, "waypoint index : %d", aiWPIndex[ iBin ] );
                Asura_Text_Output::Print( fPrintX, fPrintY++, 0xffffffff, 0, "waypoint pos   : %f %f %f",
                    s_apxWaypointStorage[ iBin ][ aiWPIndex[ iBin ] ].m_xPosition.x,
                    s_apxWaypointStorage[ iBin ][ aiWPIndex[ iBin ] ].m_xPosition.y,
                    s_apxWaypointStorage[ iBin ][ aiWPIndex[ iBin ] ].m_xPosition.z );
            }
        }
    }
    else
    {
        const int iBatchLength = Asura_Maths::Min( s_iNumIntermediateWaypointsInBatch, s_iNumWaypoints - iStartWP );

        int iWaypointIndex;
        if ( GetWaypointIndexUnderMouse( iWaypointIndex, &s_pxWaypointStorage[ iStartWP ], iBatchLength ) )
        {
            iWaypointIndex += iStartWP;

            Asura_Text_Output::Print( fPrintX, fPrintY++, 0xffffffff, 0, "waypoint index : %d", iWaypointIndex );
            Asura_Text_Output::Print( fPrintX, fPrintY++, 0xffffffff, 0, "waypoint pos   : %f %f %f",
                s_pxWaypointStorage[ iWaypointIndex ].m_xPosition.x,
                s_pxWaypointStorage[ iWaypointIndex ].m_xPosition.y,
                s_pxWaypointStorage[ iWaypointIndex ].m_xPosition.z );
        }
    }
}

int Tools_Mode_Waypointing::GetFlagsForNewWaypoint( const int iPolyFlags, const Asura_Vector_3& xPolyNormal ) const
{
	// AJG - 29/03/07 - need to have the "belongs to this environment" flag on for all generated waypoints.
	int iWaypointFlags = TOOLS_NAVVERTEX_FLAG_BELONGS_TO_THIS_ENV;
	if ( iPolyFlags & AsuraPolygonFlag_Step )
	{
		iWaypointFlags |= ASURA_AI_NAVVERTEX_FLAG_STEPS;
	}
	else if ( IsWallPoly( xPolyNormal ) )
	{
		iWaypointFlags |= ASURA_AI_NAVVERTEX_FLAG_WALL;
	}
	else if ( IsCeilingPoly( xPolyNormal ) )
	{
		iWaypointFlags |= ASURA_AI_NAVVERTEX_FLAG_CEILING;
	}
	else if ( IsSlopePoly( xPolyNormal ) )
	{
		iWaypointFlags |= ASURA_AI_NAVVERTEX_FLAG_SLOPE;
	}
	else
	{
		iWaypointFlags |= TOOLS_NAVVERTEX_FLAG_FLOOR;
	}
	if ( iPolyFlags & AsuraPolygonFlag_DenseWaypoints )
	{
		iWaypointFlags |= TOOLS_NAVVERTEX_FLAG_DENSE;
	}
	return iWaypointFlags;
}

bool Tools_Mode_Waypointing::UpdateProgress( const int iNum, const int iDen, const char* pszHeading )
{
    static const char* s_szOldHeading = 0;
    static float s_fStartTime = 0;
    static float s_fLastTime = 0;
    static int   s_iLastNum = 0;
    static float s_fSmoothedTime = 0.0f;

	Asura_Timers::SetMaxFrameRate(0.0f);
    Asura_Timers::HandlePrimaryTimer();

    if (s_szOldHeading!=pszHeading)
    {
        s_szOldHeading = pszHeading;
        s_fLastTime = -1.0f;
        s_fStartTime = Asura_Timers::GetUncappedPhase();
        s_iLastNum = iNum;
        s_fSmoothedTime = 0.0f;
    }

    float percent = ( iDen ? ((float)iNum)/((float)iDen)*100.0f : 0.0f );
    float fTime = Asura_Timers::GetUncappedPhase();

    float fTimeDiff = fTime-s_fLastTime;
    float fNumDiff = static_cast< float >( iNum-s_iLastNum );

    if (fTimeDiff<0.1f)
    {
        return false;
    }
    s_fLastTime = fTime;
    s_iLastNum = iNum;

    Asura_Windows::CheckForWindowsMessages();
    Asura_UserInput::Update();
    Asura_Cursor::OncePerFrameUpdate();

	//Avoid rendering anything with the reference rasterizer , since it will slow down the process massively
	//This can happen if tools is started up while batch processing while the computer is locked.
	//RAR 14/04/2008
	if(!Asura_Direct3D::IsReferenceRasterizer())
	{

	    Asura_Render::BeginFrame();
	    //black out the screen
	    float width = (float)Asura_Viewport::GetWidth();
	    float height = (float)Asura_Viewport::GetHeight();
	    Asura_Render::Overlay.SetRectXY( 0.0f, 0.0f, width, height );
	    Asura_Render::Overlay.SetRectUV( 0.0f, 0.0f, 1.0f, 1.0f );
	    Asura_Render::Overlay.TranslucencyMode = ASURA_TRANSLUCENCY_OFF;
	    Asura_Render::Overlay.TextureIndex = ASURA_TEXTURE_NONE;
	    Asura_Render::Overlay.Colour=0xff000000;
	    Asura_Render::Overlay.Render();
	    Asura_Render::FlushDisplay();

	    //show some text
	    float print_x = width/2;
	    float print_y = height/2;
	    Asura_Text_Output::ConvertPixelsToPrintCoords(print_x,print_y);

	    print_y-=2;
	    if(pszHeading)
	    {
	        Asura_Text_Output::Print(print_x,print_y,0xffffffff,ASURA_TEXT_FORMAT_CENTREJUSTIFIED,"%s",pszHeading);
	    }
	    print_y+=2;
	    Asura_Text_Output::Print(print_x,print_y,0xff00ff00,ASURA_TEXT_FORMAT_CENTREJUSTIFIED,"%3.0f%% Complete",percent);

	    print_y+=4;
	    Asura_Text_Output::Print(print_x,print_y,0xffff0000,ASURA_TEXT_FORMAT_CENTREJUSTIFIED,"Hold escape to abort");

	    if (iNum!=0)
	    {
	        print_y+=2;
	        float fEstimatedTime = ( fNumDiff > 0 ) ? ( ( fTimeDiff / fNumDiff ) * ( iDen - iNum ) ) : 0.0f;
	        // The longer the previous update took the more it should influence our smoothed estimated time.
	        float fSmoothingFactor = Asura_Maths::ClampToRange( fTimeDiff, 0.01f, 1.0f );
	        s_fSmoothedTime += ( fEstimatedTime - s_fSmoothedTime ) * fSmoothingFactor;
	        Asura_Text_Output::Print(print_x,print_y,0xff7f7f7f,ASURA_TEXT_FORMAT_CENTREJUSTIFIED,"Estimated time remaining: %.1fs",s_fSmoothedTime);
	    }

	    Tools_Timed_Message_System::Render();
	    Asura_Cursor::Render();

	    Asura_Render::EndFrame();
	}
	else
	{
		char acTitle[200];
		Asura_PrintfToBfr(acTitle,"%s - %s %3.0f%% Complete",sizeof(acTitle),true,Asura_Windows::GetDefaultWindowTitle(),pszHeading ? pszHeading : "",percent);
		Asura_Windows::SetWindowTitle(acTitle);
	}

    if (Asura_UserInput::KeyPressed(ASURA_KEYID_ESCAPE))
    {
        pszHeading = 0;
		if(Tools_BatchProcessing::IsBatch())
		{
			Tools_BatchProcessing::Abort();
		}
        return true;
    }
    else
    {
        return false;
    }
}

bool Tools_Mode_Waypointing::FindInHashTable( const Asura_Vector_3& xPosition, const Asura_Vector_3& xNormal )
{
    const u_int uHashKey = GetHashTableKey( xPosition, xNormal );

    for ( int i = 0; i < s_piNumHashEntries[ uHashKey ]; ++i )
    {
        const int iBin           = ( ( 0xff000000 & s_ppiHashTable[ uHashKey ][ i ] ) >> 24 );
        const int iWaypointIndex = ( 0x00ffffff & s_ppiHashTable[ uHashKey ][ i ] );

        if ( s_apxWaypointStorage[ iBin ][ iWaypointIndex ].m_xPosition.ApproximatelyEquals( xPosition, 0.001f ) &&
			s_apxWaypointStorage[ iBin ][ iWaypointIndex ].m_xNormal.ApproximatelyEquals( xNormal, 0.001f ) )
        {
            return true;
        }
    }
    return false;
}

bool Tools_Mode_Waypointing::AddToHashTable( const int iBinIndex, const int iWaypointIndex, const Asura_Vector_3& xPosition, const Asura_Vector_3& xNormal )
{
    const u_int uHashKey = GetHashTableKey( xPosition, xNormal );

    s_ppiHashTable[ uHashKey ][ s_piNumHashEntries[ uHashKey ]++ ] = ( ( 0x000000ff & iBinIndex ) << 24 ) | ( 0x00ffffff & iWaypointIndex );

    return ( s_piNumHashEntries[ uHashKey ] < NUM_HASH_BIN_ENTRIES );
}

void Tools_Mode_Waypointing::VerifyWaypointingDataStatic()
{
	Tools_MessageLog::DeleteMessages( Tools_MessageLog::WAYPOINTING, "Verification" );

	s_pxThis->VerifyWaypointingData();

	Tools_MessageLog::AddMessage( Tools_MessageLog::WAYPOINTING, "Verification", "Waypointing Data Verification Completed" );
	Tools_MessageLog::ShowOverlay( Tools_MessageLog::WAYPOINTING, "Verification" );
}

void Tools_Mode_Waypointing::ShowUDWsChanged()
{
	if ( s_bShowUserDefinedWaypoints )
	{
		for ( Asura_Hashed_List_It< Tools_Waypoint_Object > xUDWIter( Tools_Waypoint_Object::GetList() ); !xUDWIter.Done(); xUDWIter.Next() )
		{
			Tools_Waypoint_Object* pxUserWaypoint = xUDWIter.GetCurrent();
			pxUserWaypoint->HiddenFlags &= ~ObjectHideFlag_Mode;
		}
	}
	else
	{
		for ( Asura_Hashed_List_It< Tools_Waypoint_Object > xUDWIter( Tools_Waypoint_Object::GetList() ); !xUDWIter.Done(); xUDWIter.Next() )
		{
			Tools_Waypoint_Object* pxUserWaypoint = xUDWIter.GetCurrent();
			pxUserWaypoint->HiddenFlags |= ObjectHideFlag_Mode;
		}		
	}
}

void Tools_Mode_Waypointing::DeleteTestWaypoints()
{
    if ( s_pxTestWaypointStorage != NULL )
    {
        delete [] s_pxTestWaypointStorage;
    }
    s_pxTestWaypointStorage = NULL;
    s_iNumTestWaypoints     = 0;
}

bool Tools_Mode_Waypointing::AddNewWaypoint( const int             iModuleIndex,
                                             const Asura_Vector_3& xPosition,
											 const Asura_Vector_3& xNormal,
                                             const int             iWaypointFlags )
{
    // If we already have this waypoint then we're not interested.
    if ( FindInHashTable( xPosition, xNormal ) ) { return true; }

    // Add it into a bin.
    for ( int iBin = 0; iBin < NUM_WAYPOINT_BINS; ++iBin )
    {
        int& iNumWPsInBin = s_aiNumWaypoints[ iBin ];

        if ( iNumWPsInBin < NUM_WAYPOINTS_PER_BIN )
        {
            if ( AddToHashTable( iBin, iNumWPsInBin, xPosition, xNormal ) )
            {
                s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_xPosition    = xPosition;
				s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_xNormal		= xNormal;
                s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_iModuleIndex = iModuleIndex;
                s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_iFlags       = iWaypointFlags;
                ++iNumWPsInBin;

                return true;
            }
            else
            {
                return false;
            }
        }
    }

    // Out of bin storage.
    Asura_Assert( false, "Tools_Mode_Waypointing::AddNewWaypoint - run out of waypoint storage" );
    return false;
}

bool Tools_Mode_Waypointing::AddUserDefinedNewWaypoint( const Tools_Waypoint_Object* const pxUserWaypoint )
{
	if ( !pxUserWaypoint ) { return false; }

    for ( int iBin = 0; iBin < NUM_WAYPOINT_BINS; ++iBin )
    {
        int& iNumWPsInBin = s_aiNumWaypoints[ iBin ];

        if ( iNumWPsInBin < NUM_WAYPOINTS_PER_BIN )
        {
			Asura_Matrix_3x3 xOrientM;
			pxUserWaypoint->GetOrientQ().ToMatrix( xOrientM );
            const Asura_Vector_3 xObjectUp = Asura_Vector_3( 0.f, -1.f, 0.f ) * xOrientM;

			s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_uUDWID    = pxUserWaypoint->GetUDW().GetID();
            s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_xPosition = pxUserWaypoint->GetPosition();
			s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_xNormal   = xObjectUp;

            u_int uFlags = pxUserWaypoint->GetFlags() | TOOLS_NAVVERTEX_FLAG_BELONGS_TO_THIS_ENV;
            if ( IsWallPoly( xObjectUp ) )
            {
                uFlags |= ASURA_AI_NAVVERTEX_FLAG_WALL;
            }
            else if ( IsCeilingPoly( xObjectUp ) )
            {
                uFlags |= ASURA_AI_NAVVERTEX_FLAG_CEILING;
            }         
            else if ( IsSlopePoly( xObjectUp ) )
            {
                uFlags |= ASURA_AI_NAVVERTEX_FLAG_SLOPE;
            }   
			else
			{
				uFlags |= TOOLS_NAVVERTEX_FLAG_FLOOR;
			}
            s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_iFlags = uFlags;

            ++iNumWPsInBin;

            return true;
        }
    }

    // Out of bin storage.
    Asura_Assert( false, "Tools_Mode_Waypointing::AddUserDefinedNewWaypoint - run out of waypoint storage" );
    return false;
}

void Tools_Mode_Waypointing::RememberPhysicsObjectsStatus()
{
	for ( Asura_Physics_RigidBodyInstance* pxInstance = Asura_Physics_RigidBodyInstance::GetDynamicList(); pxInstance; pxInstance = pxInstance->GetNextInstance() )
	{
		if ( pxInstance->IsIgnore() )
		{
			s_xRigidBodyIgnoreList.AddToList( pxInstance );
		}
		else
		{
			s_xRigidBodyNonIgnoreList.AddToList( pxInstance );
		}
	}

	for ( Asura_Physics_RigidBodyInstance* pxInstance = Asura_Physics_RigidBodyInstance::GetExternallyControlledList(); pxInstance; pxInstance = pxInstance->GetNextInstance() )
	{
		if ( pxInstance->IsIgnore() )
		{
			s_xRigidBodyIgnoreList.AddToList( pxInstance );
		}
		else
		{
			s_xRigidBodyNonIgnoreList.AddToList( pxInstance );
		}
	}

	for ( Asura_Physics_RigidBodyInstance* pxInstance = Asura_Physics_RigidBodyInstance::GetStaticList(); pxInstance; pxInstance = pxInstance->GetNextInstance() )
	{
		if ( pxInstance->IsIgnore() )
		{
			s_xRigidBodyIgnoreList.AddToList( pxInstance );
		}
		else
		{
			s_xRigidBodyNonIgnoreList.AddToList( pxInstance );
		}
	}
}

void Tools_Mode_Waypointing::SetRequiredPhysicsObjectsAsIgnored()
{
	// Having remembered the physics objects ignore status we now set them to ignore, except the ones set to block nav data generation.
	// This causes those physics objects to block waypoint volumes and also block the waypointing physics objects when linking.
	for ( Asura_Physics_RigidBodyInstance* pxInstance = Asura_Physics_RigidBodyInstance::GetDynamicList(); pxInstance; pxInstance = pxInstance->GetNextInstance() )
	{
		pxInstance->SetIgnore( true );
	}

	for ( Asura_Physics_RigidBodyInstance* pxInstance = Asura_Physics_RigidBodyInstance::GetExternallyControlledList(); pxInstance; pxInstance = pxInstance->GetNextInstance() )
	{
		pxInstance->SetIgnore( true );
	}

	for ( Asura_Physics_RigidBodyInstance* pxInstance = Asura_Physics_RigidBodyInstance::GetStaticList(); pxInstance; pxInstance = pxInstance->GetNextInstance() )
	{
		const Asura_ServerEntity_PhysicalObject* pxPO = Asura_ServerEntity_PhysicalObject::FindEntity( pxInstance->GetAssociatedEntityGuid() );
		if ( pxPO )
		{
			if ( pxPO->HasBlockNavDataGenerationFlag() )
			{
				pxInstance->SetIgnore( false );
			}
			else
			{
				pxInstance->SetIgnore( true );
			}
		}
		else
		{
			pxInstance->SetIgnore( true );
		}
	}
}

bool Tools_Mode_Waypointing::CreateWaypointingPhysicsObjects()
{
    s_pxStandardPhysicsObject = Asura_Physics_Object::Create( ASURA_PHYSICS_OBJECT_ACTOR );
    if ( s_pxStandardPhysicsObject )
    {
        s_pxStandardPhysicsObject->SetLinVelocity( Asura_Maths::ZeroVector3 );
        s_pxStandardPhysicsObject->SetAxisConstraint( Asura_Physics_Object::AXIS_CONSTRAINT_NONE );
        s_pxStandardPhysicsObject->SetFeelsGravity( true );
        s_pxStandardPhysicsObject->SetOnlyCollideWithEnvironmentAndStaticObjects( true );
        s_pxStandardPhysicsObject->SetCollisionObject( &s_xTestCollisionBox );
		s_pxStandardPhysicsObject->SetCollisionProxy( s_eCollisionProxy );
        s_pxStandardPhysicsObject->SetIgnore( true );
        s_pxStandardPhysicsObject->SetReportObjectCollisions( true );
        s_pxStandardPhysicsObject->SetReportEnvironmentCollisions( true );

        u_int uFlagsToIgnore = s_pxStandardPhysicsObject->Tools_GetFlagsOfPolysToIgnore();
        uFlagsToIgnore |= AsuraPolygonFlag_IgnoreWhenWaypointing;
        s_pxStandardPhysicsObject->Tools_SetFlagsOfPolysToIgnore( uFlagsToIgnore );
    }

    if ( s_bCrouchEnabled )
    {
        s_pxCrouchPhysicsObject = Asura_Physics_Object::Create( ASURA_PHYSICS_OBJECT_ACTOR );
        if ( s_pxCrouchPhysicsObject )
        {
            s_pxCrouchPhysicsObject->SetLinVelocity( Asura_Maths::ZeroVector3 );
            s_pxCrouchPhysicsObject->SetAxisConstraint( Asura_Physics_Object::AXIS_CONSTRAINT_NONE );
            s_pxCrouchPhysicsObject->SetFeelsGravity( true );
            s_pxCrouchPhysicsObject->SetOnlyCollideWithEnvironmentAndStaticObjects( true );
            s_pxCrouchPhysicsObject->SetCollisionObject( &s_xCrouchCollisionBox );
			s_pxCrouchPhysicsObject->SetCollisionProxy( s_eCollisionProxy );
            s_pxCrouchPhysicsObject->SetIgnore( true );
            s_pxCrouchPhysicsObject->SetReportObjectCollisions( true );
            s_pxCrouchPhysicsObject->SetReportEnvironmentCollisions( true );

            u_int uFlagsToIgnore = s_pxStandardPhysicsObject->Tools_GetFlagsOfPolysToIgnore();
            uFlagsToIgnore |= AsuraPolygonFlag_IgnoreWhenWaypointing;
            s_pxCrouchPhysicsObject->Tools_SetFlagsOfPolysToIgnore( uFlagsToIgnore );
        }
    }

    if ( s_bProneEnabled )
    {
        s_pxPronePhysicsObject = Asura_Physics_Object::Create( ASURA_PHYSICS_OBJECT_ACTOR );
        if ( s_pxPronePhysicsObject )
        {
            s_pxPronePhysicsObject->SetLinVelocity( Asura_Maths::ZeroVector3 );
            s_pxPronePhysicsObject->SetAxisConstraint( Asura_Physics_Object::AXIS_CONSTRAINT_NONE );
            s_pxPronePhysicsObject->SetFeelsGravity( true );
            s_pxPronePhysicsObject->SetOnlyCollideWithEnvironmentAndStaticObjects( true );
            s_pxPronePhysicsObject->SetCollisionObject( &s_xProneCollisionBox );
			s_pxPronePhysicsObject->SetCollisionProxy( s_eCollisionProxy );
            s_pxPronePhysicsObject->SetIgnore( true );
            s_pxPronePhysicsObject->SetReportObjectCollisions( true );
            s_pxPronePhysicsObject->SetReportEnvironmentCollisions( true );

            u_int uFlagsToIgnore = s_pxStandardPhysicsObject->Tools_GetFlagsOfPolysToIgnore();
            uFlagsToIgnore |= AsuraPolygonFlag_IgnoreWhenWaypointing;
            s_pxPronePhysicsObject->Tools_SetFlagsOfPolysToIgnore( uFlagsToIgnore );
        }
    }

    return ( ( s_pxStandardPhysicsObject != NULL ) && ( !s_bCrouchEnabled || s_pxCrouchPhysicsObject ) && ( !s_bProneEnabled || s_pxPronePhysicsObject ) );
}

void Tools_Mode_Waypointing::DestroyWaypointingPhysicsObjects()
{
    if ( s_pxStandardPhysicsObject )
    {
        Asura_Physics_Object::Destroy( s_pxStandardPhysicsObject );
        s_pxStandardPhysicsObject = NULL;
    }
    if ( s_pxCrouchPhysicsObject )
    {
        Asura_Physics_Object::Destroy( s_pxCrouchPhysicsObject );
        s_pxCrouchPhysicsObject = NULL;
    }
    if ( s_pxPronePhysicsObject )
    {
        Asura_Physics_Object::Destroy( s_pxPronePhysicsObject );
        s_pxPronePhysicsObject = NULL;
    }
}

void Tools_Mode_Waypointing::RestorePhysicsObjectsStatus()
{
	for ( Asura_List_It< Asura_Physics_RigidBodyInstance > xListIt( &s_xRigidBodyIgnoreList ); !xListIt.Done(); xListIt.Next() )
	{
		xListIt.GetCurrent()->SetIgnore( true );
	}
	for ( Asura_List_It< Asura_Physics_RigidBodyInstance > xListIt( &s_xRigidBodyNonIgnoreList ); !xListIt.Done(); xListIt.Next() )
	{
		xListIt.GetCurrent()->SetIgnore( false );
	}
}

void Tools_Mode_Waypointing::ForgetPhysicsObjectsStatus()
{
	s_xRigidBodyIgnoreList.ClearList();
	s_xRigidBodyNonIgnoreList.ClearList();
}

bool Tools_Mode_Waypointing::CullAndFlagLinkWithPhysics( unsigned int&         uFlags,
                                                         const Asura_Vector_3& xStart,
														 const Asura_Vector_3& xStartNormal,
                                                         const int             iStartWPIndex,
                                                         const Asura_Vector_3& xFinish,
														 const Asura_Vector_3& xFinishNormal,
                                                         const int             iFinishWPIndex )
{
	Asura_Vector_3		xUpMovement;
	Asura_Vector_3		xStartPosition;
	Asura_Matrix_3x3	xOrientation;

	if(s_bGravityFollowsSurfaceNormals)
	{
		if(s_pxStandardPhysicsObject != NULL)
		{
			s_pxStandardPhysicsObject->SetOwnGravityDirection(Asura_AVector3(-xStartNormal));
		}
		if(s_pxCrouchPhysicsObject != NULL)
		{
			s_pxCrouchPhysicsObject->SetOwnGravityDirection(Asura_AVector3(-xStartNormal));
		}
		if(s_pxPronePhysicsObject != NULL)
		{
			s_pxPronePhysicsObject->SetOwnGravityDirection(Asura_AVector3(-xStartNormal));
		}
	}

	// Generate the orientation matrix based on the up-vector and direction...
	CalculateOrientationFromNormalAndDirection(xStartNormal, (xFinish - xStart), xOrientation);

	xUpMovement[0]	= xOrientation.e12 * -s_fPlacedObjectMovementScale;
	xUpMovement[1]	= xOrientation.e22 * -s_fPlacedObjectMovementScale;
	xUpMovement[2]	= xOrientation.e32 * -s_fPlacedObjectMovementScale;

    // First place the box to ensure we start correctly.
	Asura_Physics_Object	*pxPhysicsObject;

	if ( s_bProneEnabled )
		pxPhysicsObject = s_pxPronePhysicsObject;
	else if ( s_bCrouchEnabled )
		pxPhysicsObject = s_pxCrouchPhysicsObject;
	else
		pxPhysicsObject = s_pxStandardPhysicsObject;

	if ( !PlaceObjectAt( xStart, xUpMovement, s_fPlacedObjectAllowedMovement * s_fPlacedObjectAllowedMovement,
						 *pxPhysicsObject->GetCollisionObject(), pxPhysicsObject->GetFlagsOfPolysToIgnore(),
						 xOrientation, xStartPosition ) )
	{
		Asura_Assert( false, "Tools_Mode_Waypointing::CullAndFlagLinkWithPhysics - could not place object at start" );
		return false;
	}

    // Now move to the destination.
    float fFallDistance = 0.0f;
    s_pxStandardPhysicsObject->SetIgnore( false );
	s_pxStandardPhysicsObject->SetOrientation(xOrientation);
    bool bAtDestination = MoveToDestination( xStartPosition, xFinish, s_xMovedObjectAllowedOffset,
                                             s_pxStandardPhysicsObject, fFallDistance,
                                             s_pxPhysicsPositions, &s_uNumPhysicsPositions, MAX_STORED_PHYSICS_POSITIONS );
    s_pxStandardPhysicsObject->SetIgnore( true );

    if ( fFallDistance > s_fMaxLinkFallDistance )
    {
        return false;
    }
    else if ( !bAtDestination )
    {
        // Test if it is necessary to crouch.
        if ( s_bCrouchEnabled )
        {
            if ( PlaceObjectAt( xStart, xUpMovement, s_fPlacedObjectAllowedMovement * s_fPlacedObjectAllowedMovement,
                                *s_pxCrouchPhysicsObject->GetCollisionObject(), s_pxCrouchPhysicsObject->GetFlagsOfPolysToIgnore(),
                                xOrientation, xStartPosition ) )
            {
				s_pxCrouchPhysicsObject->SetIgnore( false );
				s_pxCrouchPhysicsObject->SetOrientation( xOrientation );

                bAtDestination = MoveToDestination( xStartPosition, xFinish, s_xMovedObjectAllowedOffset,
													s_pxCrouchPhysicsObject, fFallDistance,
										            s_pxPhysicsPositions, &s_uNumPhysicsPositions, MAX_STORED_PHYSICS_POSITIONS );
				
				s_pxCrouchPhysicsObject->SetIgnore( true );
				
				if ( fFallDistance > s_fMaxLinkFallDistance )
				{
					return false;
				}
				else if ( bAtDestination )
                {
                    uFlags |= ASURA_AI_CONNECTION_FLAGS_CROUCH;
                    return true;
                }
            }
        }

        // Test if it is necessary to go prone.
        if ( s_bProneEnabled )
        {
            if ( PlaceObjectAt( xStart, xUpMovement, s_fPlacedObjectAllowedMovement * s_fPlacedObjectAllowedMovement,
                                *s_pxPronePhysicsObject->GetCollisionObject(), s_pxPronePhysicsObject->GetFlagsOfPolysToIgnore(),
                                xOrientation, xStartPosition ) )
            {
                s_pxPronePhysicsObject->SetIgnore( false );
				s_pxPronePhysicsObject->SetOrientation( xOrientation );
				
				bAtDestination = MoveToDestination( xStartPosition, xFinish, s_xMovedObjectAllowedOffset,
													s_pxPronePhysicsObject, fFallDistance,
											        s_pxPhysicsPositions, &s_uNumPhysicsPositions, MAX_STORED_PHYSICS_POSITIONS );

				s_pxPronePhysicsObject->SetIgnore( true );

				if ( fFallDistance > s_fMaxLinkFallDistance )
				{
					return false;
			    }
				else if ( bAtDestination )
                {
                    uFlags |= ASURA_AI_CONNECTION_FLAGS_PRONE;
                    return true;
                }
            }
        }

        return false;
    }
    else
    {
        // Flag the link for the correct fall distance.
        bool bFurtherCullingRequired = false;
        if ( fFallDistance >= s_fLongFallLinkDistance )
        {
            uFlags |= ASURA_AI_CONNECTION_FLAGS_LONGFALL;
            bFurtherCullingRequired = true;
        }
        else if ( fFallDistance >= s_fFallLinkDistance )
        {
            uFlags |= ASURA_AI_CONNECTION_FLAGS_FALL;
            bFurtherCullingRequired = true;
        }

        if ( bFurtherCullingRequired )
        {
            // This is a fall link - it may need to be culled:
            //     1) if it travels through another waypoint before the fall in the physics block.
            //     2) if it does not land in its target waypoint after the fall in the physics block.
            // This is to try and reduce the proliferation of links around drops.

            int  iWPIndex        = iStartWPIndex;
            bool bFallingPrev    = false;
            bool bFalling        = false;
            bool bHaveEverFallen = false;
            for ( u_int u = 1; u < s_uNumPhysicsPositions; ++u )
            {
                const int iEnclosingWPIndex = GetEnclosingWaypointIndex( s_pxPhysicsPositions[ u ] );
                if ( iEnclosingWPIndex != -1 )
                {
                    iWPIndex = iEnclosingWPIndex;
                }

                bFallingPrev     = bFalling;
                // Equivalent to greater than 1 m/s downwards.
                bFalling         = ( Asura_Maths::Abs( s_pxPhysicsPositions[ u ].y - s_pxPhysicsPositions[ u - 1 ].y ) > 0.02f );
                bHaveEverFallen |= bFalling;

                if ( ( iWPIndex != iFinishWPIndex ) && ( iWPIndex != iStartWPIndex ) )
                {
                    // Test 1.
                    if ( bHaveEverFallen == false )
                    {
                        return false;
                    }

                    // Test 2.
                    if ( bFallingPrev && bFalling == false )
                    {
                        return false;
                    }
                }
            }
        }

        return true;
    }
}

void Tools_Mode_Waypointing::WarnAboutNonCollisionMeshWaypointingFlags()
{
    for ( int iModule = 0; iModule < Asura_Module_System::GetNumberOfModules(); ++iModule )
    {
        u_int uBadPolys = 0;

        const Asura_Mesh* pxMesh = Asura_Module_System::GetMeshForModule( iModule );
        if ( !pxMesh->AreNormalsValid() )
        {
            continue;
        }

        for ( int iPoly = 0; iPoly < pxMesh->NumberOfPolygons; ++iPoly )
        {
            const u_int uPolyFlags = static_cast< u_int >( pxMesh->PolygonFlagDataPtr ? pxMesh->PolygonFlagDataPtr[ iPoly ].Flags : 0 );
            const bool bGotWaypointingFlag = ( uPolyFlags & AsuraPolygonFlag_Floor ) ||
                                             ( uPolyFlags & AsuraPolygonFlag_Step ) ||
                                             ( uPolyFlags & AsuraPolygonFlag_DenseWaypoints );
            if ( bGotWaypointingFlag && ( uPolyFlags & AsuraPolygonFlag_NoCollisions ) )
            {
                ++uBadPolys;
            }
        }

        if ( uBadPolys )
        {
            Tools_MessageLog::AddMessage( Tools_MessageLog::WAYPOINTING, "Verification",
                                          new Tools_MessageLogAction_GotoAndSelectBadlyFlaggedPolys( iModule ),
                                          "%u non-collision polys with waypointing flags found in module %s",
                                          uBadPolys, pxMesh->Name.GetString() );
        }
    }
}

void Tools_Mode_Waypointing::WarnAboutBadPhysicalObjectPosOri()
{
	for ( Asura_Hashed_List_ConstIt< Asura_ServerEntity_PhysicalObject > xPhyObjIter( Asura_ServerEntity_PhysicalObject::GetList() );
		!xPhyObjIter.Done();
		xPhyObjIter.Next() )
	{
		const Asura_ServerEntity_PhysicalObject* pxPhyObj = xPhyObjIter.GetCurrent();

		if ( pxPhyObj->GetPosition() != pxPhyObj->GetPosition() )
		{
			Asura_String xErrorString( "Error: Object '");
			if ( pxPhyObj->ToolsData )
			{
				xErrorString += pxPhyObj->ToolsData->Name;
			}
			else
			{
				Asura_String xGuidString;
				Asura_StringUtil::ToString( pxPhyObj->GetGuid(), xGuidString );
				xErrorString += xGuidString;
			}
			xErrorString += "' has a corrupt position.";

			Tools_MessageLog::AddMessage( Tools_MessageLog::WAYPOINTING, "Verification",
				new Tools_MessageLogAction_GotoEntity( pxPhyObj->GetGuid() ),
				xErrorString.GetString() );
		}

		if ( pxPhyObj->GetOrientation() != pxPhyObj->GetOrientation() )
		{
			Asura_String xErrorString( "Error: Object '");
			if ( pxPhyObj->ToolsData )
			{
				xErrorString += pxPhyObj->ToolsData->Name;
			}
			else
			{
				Asura_String xGuidString;
				Asura_StringUtil::ToString( pxPhyObj->GetGuid(), xGuidString );
				xErrorString += xGuidString;
			}
			xErrorString += "' has a corrupt orientation.";

			Tools_MessageLog::AddMessage( Tools_MessageLog::WAYPOINTING, "Verification",
				new Tools_MessageLogAction_GotoEntity( pxPhyObj->GetGuid() ),
				xErrorString.GetString() );
		}
	}
}

void Tools_Mode_Waypointing::WarnAboutBadPhysicalObjectWPFlags()
{
	for ( Asura_Hashed_List_ConstIt< Asura_ServerEntity_PhysicalObject > xPhyObjIter( Asura_ServerEntity_PhysicalObject::GetList() );
		!xPhyObjIter.Done();
		xPhyObjIter.Next() )
	{
		const Asura_ServerEntity_PhysicalObject* pxPhyObj = xPhyObjIter.GetCurrent();

		if ( pxPhyObj->HasNoCollisionsFlag() )
		{
			if ( pxPhyObj->HasBlockNavDataGenerationFlag() || pxPhyObj->HasBlockWaypointLinksFlag() )
			{
				Asura_String xErrorString( "Warning: Object '");
				if ( pxPhyObj->ToolsData )
				{
					xErrorString += pxPhyObj->ToolsData->Name;
				}
				else
				{
					Asura_String xGuidString;
					Asura_StringUtil::ToString( pxPhyObj->GetGuid(), xGuidString );
					xErrorString += xGuidString;
				}
				xErrorString += "' has no collisions but is blocking nav data generation/waypoint links.";

				Tools_MessageLog::AddMessage( Tools_MessageLog::WAYPOINTING, "Verification",
					new Tools_MessageLogAction_GotoEntity( pxPhyObj->GetGuid() ),
					xErrorString.GetString() );
			}
		}
		else if ( pxPhyObj->GetPhysicsObjectType() == ASURA_PHYSICS_OBJECT_STATIC )
		{
			if ( !pxPhyObj->HasBlockNavDataGenerationFlag() && !pxPhyObj->HasBlockWaypointLinksFlag() )
			{
				Asura_String xErrorString( "Error: Object '");
				if ( pxPhyObj->ToolsData )
				{
					xErrorString += pxPhyObj->ToolsData->Name;
				}
				else
				{
					Asura_String xGuidString;
					Asura_StringUtil::ToString( pxPhyObj->GetGuid(), xGuidString );
					xErrorString += xGuidString;
				}
				xErrorString += "' has collisions set but is not blocking nav data generation/waypoint links.";

				Tools_MessageLog::AddMessage( Tools_MessageLog::WAYPOINTING, "Verification",
					new Tools_MessageLogAction_GotoEntity( pxPhyObj->GetGuid() ),
					xErrorString.GetString() );
			}
		}
	}
}

#ifdef ASURA_USE_STREAMING
bool Tools_Mode_Waypointing::AddInterZoneNewWaypoint( const Asura_Vector_3& xPosition, int iPairIndex, bool bBelongsToThisEnv )
{
    for ( int iBin = 0; iBin < NUM_WAYPOINT_BINS; ++iBin )
    {
        int& iNumWPsInBin = s_aiNumWaypoints[ iBin ];

        if ( iNumWPsInBin < NUM_WAYPOINTS_PER_BIN )
        {
            s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_xPosition           = xPosition;
            s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_iFlags		       = TOOLS_NAVVERTEX_FLAG_DENSE | TOOLS_NAVVERTEX_FLAG_USER_DEFINED;
            s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_iInterZonePairIndex = iPairIndex;
			if (bBelongsToThisEnv)
			{
				s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_iFlags |= TOOLS_NAVVERTEX_FLAG_BELONGS_TO_THIS_ENV;
			}
			else
			{
				s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_iFlags &= ~TOOLS_NAVVERTEX_FLAG_BELONGS_TO_THIS_ENV;
			}

            ++iNumWPsInBin;

            return true;
        }
    }

    // Out of bin storage.
    Asura_Assert( false, "Tools_Mode_Waypointing::AddInterZoneNewWaypoint - run out of waypoint storage" );
    return false;
}
#endif //ASURA_USE_STREAMING

bool Tools_Mode_Waypointing::SubdivideAndAddWaypoints( const Asura_Vector_3& xVertex0,
                                                       const Asura_Vector_3& xVertex1,
                                                       const Asura_Vector_3& xVertex2,
                                                       const Asura_Vector_3& xVertex3,
													   int                   iRecursionLevel )
{
	const Asura_Vector_3 xCentre  = ( xVertex0 + xVertex1 + xVertex2 + xVertex3 ) * 0.25f;
    const Asura_Vector_3 xCentre0 = ( xVertex0 + xVertex1 ) * 0.5f;
    const Asura_Vector_3 xCentre1 = ( xVertex1 + xVertex2 ) * 0.5f;
    const Asura_Vector_3 xCentre2 = ( xVertex2 + xVertex3 ) * 0.5f;
    const Asura_Vector_3 xCentre3 = ( xVertex3 + xVertex0 ) * 0.5f;

	bool bOk = AddNewWaypoint( s_iAWCurrentModuleIndex, xCentre, s_xAWCurrentNormal, s_iAWCurrentWPFlags );
	bOk	&= AddNewWaypoint( s_iAWCurrentModuleIndex, xCentre0, s_xAWCurrentNormal, s_iAWCurrentWPFlags );
	bOk	&= AddNewWaypoint( s_iAWCurrentModuleIndex, xCentre1, s_xAWCurrentNormal, s_iAWCurrentWPFlags );
	bOk	&= AddNewWaypoint( s_iAWCurrentModuleIndex, xCentre2, s_xAWCurrentNormal, s_iAWCurrentWPFlags );
	bOk	&= AddNewWaypoint( s_iAWCurrentModuleIndex, xCentre3, s_xAWCurrentNormal, s_iAWCurrentWPFlags );
	if ( s_bAWDoubleSidedPoly )
	{
		bOk &= AddNewWaypoint( s_iAWCurrentModuleIndex, xCentre,  -s_xAWCurrentNormal, s_iAWCurrentWPFlags );
		bOk	&= AddNewWaypoint( s_iAWCurrentModuleIndex, xCentre0, -s_xAWCurrentNormal, s_iAWCurrentWPFlags );
		bOk	&= AddNewWaypoint( s_iAWCurrentModuleIndex, xCentre1, -s_xAWCurrentNormal, s_iAWCurrentWPFlags );
		bOk	&= AddNewWaypoint( s_iAWCurrentModuleIndex, xCentre2, -s_xAWCurrentNormal, s_iAWCurrentWPFlags );
		bOk	&= AddNewWaypoint( s_iAWCurrentModuleIndex, xCentre3, -s_xAWCurrentNormal, s_iAWCurrentWPFlags );
	}

	// Determine if we should stop the recursion.
	if ( !bOk )
	{
		return false;
	}
	if ( !iRecursionLevel-- )
	{
		return true;
	}
	if ( ( ( xCentre - xCentre0 ).MagnitudeSquared() < s_fAWCurrentMinSubdivideSizeSq ) &&
		( ( xCentre - xCentre1 ).MagnitudeSquared() < s_fAWCurrentMinSubdivideSizeSq ) &&
		( ( xCentre - xCentre2 ).MagnitudeSquared() < s_fAWCurrentMinSubdivideSizeSq ) &&
		( ( xCentre - xCentre3 ).MagnitudeSquared() < s_fAWCurrentMinSubdivideSizeSq ) )
	{
		return true;
	}

	// Recurse further.
    if ( !SubdivideAndAddWaypoints( xVertex0, xCentre0, xCentre,  xCentre3, iRecursionLevel ) )
    {
        return false;
    }
    if ( !SubdivideAndAddWaypoints( xCentre0, xVertex1, xCentre1, xCentre,  iRecursionLevel ) )
    {
        return false;
    }
    if ( !SubdivideAndAddWaypoints( xCentre,  xCentre1, xVertex2, xCentre2, iRecursionLevel ) )
    {
        return false;
    }
    if ( !SubdivideAndAddWaypoints( xCentre3, xCentre,  xCentre2, xVertex3, iRecursionLevel ) )
    {
        return false;
    }

    return true;
}

bool Tools_Mode_Waypointing::SubdivideAndAddWaypoints( const Asura_Vector_3& xVertex0,
                                                       const Asura_Vector_3& xVertex1,
                                                       const Asura_Vector_3& xVertex2,
													   int                   iRecursionLevel )
{
	const Asura_Vector_3 xCentre  = ( xVertex0 + xVertex1 + xVertex2 ) * 0.3333333333334f;
	const Asura_Vector_3 xCentre0 = ( xVertex0 + xVertex1 ) * 0.5f;
	const Asura_Vector_3 xCentre1 = ( xVertex1 + xVertex2 ) * 0.5f;
	const Asura_Vector_3 xCentre2 = ( xVertex2 + xVertex0 ) * 0.5f;

	bool bOk = AddNewWaypoint( s_iAWCurrentModuleIndex, xCentre, s_xAWCurrentNormal, s_iAWCurrentWPFlags );
	bOk &= AddNewWaypoint( s_iAWCurrentModuleIndex, xCentre0, s_xAWCurrentNormal, s_iAWCurrentWPFlags );
	bOk &= AddNewWaypoint( s_iAWCurrentModuleIndex, xCentre1, s_xAWCurrentNormal, s_iAWCurrentWPFlags );
	bOk &= AddNewWaypoint( s_iAWCurrentModuleIndex, xCentre2, s_xAWCurrentNormal, s_iAWCurrentWPFlags );
	if ( s_bAWDoubleSidedPoly )
	{
		bOk &= AddNewWaypoint( s_iAWCurrentModuleIndex, xCentre,  -s_xAWCurrentNormal, s_iAWCurrentWPFlags );
		bOk &= AddNewWaypoint( s_iAWCurrentModuleIndex, xCentre0, -s_xAWCurrentNormal, s_iAWCurrentWPFlags );
		bOk &= AddNewWaypoint( s_iAWCurrentModuleIndex, xCentre1, -s_xAWCurrentNormal, s_iAWCurrentWPFlags );
		bOk &= AddNewWaypoint( s_iAWCurrentModuleIndex, xCentre2, -s_xAWCurrentNormal, s_iAWCurrentWPFlags );
	}

	// Determine if we should stop the recursion.
	if ( !bOk )
	{
		return false;
	}
	if ( !iRecursionLevel-- )
	{
		return true;
	}
	if ( ( ( xCentre - xCentre0 ).MagnitudeSquared() < s_fAWCurrentMinSubdivideSizeSq ) &&
		( ( xCentre - xCentre1 ).MagnitudeSquared() < s_fAWCurrentMinSubdivideSizeSq ) &&
		( ( xCentre - xCentre2 ).MagnitudeSquared() < s_fAWCurrentMinSubdivideSizeSq ) )
	{
		return true;
	}

	// Recurse further.
	if ( !SubdivideAndAddWaypoints( xVertex0, xCentre, xCentre0, iRecursionLevel ) )
	{
		return false;
	}
	if ( !SubdivideAndAddWaypoints( xCentre0, xCentre, xVertex1, iRecursionLevel ) )
	{
		return false;
	}
	if ( !SubdivideAndAddWaypoints( xVertex1, xCentre, xCentre1, iRecursionLevel ) )
	{
		return false;
	}
	if ( !SubdivideAndAddWaypoints( xCentre1, xCentre, xVertex2, iRecursionLevel ) )
	{
		return false;
	}
	if ( !SubdivideAndAddWaypoints( xVertex2, xCentre, xCentre2, iRecursionLevel ) )
	{
		return false;
	}
	if ( !SubdivideAndAddWaypoints( xCentre2, xCentre, xVertex0, iRecursionLevel ) )
	{
		return false;
	}

	return true;
}

bool Tools_Mode_Waypointing::GetNextLargestWaypoint( Tools_Nav_Vertex* apxBinPointers[],
                                                     Tools_Nav_Vertex*& pxLargestWaypoint,
                                                     int& iBinIndex,
                                                     int& iIntraBinIndex )
{
    // Return the largest waypoint out of all the bins.
    // Move that bin pointer along.
    // If all the bin pointers have overshot then we're done.
    // NOTE: This function will cause the bin pointers to overshoot the end of the bins. Do not use the bin pointers once this function returns false.
    pxLargestWaypoint = NULL;

    int iLargestWaypointBin = -1;
    for ( int iBin = 0; iBin < NUM_WAYPOINT_BINS; ++iBin )
    {
		// Look at the next waypoint in this bin. We may have to skip some if we are currently pointing at a deleted waypoint or a user defined waypoint.
        bool bPointerValid = false;
        for (  ; ( apxBinPointers[ iBin ] - s_apxWaypointStorage[ iBin ] ) < s_aiNumWaypoints[ iBin ]; ++apxBinPointers[ iBin ] )
        {
            if ( apxBinPointers[ iBin ]->m_fRadius > 0.0f && !apxBinPointers[ iBin ]->IsUserDefined() )
            {
                bPointerValid = true;
                break;	
            }
        }

        if ( bPointerValid )
        {
            if ( ( pxLargestWaypoint == NULL ) || ( apxBinPointers[ iBin ]->m_fRadius > pxLargestWaypoint->m_fRadius ) )
            {
                pxLargestWaypoint   = apxBinPointers[ iBin ];
                iLargestWaypointBin = iBin;
            }
        }
    }

    if ( pxLargestWaypoint )
    {
        iBinIndex      = iLargestWaypointBin;
        iIntraBinIndex = apxBinPointers[ iLargestWaypointBin ] - s_apxWaypointStorage[ iLargestWaypointBin ];
        ++apxBinPointers[ iLargestWaypointBin ];
        return true;
    }
    else
    {
        return false;
    }
}

void Tools_Mode_Waypointing::AABBTreePerBinSizeTestCallback( u_int uIndex )
{
    // Work out which bin index and intra-bin index this callback index corresponds to.
    int iBinIndex      = 0;
    int iIntraBinIndex = 0;
    if ( !CalculateBinAndIntraBinIndex( uIndex, iBinIndex, iIntraBinIndex ) )
    {
        Asura_Assert( false, "Tools_Mode_Waypointing::AABBTreePerBinSizeTestCallback - could not resolve index to bin and intra-bin index" );
        return;
    }

    const Tools_Nav_Vertex& xTestWaypoint     = s_apxWaypointStorage[ s_iBinTestIndex ][ s_uTestIndex ];
    Tools_Nav_Vertex&       xCallbackWaypoint = s_apxWaypointStorage[ iBinIndex ][ iIntraBinIndex ];

    if ( xCallbackWaypoint.m_iFlags & TOOLS_NAVVERTEX_FLAG_ALREADY_VISITED )
    {
        // If we have already used this waypoint to cull other waypoints then don't cull it.
        return;
    }
    if ( iIntraBinIndex == static_cast< int >( s_uTestIndex ) && iBinIndex == s_iBinTestIndex )
    {
        // Don't cull ourselves.
        return;
    }
    if ( xCallbackWaypoint.IsUserDefined() )
    {
        return;
    }
    if ( xCallbackWaypoint.m_fRadius == 0.0f )
    {
        // If we've already culled this waypoint then don't try again.
        return;
    }

    // Cull the callback waypoint if its centre is within the test waypoint.
    const Asura_Vector_3 xDist       = xCallbackWaypoint.m_xPosition - xTestWaypoint.m_xPosition;
    const float          fTestRadius = xTestWaypoint.m_fRadius;
    if ( xDist.MagnitudeSquared() < fTestRadius * fTestRadius )
    {
        xCallbackWaypoint.m_fRadius = 0.0f;
    }
}

bool Tools_Mode_Waypointing::CalculateBinAndIntraBinIndex( const u_int uIndex, int& iBinIndex, int& iIntraBinIndex )
{
    int iRunningIndex = static_cast< int >( uIndex );
    for ( int iBin = 0; iBin < NUM_WAYPOINT_BINS; ++iBin )
    {
        if ( iRunningIndex < s_aiNumWaypoints[ iBin ] )
        {
            iBinIndex      = iBin;
            iIntraBinIndex = iRunningIndex;
            return true;
        }

        iRunningIndex -= s_aiNumWaypoints[ iBin ];
    }

    return false;
}

void Tools_Mode_Waypointing::CalculateGlobalIndex( int iBinIndex, const int iIntraBinIndex, u_int& uGlobalIndex )
{
	uGlobalIndex = iIntraBinIndex;
	while ( iBinIndex > 0 ) { uGlobalIndex += s_aiNumWaypoints[ --iBinIndex ]; }
}

void Tools_Mode_Waypointing::WaypointVisibilityCheckCallback( const u_int uIndex )
{
    const Asura_Module* pxModule = Asura_Module_System::GetModuleData( uIndex );

	bool bNonExterior = !Asura_Module_System::GetCurrentHandler().IsExteriorModule(uIndex);
	if(bNonExterior && !s_bBBInNonExteriorModule)
	{
		//This is the first non exterior module found ,so reset the results up until this point
		//RAR 10/04/2007
		s_bBBInNonExteriorModule = true;
        for ( int j = 0; j < Asura_Module_System::GetNumberOfModules(); ++j )
        {
            s_bVisibleModules[ j ] = false;
        }

	}

	if(bNonExterior == s_bBBInNonExteriorModule)
	{
    	s_bVisibleModules[ uIndex ] = true;

    	for ( int i = 0; i < pxModule->NumberOfVisibleModules; ++i )
    	{
			//Added test for module index being in range , since in streaming builds there may be 
			//values >65526 representing links to modules in other zones
			//RAR 19/12/2006
			if(pxModule->VisibleModules[ i ] < Asura_Module_System::GetNumberOfModules())
			{
				s_bVisibleModules[ pxModule->VisibleModules[ i ] ] = true;
			}
    	}
	}
}

void Tools_Mode_Waypointing::CoverpointVisibilityCheckCallback( const u_int uIndex )
{
	bool bNonExterior = !Asura_Module_System::GetCurrentHandler().IsExteriorModule(uIndex);
	if(bNonExterior && !s_bBBInNonExteriorModule)
	{
		//This is the first non exterior module found ,so reset the results up until this point
		//RAR 10/04/2007
		s_bBBInNonExteriorModule = true;
		s_pxCoverpointStorage[ s_uTestIndex ].m_bValid = true;
        
	}
    
	if( bNonExterior == s_bBBInNonExteriorModule)
	{
    	if ( s_bVisibleModules[ uIndex ] == false ) 
    	{
    	    s_pxCoverpointStorage[ s_uTestIndex ].m_bValid = false;
    	}
	}
}

bool Tools_Mode_Waypointing::GetWaypointIndexUnderMouse( int& iIndex, const Tools_Nav_Vertex* pxVerts, const int iNumVerts )
{
    //get the z direction for the mouse
    Asura_Vector_3 xZVector;
    Asura_Cursor::GetLOSDirection(xZVector);

    // Kevin Lea - 22/06/2001 12:49:20 - and transform this direction into worldspace
    const Asura_Vector_3* pxCameraPosition = Asura_Camera::GetPosition();
    const Asura_Matrix_3x3* pxCameraOrientation = Asura_Camera::GetOrientation();

    xZVector = xZVector * (*pxCameraOrientation);

    int iJunkPolyIndex;
    int iJunkModuleIndex;
    float fDistanceToHit = Asura_Maths::LargeFloat;
    Tools_LOS::FindPolyInLOS(*pxCameraPosition,xZVector,iJunkPolyIndex,iJunkModuleIndex,fDistanceToHit);

    iIndex = -1;
    for ( int i = 0; i < iNumVerts; ++i )
    {
        if ( pxVerts[ i ].m_fRadius == 0.0f )
        {
            continue;
        }

        float fDistanceToSphereCollision;
        if ( Asura_Maths::RayIntersectsSphere( *pxCameraPosition, xZVector,
                                               pxVerts[ i ].m_xPosition,
                                               pxVerts[ i ].m_fRadius,
                                               &fDistanceToSphereCollision ) && ( fDistanceToSphereCollision > 0.0f ) )
        {
            if ( fDistanceToSphereCollision < fDistanceToHit )
            {
                fDistanceToHit = fDistanceToSphereCollision;
                iIndex         = i;
            }
        }
    }

    return ( iIndex != -1 );
}

unsigned short Tools_Mode_Waypointing::LOSExistsFromCoverpointToWaypoint( const int iCoverpoint,
                                                                          const int iWaypoint )
{
	int i;

    Asura_Vector_3 xCoverpointPos	= s_pxCoverpointStorage[iCoverpoint].m_xCentre;
	Asura_Vector_3 xWaypointPos		= s_pxWaypointStorage[iWaypoint].m_xPosition;
	Asura_Vector_3 xDeltaPos		= xWaypointPos - xCoverpointPos;
	if(xDeltaPos.MagnitudeSquared() < 10.0f * 10.0f)
	{
		return 0;
	}
	Asura_Vector_3 xXVector(xDeltaPos.z, 0.0f, -xDeltaPos.x);
	xXVector.Normalise();

	xCoverpointPos.y += 0.1f;
	xWaypointPos.y   += 0.1f;
	xWaypointPos.y   -= 1.625f;

	bool bTargetVisible = false;
	Asura_Vector_3 xGunPos = xCoverpointPos;
	xGunPos.y -= 0.875f;

	float fScanRadius = s_pxWaypointStorage[iWaypoint].m_fRadius - 0.25f;
	const int iNumVisibleScans = 5;

	for(i = 0; i < iNumVisibleScans; ++i)
	{
		Asura_Vector_3 xWaypointScanPos = xWaypointPos;
		xWaypointScanPos += xXVector * fScanRadius * (-1.0f + 2.0f * (float(i) / float(iNumVisibleScans - 1)));

		Asura_Vector_3 xMidPos = (xGunPos + xWaypointScanPos) * 0.5f;
		if(!LOSTestHit(xMidPos, xWaypointPos))
		{
			bTargetVisible = true;
			break;
		}
	}

	if(!bTargetVisible)	//Target is behind a wall
	{
		return 0;
	}

//Check for FULL cover points (Hiding)
	Asura_Vector_3 xBodyPos[3];
	xBodyPos[0] = Asura_Vector_3(-0.375f, -0.75f, 0.0f);	//Left Elbow
	xBodyPos[1] = Asura_Vector_3( 0.125f, -1.25f, 0.0f);	//Head
	xBodyPos[2] = Asura_Vector_3( 0.625f, -0.75f, 0.0f);	//Right Elbow

	bool bBodyHidden = true;
	for(i = 0; i < 3; ++i)
	{
		Asura_Vector_3 xTestPos = xCoverpointPos;
		xTestPos   += xXVector * xBodyPos[i].x;
		xTestPos.y += xBodyPos[i].y;
		Asura_Vector_3 xMidPos = (xTestPos + xWaypointPos) * 0.5f;
		if(!LOSTestHit(xMidPos, xTestPos))
		{
			bBodyHidden = false;
			break;
		}
	}

	if(bBodyHidden)
	{
		Asura_Vector_3 xWidePos[3];
		xWidePos[0] = Asura_Vector_3(-1.0f,   -1.25f, 0.0f);	//Wide to the Left
		xWidePos[1] = Asura_Vector_3( 0.125f, -1.75f, 0.0f);	//Above the Head
		xWidePos[2] = Asura_Vector_3( 1.25f,  -1.25f, 0.0f);	//Wide to the Right

		for(i = 0; i < 3; ++i)
		{
			Asura_Vector_3 xTestPos = xCoverpointPos;
			xTestPos   += xXVector * xWidePos[i].x;
			xTestPos.y += xWidePos[i].y;
			Asura_Vector_3 xMidPos = (xTestPos + xWaypointPos) * 0.5f;
			if(!LOSTestHit(xMidPos, xTestPos))
			{
				return ASURA_AI_COVERPOINT_FULL;
			}
		}
	}

//Check for SEMI cover points (Firing)
	int iNumGoodCover  = 0;
	int iNumGoodFiring = 0;
	for(i = 0; i < 5; ++i)
	{
		float fDeltaX = -0.5f + 0.25f * i;	//-0.5m, -0.25m, 0.0m, 0.25m, 0.5m
		Asura_Vector_3 xTestPos = xCoverpointPos + xXVector * fDeltaX;
		xTestPos.y -= 0.625f;	//Chest Crouched
		Asura_Vector_3 xMidPos = (xTestPos + xWaypointPos) * 0.5f;
		if(LOSTestHit(xMidPos, xTestPos))
		{
			++iNumGoodCover;
		}
		xTestPos.y -= 0.25f;	//Gun Crouched
		xMidPos = (xTestPos + xWaypointPos) * 0.5f;
		if(!LOSTestHit(xTestPos, xMidPos))
		{
			++iNumGoodFiring;
		}
		xTestPos.y -= 0.5f;		//Chest Standing
		xMidPos = (xTestPos + xWaypointPos) * 0.5f;
		if(LOSTestHit(xMidPos, xTestPos))
		{
			++iNumGoodCover;
		}
		xTestPos.y -= 0.25f;	//Gun Standing
		xMidPos = (xTestPos + xWaypointPos) * 0.5f;
		if(!LOSTestHit(xTestPos, xMidPos))
		{
			++iNumGoodFiring;
		}

		if((iNumGoodCover >= 2) && (iNumGoodFiring >= 2))
		{
			return ASURA_AI_COVERPOINT_SEMI;
		}
	}
    
// Not any kind of cover
    return 0;
}

void Tools_Mode_Waypointing::FindCoverpointExits( const int iCoverpoint,
                                                  unsigned char& ucShortExits,
                                                  unsigned char& ucLongExits )
{
    // Check the eight compass points around a coverpoint to see if they are valid positions
    // If they are set the Exit Bitfield accordingly.

    Asura_Vector_3 xDisp;
    xDisp.Zero();

    Asura_Vector_3 xTestPosition;

    const float fShortExitLength = 1.0f;
    const float fLongExitLength = 3.0f;

//  float fDisplacementX = 0.0f;
//  float fDisplacementZ = 0.0f;
    unsigned char ucExitMask = 1;

    bool bDone = false;
    float fDispSize = fShortExitLength;


    // If this is a LOW cover but not a HIGH cover, then we just need to stand up to get out of cover.
    
    if(s_pxCoverpointStorage[iCoverpoint].m_usFlags & ASURA_AI_COVERPOINT_LOW &&
        !(s_pxCoverpointStorage[iCoverpoint].m_usFlags & ASURA_AI_COVERPOINT_HIGH))
    {
        ucShortExits = 0;
        ucLongExits = 0;
        return;
    }

    while(!bDone)
    {   
        
        for(xDisp.z = -fDispSize; xDisp.z <= fDispSize; xDisp.z += fDispSize)
        {
            for(xDisp.x = -fDispSize; xDisp.x <= fDispSize; xDisp.x += fDispSize)
            {
                float fSavedX, fSavedZ;
                bool bSavedValues = false;

                // don't want the centre point
                if(xDisp.x == 0.0f && xDisp.z == 0.0f)
                {
                    continue;
                }

                // if this is one of the diagonal points, alter the length so that is a circle not a grid.
                if(xDisp.x != 0.0f && xDisp.z != 0.0f)
                {
                    bSavedValues = true;
                    fSavedX = xDisp.x;
                    fSavedZ = xDisp.z;

                    xDisp.x *= 0.707107f;   // sine(45) 
                    xDisp.z *= 0.707107f;
                }

                xTestPosition = s_pxCoverpointStorage[iCoverpoint].m_xCentre + xDisp;
                

//              int iNearestWaypoint = Asura_AI_Navigation_System::FindWaypointNearestTo(xTestPosition);
        
                Asura_Vector_3 xStart(s_pxCoverpointStorage[iCoverpoint].m_xCentre.x, s_pxCoverpointStorage[iCoverpoint].m_xCentre.y-0.1f, s_pxCoverpointStorage[iCoverpoint].m_xCentre.z);
                xTestPosition.y -= 0.1f;
                float fFallDistance;
                if ( MoveToDestination( xStart, xTestPosition, s_xMovedObjectAllowedOffset, s_pxStandardPhysicsObject, fFallDistance ) )
                {
                    if(fDispSize == fShortExitLength)
                    {
                        ucShortExits |= ucExitMask;
                    }
                    else if(fDispSize == fLongExitLength)
                    {
                        ucLongExits |= ucExitMask;
                    }
                    else
                    {
                        Asura_Assert(0,"Unknown displacement size");
                    }
                }
                else
                {
                    if(fDispSize == fShortExitLength)
                    {
                        ucShortExits &= ~ucExitMask;
                    }
                    else if(fDispSize == fLongExitLength)
                    {
                        ucLongExits &= ~ucExitMask;
                    }
                    else
                    {
                        Asura_Assert(0,"Unknown displacement size");
                    }
                }


                if(bSavedValues)
                {
                    xDisp.x = fSavedX;
                    xDisp.z = fSavedZ;
                }

                // Move ExitMask onto next mask.
                ucExitMask *= 2;
            }
        }
        
        if(fDispSize == fShortExitLength)
        {
            fDispSize = fLongExitLength;
            ucExitMask = 1;
        }
        else if(fDispSize == fLongExitLength)
        {
            bDone = true;
        }
    }
}

bool Tools_Mode_Waypointing::FloodFillWaypointGraph( const int iStartWaypointIndex, const u_short usZoneID )
{
    // cf Introduction To Algorithms, Cormen, Leiserson, Rivest, Stein, pp 532->533

    // give up if we have too many zones
    if ( usZoneID == ASURA_NAVVERTEX_MAX_ZONE_ID )
    {
        Asura_Assert( false, "Tools_Mode_Waypointing::FloodFillWaypointGraph - too many zones" );
        return false;
    }

    int i;
    Asura_LinkList< int > xPendingQueue;

    xPendingQueue.Push_Back( iStartWaypointIndex );

    while ( xPendingQueue.Front() != NULL )
    {
        Asura_ListElt< int >* pxElt = xPendingQueue.Front();
        const int iWaypointIndex = pxElt->Data();
        xPendingQueue.Remove( pxElt );

        Asura_AI_NavVertex* pxBaseWaypoint = Asura_AI_Navigation_System::GetWaypoint( static_cast< unsigned short >( iWaypointIndex ) );
        for ( int c = 0; c < pxBaseWaypoint->m_ucNumConnections; ++c )
        {
            Asura_AI_NavVertex* pxNextWaypoint =
                Asura_AI_Navigation_System::GetWaypoint( pxBaseWaypoint->m_pxConnections[ c ].m_usIndex );
            if ( pxNextWaypoint->m_usZone == s_usWhiteZoneID )
            {
                pxNextWaypoint->m_usZone = s_usGrayZoneID;
                xPendingQueue.Push_Back( pxBaseWaypoint->m_pxConnections[ c ].m_usIndex );
            }
        }

        pxBaseWaypoint->m_usZone = s_usBlackZoneID;
    }

    // all the waypoints with black zone ids are the ones that belong in this zone
    for ( i = 0; i < Asura_AI_Navigation_System::GetNumberOfWaypoints(); ++i )
    {
        if ( Asura_AI_Navigation_System::GetWaypoint( i )->m_usZone == s_usBlackZoneID )
        {
            Asura_AI_Navigation_System::GetWaypoint( i )->m_usZone = usZoneID;
        }
    }

    return true;
}

int Tools_Mode_Waypointing::GetEnclosingWaypointIndex( const Asura_Vector_3& xPosition )
{
    // find the waypoint we're inside
    // if we're inside more than one then return the one we're nearest to

    float fLeastDistanceSq  = Asura_Maths::LargeFloat;
    int   iEnclosingWPIndex = -1;

    for ( int i = 0; i < s_iNumWaypoints; ++i )
    {
        const Asura_Vector_3 xWPToPositionVector = xPosition - s_pxWaypointStorage[ i ].m_xPosition;
        const float          fDistanceSq         = xWPToPositionVector.MagnitudeSquared();
        if ( fDistanceSq < s_pxWaypointStorage[ i ].m_fRadius * s_pxWaypointStorage[ i ].m_fRadius )
        {
            if ( iEnclosingWPIndex == -1 || fDistanceSq < fLeastDistanceSq )
            {
                iEnclosingWPIndex = i;
                fLeastDistanceSq  = fDistanceSq;
            }
        }
    }

    return iEnclosingWPIndex;
}

void Tools_Mode_Waypointing::RenderLine( const Asura_Vector_3& xStart, const Asura_Vector_3& xEnd,
                                         const Asura_Vector_3& xStartColour, const Asura_Vector_3& xEndColour, 
                                         const int iNumSubDivisions )
{
    const Asura_Vector_3 xDiff      = ( xEnd - xStart ) / static_cast< float >( iNumSubDivisions );
    const float          fInvLength = 1.0f / ( xEnd - xStart ).Magnitude();
    Asura_Vector_3       xP1        = xStart;
    for ( int i = 0; i < iNumSubDivisions; ++i )
    {
        const float    fColourBlend = ( ( ( xP1 + ( xDiff * 0.5f ) ) - xStart ) ).Magnitude() * fInvLength;
        Asura_Vector_3 xColourBlend = ( xEndColour * fColourBlend ) + ( xStartColour * ( 1.0f - fColourBlend ) );

        unsigned int   uColour = 0xff000000;
        uColour |= Asura_Maths::FloatToInt( xColourBlend.x * 255.0f ) << 16;
        uColour |= Asura_Maths::FloatToInt( xColourBlend.y * 255.0f ) << 8;
        uColour |= Asura_Maths::FloatToInt( xColourBlend.z * 255.0f );
        Asura_Render::RenderLine( xP1, xP1 + xDiff, uColour );

        xP1 += xDiff;
    }
}

bool Tools_Mode_Waypointing::LOSTestHit( const Asura_Vector_3& xStart, const Asura_Vector_3& xEnd )
{
    Asura_Vector_3 xScanVec = xEnd - xStart;
    float fScanVecLength = xScanVec.Normalise();
    const Asura_LOS_InputData xInputData(xStart, xScanVec, ASURA_GUID_UNREGISTERED, fScanVecLength);
    const Asura_LOS_OutputData* pxOutputData = Asura_LOS::TestAgainstEverything(&xInputData);
    return((pxOutputData->m_bHitEnvironment) || (pxOutputData->m_uHitEntity != 0));
}


void Tools_Mode_Waypointing::CreateToolsEntities()
{
	//already exist?
	if(s_bToolsEntitiesCreated) return;

	// Create all the objects, and start them off as invisible.
	// Note that although CreateAllObjects() doesn't create UDW objects, they may already have been created elsewhere.
	Tools_Entity_Object::CreateAllObjects();
	for(LIF<Tools_Object*> xObjectLIF(&Tools_Object_System::ObjectList);!xObjectLIF.done();xObjectLIF.next())
	{
		const int iObjectType = xObjectLIF()->Type;
		if ( iObjectType == TOT_UserDefinedWaypointPosition )
		{
			if ( s_bShowUserDefinedWaypoints )
			{
				xObjectLIF()->HiddenFlags &= ~ObjectHideFlag_Mode;
			}
			else
			{
				xObjectLIF()->HiddenFlags |= ObjectHideFlag_Mode;
			}
		}
		else if ( !xObjectLIF()->ShouldPersistBetweenModes() )
		{
			xObjectLIF()->HiddenFlags |= ObjectHideFlag_Mode;
		}

        // Disable selection on all objects except for user waypoint objects and tools lights (?)(JAT 21/09/07).
		const bool bSelectable = ( iObjectType == TOT_UserDefinedWaypointPosition || iObjectType == TOT_ToolsPhoton );
        xObjectLIF()->EnableSelection( bSelectable );
	}

	s_bToolsEntitiesCreated = true;
	s_bStaticObjectsVisible = false;
}

void Tools_Mode_Waypointing::UpdateStaticObjects()
{
	//create tools entities if needed
	if(s_bShowStaticObjects && !s_bToolsEntitiesCreated)
	{
		CreateToolsEntities();
	}

	//update visibility
	if(s_bShowStaticObjects != s_bStaticObjectsVisible)
	{
		for(LIF<Tools_Object*> xObjectLIF(&Tools_Object_System::ObjectList);!xObjectLIF.done();xObjectLIF.next())
		{
			if(xObjectLIF()->Type == TOT_Entity_PhysicsObject || xObjectLIF()->Type == TOT_Entity_Container)
			{
				if(s_bShowStaticObjects)
				{
					xObjectLIF()->HiddenFlags &= ~ObjectHideFlag_Mode;
				}
				else
				{
					xObjectLIF()->HiddenFlags |= ObjectHideFlag_Mode;
				}
			}
		}
		s_bStaticObjectsVisible = s_bShowStaticObjects;
	}
}

void Tools_Mode_Waypointing::HandleMeshChange(int iModuleIndex)
{
	Tools_Mode_Flags::ClearUndo();
}

void Tools_Mode_Waypointing::CalculateOrientationFromNormalAndDirection(const Asura_Vector_3& xNormal,
																		const Asura_Vector_3& xDirection,
																		Asura_Matrix_3x3& xOrientation)
{
	if(xNormal.MagnitudeSquared() < 0.000001f)
	{
		Asura_Maths::OrientationFromDirection(xOrientation, xDirection);
	}
	else if(xDirection.MagnitudeSquared() < 0.000001f)
	{
        Asura_Maths::OrientationFromNormal(xOrientation, xNormal);
	}
	else
	{
		Asura_Vector_3	xForwards(xDirection);
		Asura_Vector_3	xUp(-xNormal);

		Asura_Vector_3	xSide;

		// Generate the orientation matrix based on the up-vector and direction...
		xSide		= xUp ^ xForwards;
		xForwards	= xSide ^ xUp;

		xForwards.Normalise();
		xSide.Normalise();

		// Re-create the orientation matrix...
		xOrientation.e11	= xSide[0];
		xOrientation.e21	= xSide[1];
		xOrientation.e31	= xSide[2];
		xOrientation.e12	= xUp[0];
		xOrientation.e22	= xUp[1];
		xOrientation.e32	= xUp[2];
		xOrientation.e13	= xForwards[0];
		xOrientation.e23	= xForwards[1];
		xOrientation.e33	= xForwards[2];
	}
}

//////////////////////////////////////////////////////////////////////////////
//
// 
//
//////////////////////////////////////////////////////////////////////////////
