
///////////////////////////////////////////////////////////////////////////////
//
// File   : ToolsAVP_Mode_Waypointing.cpp
// Author : Andy Tate
// Date   : 4-Jun-2007
//
// Copyright (C) 2007 Rebellion, All Rights Reserved.
//
///////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////
// Include files...
////////////////////////////////////////////////////////////////////////////////

#include "Core/Asura.h"

// This file's header...
#include "Waypointing/ToolsAVP_Mode_Waypointing.h"

// Asura independent header-files...
#include "Blueprint/Asura_Blueprint_System.h"
#ifdef ASURA_USE_BOTANICALS
#include "Botanicals/Tools_Botanicals_Object.h"
#include "Botanicals/Asura_Botanicals_InstanceManager.h"
#endif
#include "Core/Asura_AABB_Tree.h"
#include "Core/Asura_HashedList.h"
#include "Core/Asura_OrientedVolume.h"
#include "Core/Asura_UserInput.h"
#include "Core/Memory/Asura_Mem.h"
#include "Debugging/Asura_Debug_Timers.h"
#include "Debugging/Asura_DebugLineRender.h"
#include "Entity/PhysicalObject/Asura_ServerEntity_PhysicalObject.h"
#include "Interface/Asura_Dialog.h"
#include "Interface/Asura_Menubar.h"
#include "Interface/Asura_Menubar_System.h"
#include "Interface/Asura_Overlay.h"
#include "Interface/Asura_Overlay_System.h"
#include "Load&Save/Tools_SaveAsura.h"
#include "Mesh/Asura_Mesh.h"
#include "Mesh/Asura_Mesh_PolygonFlags.h"
#include "Physics/Asura_Physics.h"
#include "Physics/Asura_Physics_CCD.h"
#include "Rendering/Asura_Module.h"
#include "Rendering/Asura_Render.h"
#include "Rendering/Asura_Text_Output.h"

// Game independent header-files...
#include "Actor/Vertebrates/Alien/AVP_Alien_Description.h"
#include "Actor/Vertebrates/AVP_Vertebrate_Description.h"
#include "AI/AVP_AI_Navigation.h"
#include "Attractors/AVP_Attractors_System.h"
#include "Behaviour/BehaviourTypes/Alien/AVP_Behaviour_Alien_Movement.h"
#include "Blueprint/AVP_Blueprint_HashIDs.h"
#include "Core/AVP_Navigation.h"
#include "Core/AVP_LOS.h"
#include "Core/AVP_Plane.h"
#include "Core/AVP_Waypoint_Manager.h"
#include "Entity/Locations/OrientedNode/GamesceneNode/AVP_ServerNode_GS2Node.h"
#include "Entity/Locations/OrientedNode/LiftNode/Asura_ServerNode_LiftNode.h"
#include "Entity/Locations/Spline/GamesceneSpline/AVP_ServerNode_GS2Spline.h"
#include "Mesh/AVP_PolygonFlags.h"
#include "Physics/AVP_PhysicsObjectHandler.h"
// Tools header-files...
#include "Attractors/Tools_Attractor.h"
#include "Attractors/Tools_Attractor_Node.h"
#include "Attractors/Tools_Attractor_System.h"
#include "Attractors/Tools_Object_Attractor_Node.h"
#ifdef ASURA_USE_CONTAINER_SYSTEM
#include "Entity/Container/Tools_Entity_Container.h"
#endif
#include "Load&Save/Tools_ModeDataFile.h"
#include "Hierarchy/Tools_Animation_Files.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 "Objects/ToolsAVP_Object_Type.h"
#include "Waypointing/Tools_Waypoint_Editing.h"
#include "Waypointing/Tools_Waypoint_Object.h"
#include "Waypointing/Tools_Waypoint_Selection.h"
#include "Waypointing/ToolsAVP_Waypoint_Analysis.h"
#include "Entity/GamePlayVolume/AVP_ServerEntity_GamePlayVolume.h"

#define SCORE_DISTANCE_LINEARLY  (1)

//////////////////////////////////////////////////////////////////////////////
//
// ToolsAVP_Waypoint_Object
//
//////////////////////////////////////////////////////////////////////////////

class ToolsAVP_Waypoint_Object : public Tools_Waypoint_Object
{
	typedef Tools_Waypoint_Object PARENT;

public:
	ToolsAVP_Waypoint_Object( Asura_AI_Navigation_System::UserDefinedWaypoint* pxUDW )
	: Tools_Waypoint_Object( pxUDW )
	, m_bChanged( false )
	, m_bAddLink( false )
	, m_bRemoveLink( false )
	, m_uUDWID( Asura_AI_Navigation_System::UserDefinedWaypoint::uINVALID_ID )
	{
		Asura_String xName;
		Asura_StringUtil::ToString( GetUDW().GetID(), xName );
		xName.Prepend( "AVP User Defined Waypoint " );
		SetName( xName );
	}

	virtual const char* GetTypeDescription() { return "AVP User Defined Waypoint"; }
	virtual void Destroy();
	virtual bool ShouldCreateObjectOverlay() { return true; }
	virtual void AddOverlayControls( Asura_Overlay* pxOverlay, Asura_Vector_2& xOffset );
	virtual void Update();
	virtual void Render();
	virtual bool WaypointingVerify();

	const AVP_AI_Navigation_System::AVPUserDefinedWaypoint& GetAVPUDW() const
	{
		return static_cast< const AVP_AI_Navigation_System::AVPUserDefinedWaypoint& >( GetUDW() );
	}

protected:
	AVP_AI_Navigation_System::AVPUserDefinedWaypoint& GetAVPUDW()
	{
		return static_cast< AVP_AI_Navigation_System::AVPUserDefinedWaypoint& >( GetUDW() );
	}

	static bool IsLiftWaypoint( int iThis );
	static void SetAsLiftWaypoint( bool bSet, int iThis );
	static void AddLink( int iThis );
	static void RemoveLink( int iThis );

private:
	bool m_bChanged;
	bool m_bAddLink;
	bool m_bRemoveLink;
	u_int m_uUDWID;
};

void ToolsAVP_Waypoint_Object::Destroy()
{
	// Remove any links to us.
	for ( Asura_Hashed_List_It< Tools_Waypoint_Object > xObjIter( Tools_Waypoint_Object::GetList() ); !xObjIter.Done(); xObjIter.Next() )
	{
		ToolsAVP_Waypoint_Object* pxAVPObj = static_cast< ToolsAVP_Waypoint_Object* >( xObjIter.GetCurrent() );
		pxAVPObj->GetAVPUDW().m_xLinkedUDWs.Remove( GetGuid() );
	}

	PARENT::Destroy();
}

void ToolsAVP_Waypoint_Object::AddOverlayControls( Asura_Overlay* pxOverlay, Asura_Vector_2& xOffset )
{
	PARENT::AddOverlayControls( pxOverlay, xOffset );

	Asura_Dialog* pxDialogue = new Asura_FunctionTickbox_Dialog( "Lift Waypoint", xOffset, reinterpret_cast< int >( this ), IsLiftWaypoint, SetAsLiftWaypoint );
	pxOverlay->AddDialog( pxDialogue );
	xOffset.y += Asura_Dialog::LargeSpacing();

	pxDialogue = new Asura_Numerical_Dialog( "Radius", xOffset, &GetAVPUDW().m_fRadius, 1.0f, 20.0f, 0.25f, "%0.2f", &m_bChanged );
	pxDialogue->SetIsEnabledFunction( IsLiftWaypoint, reinterpret_cast< int >( this ) );
	pxOverlay->AddDialog( pxDialogue );
	xOffset.y += Asura_Dialog::DefaultSpacing();

	pxDialogue = new Asura_Function_Dialog( "Add Two-Way Link", xOffset, AddLink, reinterpret_cast< int >( this ) );
	pxDialogue->SetIsEnabledFunction( IsLiftWaypoint, reinterpret_cast< int >( this ) );
	pxOverlay->AddDialog( pxDialogue );
	xOffset.y += Asura_Dialog::DefaultSpacing();

	pxDialogue = new Asura_Function_Dialog( "Remove Two-Way Link", xOffset, RemoveLink, reinterpret_cast< int >( this ) );
	pxDialogue->SetIsEnabledFunction( IsLiftWaypoint, reinterpret_cast< int >( this ) );
	pxOverlay->AddDialog( pxDialogue );
	xOffset.y += Asura_Dialog::DefaultSpacing();
}

void ToolsAVP_Waypoint_Object::Update()
{
	PARENT::Update();

	if ( m_bAddLink )
	{
		m_bAddLink = false;
		m_bChanged = true;

		ToolsAVP_Waypoint_Object* pxOther = static_cast< ToolsAVP_Waypoint_Object* >( Tools_Waypoint_Object::GetList()->FindObject( m_uUDWID ) );
		if ( pxOther )
		{
			GetAVPUDW().AddLink( m_uUDWID );
			pxOther->GetAVPUDW().AddLink( GetGuid() );
			Tools_Timed_Message_System::AddMessage( Tools_Timed_Message_System::COLOUR_SUCCESS, "Added two-way link." );
		}
		else
		{
			Tools_Timed_Message_System::AddMessage( Tools_Timed_Message_System::COLOUR_ERROR, "Could not add two-way link." );
		}
	}
	else if ( m_bRemoveLink )
	{
		m_bRemoveLink = false;
		m_bChanged = true;

		ToolsAVP_Waypoint_Object* pxOther = static_cast< ToolsAVP_Waypoint_Object* >( Tools_Waypoint_Object::GetList()->FindObject( m_uUDWID ) );
		if ( pxOther )
		{
			GetAVPUDW().RemoveLink( m_uUDWID );
			pxOther->GetAVPUDW().RemoveLink( GetGuid() );
			Tools_Timed_Message_System::AddMessage( Tools_Timed_Message_System::COLOUR_SUCCESS, "Removed two-way link." );
		}
		else
		{
			Tools_Timed_Message_System::AddMessage( Tools_Timed_Message_System::COLOUR_ERROR, "Could not remove two-way link." );
		}
	}

	if ( m_bChanged )
	{
		m_bChanged = false;
		Tools_SaveAsura::SetWaypointingModified();
	}
}

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

	PARENT::Render();

	if ( GetAVPUDW().IsLiftWaypoint() )
	{
		// Render the radius.
		Render_Sphere( GetUDW().m_xPosition, GetAVPUDW().m_fRadius, 0xffffffff );

		// Render any links.
		for ( Asura_LinkList_ConstIt< u_int > xLinkIter( GetAVPUDW().m_xLinkedUDWs ); !xLinkIter.Done(); xLinkIter.Next() )
		{
			const Tools_Waypoint_Object* pxOther = Tools_Waypoint_Object::GetList()->FindObject( *xLinkIter.GetCurrent() );
			if ( pxOther )
			{
				Tools_Render::RenderCylinder( Position, pxOther->Position, 0.025f, 0xffffffff, false );
			}
		}
	}
}

bool ToolsAVP_Waypoint_Object::WaypointingVerify()
{
	const bool bParentOk = PARENT::WaypointingVerify();

	bool bUsOk = true;
	if ( GetAVPUDW().IsLiftWaypoint() )
	{
		// Make sure our linked UDWs still exist and are lift UDWs.
		for ( Asura_LinkList_It< u_int > xLinkIter( GetAVPUDW().m_xLinkedUDWs ); !xLinkIter.Done(); xLinkIter.Next() )
		{
			ToolsAVP_Waypoint_Object* pxOther = static_cast< ToolsAVP_Waypoint_Object* >( GetList()->FindObject( *xLinkIter.GetCurrent() ) );
			if ( !pxOther )
			{
				Tools_MessageLog::AddMessage( Tools_MessageLog::WAYPOINTING, "Verification", new Tools_MessageLogAction_GotoEntity( GetGuid() ),
					"'%s' could not find linked user defined waypoint.", GetName() );
				bUsOk = false;
			}
			else if ( !pxOther->GetAVPUDW().IsLiftWaypoint() )
			{
				Tools_MessageLog::AddMessage( Tools_MessageLog::WAYPOINTING, "Verification", new Tools_MessageLogAction_GotoEntity( GetGuid() ),
					"'%s' is linked to non-lift user defined waypoint '%s'.", GetName(), pxOther->GetName() );
				bUsOk = false;
			}
		}

		// Make sure we're close enough to a lift node.
		float fDistSqMin = Asura_Maths::LargeFloat;
		for ( Asura_Hashed_List_It< Asura_ServerNode_LiftNode > xNodeIter( Asura_ServerNode_LiftNode::GetList() ); !xNodeIter.Done(); xNodeIter.Next() )
		{
			const float fDistSq = ( Position - xNodeIter.GetCurrent()->GetPosition() ).MagnitudeSquared();
			if ( fDistSq < fDistSqMin )
			{
				fDistSqMin = fDistSq;
			}
		}
		if ( fDistSqMin != Asura_Maths::LargeFloat && fDistSqMin > 15.0f * 15.0f )
		{
			Tools_MessageLog::AddMessage( Tools_MessageLog::WAYPOINTING, "Verification", new Tools_MessageLogAction_GotoEntity( GetGuid() ),
				"'%s' is too far away from the nearest lift node.", GetName() );
			bUsOk = false;
		}
	}

	return bParentOk && bUsOk;
}

bool ToolsAVP_Waypoint_Object::IsLiftWaypoint( int iThis )
{
	ToolsAVP_Waypoint_Object* pxThis = reinterpret_cast< ToolsAVP_Waypoint_Object* >( iThis );
	return pxThis->GetAVPUDW().IsLiftWaypoint();
}

void ToolsAVP_Waypoint_Object::SetAsLiftWaypoint( bool bSet, int iThis )
{
	ToolsAVP_Waypoint_Object* pxThis = reinterpret_cast< ToolsAVP_Waypoint_Object* >( iThis );
	if ( bSet )
	{
		pxThis->GetUDW().m_uFlags |= TOOLS_NAVVERTEX_FLAG_LIFT_UDW;
		pxThis->m_bChanged = true;
	}
	else
	{
		pxThis->GetUDW().m_uFlags &= ~TOOLS_NAVVERTEX_FLAG_LIFT_UDW;
		pxThis->m_bChanged = true;
	}
}

void ToolsAVP_Waypoint_Object::AddLink( int iThis )
{
	ToolsAVP_Waypoint_Object* pxThis = reinterpret_cast< ToolsAVP_Waypoint_Object* >( iThis );

	Tools_Object_System::ChooseObject( "Choose the waypoint to link to:",
		new Tools_ChooseObject_ByType( TOT_UserDefinedWaypointPosition, pxThis->GetUDW().GetID(), &pxThis->m_uUDWID, &pxThis->m_bAddLink ) );
}

void ToolsAVP_Waypoint_Object::RemoveLink( int iThis )
{
	ToolsAVP_Waypoint_Object* pxThis = reinterpret_cast< ToolsAVP_Waypoint_Object* >( iThis );

	Tools_Object_System::ChooseObject( "Choose the waypoint to unlink from:",
		new Tools_ChooseObject_ByType( TOT_UserDefinedWaypointPosition, pxThis->GetUDW().GetID(), &pxThis->m_uUDWID, &pxThis->m_bRemoveLink ) );
}

//////////////////////////////////////////////////////////////////////////////
//
// AVP_Tools_Waypoint_Searcher
//
//////////////////////////////////////////////////////////////////////////////

class AVP_Tools_Waypoint_Searcher
{
public:
    AVP_Tools_Waypoint_Searcher( const Tools_Nav_Vertex* pxWaypoints, const int iNumWaypoints )
    : m_pxWaypoints( pxWaypoints )
    , m_iNumWaypoints( iNumWaypoints )
    , m_pxWaypointTree( NULL )
    {
		Asura_Assert(!s_pxThis, "Creating Waypoint searcher while another exist");
        s_pxThis = this;
        CreateWaypointTree();
    }
    ~AVP_Tools_Waypoint_Searcher()
    {
        DestroyWaypointTree();
        s_pxThis = NULL;
    }

    bool IsOk() const { return ( m_pxWaypointTree != NULL ); }
    int FindAlienWaypointNearestTo( const Asura_Vector_3& xPosition ) const;
	int FindWaypointNearestTo( const Asura_Vector_3& xPosition, int iFlagsFilter ) const;

    enum { INVALID_SEARCH_RESULT = -1 };

private:
    void CreateWaypointTree();
    void DestroyWaypointTree();

    static bool WaypointTreeCreateCallback( u_int uNumWaypoints, Asura_Bounding_Box* pxBBs, Asura_Vector_3* );
    static bool WaypointTreeSearchCallback( u_short usIndex );

	static bool WaypointTreeNearestToCallback( u_short usIndex );

private:
    bool                          m_bOk;
    const Tools_Nav_Vertex* const m_pxWaypoints;
    const int                     m_iNumWaypoints;
    Asura_AABB_Tree*              m_pxWaypointTree;

    static AVP_Tools_Waypoint_Searcher* s_pxThis;
    static Asura_Vector_3               s_xSearchPosition;
    static float                        s_fSearchLeastDistance;
    static int                          s_iSearchBestResult;
	static int							s_iFlagsFilter;
};

AVP_Tools_Waypoint_Searcher* AVP_Tools_Waypoint_Searcher::s_pxThis               = NULL;
Asura_Vector_3               AVP_Tools_Waypoint_Searcher::s_xSearchPosition      = Asura_Maths::ZeroVector3;
float                        AVP_Tools_Waypoint_Searcher::s_fSearchLeastDistance = Asura_Maths::LargeFloat;
int                          AVP_Tools_Waypoint_Searcher::s_iSearchBestResult    = AVP_Tools_Waypoint_Searcher::INVALID_SEARCH_RESULT;
int							 AVP_Tools_Waypoint_Searcher::s_iFlagsFilter		 = 0;


int AVP_Tools_Waypoint_Searcher::FindAlienWaypointNearestTo( const Asura_Vector_3& xPosition ) const
{
    if ( !IsOk() )
    {
        return INVALID_SEARCH_RESULT;
    }

    Asura_Bounding_Box xBB;
    xBB.Reset();
    xBB.Set(xPosition);
    Asura_Vector_3 xP = xPosition;
    xP.x -= 0.5f;
    xP.z -= 0.5f;
    xP.y -= 1.0f;
    xBB.Set(xP);
    xP.x += 1.0f;
    xP.z += 1.0f;
    xP.y += 2.0f;
    xBB.Set(xP);

    s_xSearchPosition      = xPosition;
    s_fSearchLeastDistance = Asura_Maths::LargeFloat;
    s_iSearchBestResult    = INVALID_SEARCH_RESULT;

    m_pxWaypointTree->TestTree( xBB, WaypointTreeSearchCallback );

    return s_iSearchBestResult;
}

int AVP_Tools_Waypoint_Searcher::FindWaypointNearestTo( const Asura_Vector_3& xPosition, int iFlagsFilter ) const
{
	if ( !IsOk() )
	{
		return INVALID_SEARCH_RESULT;
	}


	// create some random size bounding box
	Asura_Bounding_Box xBB;
	xBB.Reset();
	xBB.Set(xPosition);
	Asura_Vector_3 xP = xPosition;
	xP.x -= 0.5f;
	xP.z -= 0.5f;
	xP.y -= 1.0f;
	xBB.Set(xP);
	xP.x += 1.0f;
	xP.z += 1.0f;
	xP.y += 2.0f;
	xBB.Set(xP);

	s_xSearchPosition      = xPosition;
	s_fSearchLeastDistance = Asura_Maths::LargeFloat;
	s_iSearchBestResult    = INVALID_SEARCH_RESULT;
	s_iFlagsFilter = iFlagsFilter;

	m_pxWaypointTree->TestTree( xBB, WaypointTreeNearestToCallback );

	return s_iSearchBestResult;
}


void AVP_Tools_Waypoint_Searcher::CreateWaypointTree()
{
    m_pxWaypointTree = Asura_AABB_Tree_Creator::Create( m_iNumWaypoints, WaypointTreeCreateCallback );
}

void AVP_Tools_Waypoint_Searcher::DestroyWaypointTree()
{
    delete m_pxWaypointTree;
    m_pxWaypointTree = NULL;
}

bool AVP_Tools_Waypoint_Searcher::WaypointTreeCreateCallback( u_int uNumWaypoints, Asura_Bounding_Box* pxBBs, Asura_Vector_3* )
{
    Asura_Bounding_Box xTempBB;
    Asura_Matrix_3x3   xOrientation;

    for ( int i = 0; i < s_pxThis->m_iNumWaypoints; ++i )
    {
        const float fRadius = s_pxThis->m_pxWaypoints[ i ].m_fRadius;

        const Asura_Vector_3 xMin( -fRadius, -Asura_Maths::Min( 2.0f, fRadius ), -fRadius );
        const Asura_Vector_3 xMax(  fRadius, fRadius, fRadius );

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

        Asura_Maths::OrientationFromNormal( xOrientation, s_pxThis->m_pxWaypoints[ i ].m_xNormal );
        pxBBs[ i ].FromOBB( xTempBB, xOrientation );
        pxBBs[ i ].Displace( s_pxThis->m_pxWaypoints[ i ].m_xPosition );
    }

    // Let tree creator class calculate centres for us.
    return false;
}

bool AVP_Tools_Waypoint_Searcher::WaypointTreeSearchCallback( u_short usIndex )
{
    if ( !( s_pxThis->m_pxWaypoints[ usIndex ].m_iFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN ) )
    {
        return true;
    }

    const Asura_Vector_3 xDisp = s_xSearchPosition - s_pxThis->m_pxWaypoints[ usIndex ].m_xPosition;
    const float          fDist = xDisp.Magnitude() - s_pxThis->m_pxWaypoints[ usIndex ].m_fRadius;

    // Prefer larger waypoints rather than smaller.
    if ( fDist < 0.0f && fDist < s_fSearchLeastDistance )
    {
        s_fSearchLeastDistance = fDist;
        s_iSearchBestResult    = usIndex;
    }

    return true;
}

bool AVP_Tools_Waypoint_Searcher::WaypointTreeNearestToCallback( u_short usIndex )
{
	if ( !( s_pxThis->m_pxWaypoints[ usIndex ].m_iFlags & s_iFlagsFilter ) )
	{
		return true;
	}

	const Asura_Vector_3 xDisp = s_xSearchPosition - s_pxThis->m_pxWaypoints[ usIndex ].m_xPosition;
	const float          fDist = xDisp.Magnitude();

	// Prefer larger waypoints rather than smaller.
	if ( fDist < s_pxThis->m_pxWaypoints[ usIndex ].m_fRadius && fDist < s_fSearchLeastDistance )
	{
		s_fSearchLeastDistance = fDist;
		s_iSearchBestResult    = usIndex;
	}
	return true;
}


//////////////////////////////////////////////////////////////////////////////
//
// ToolsAVP_Mode_Waypointing
//
//////////////////////////////////////////////////////////////////////////////

ToolsAVP_Mode_Waypointing* ToolsAVP_Mode_Waypointing::s_pxThis = NULL;
Asura_Vector_2  ToolsAVP_Mode_Waypointing::s_xAVPOverlayPosition          = Asura_Vector_2( 5.0f, 5.0f );
bool            ToolsAVP_Mode_Waypointing::s_bShowEnvHooks                = false;
bool            ToolsAVP_Mode_Waypointing::s_bShowEnvHooksChanged         = false;
bool            ToolsAVP_Mode_Waypointing::s_bShowGS2Objects			  = false;
bool            ToolsAVP_Mode_Waypointing::s_bShowGS2ObjectsChanged		  = false;
int				ToolsAVP_Mode_Waypointing::s_iShowDataType				  = SDT_ALIEN;
bool            ToolsAVP_Mode_Waypointing::s_bOnlyShowForwardFacingData   = false;
bool            ToolsAVP_Mode_Waypointing::s_bRenderColoursAsFWC          = false;
bool            ToolsAVP_Mode_Waypointing::s_bShowFailedLinkRoutes        = false;
u_int           ToolsAVP_Mode_Waypointing::s_uFailedLinkRouteIndex        = 0;
bool            ToolsAVP_Mode_Waypointing::s_bFailedLinkRouteIndexChanged = false;
u_int           ToolsAVP_Mode_Waypointing::s_uFailedLinkPositionIndex     = 0;
float           ToolsAVP_Mode_Waypointing::s_fFLPIKeyDownTimer            = 0.0f;
float           ToolsAVP_Mode_Waypointing::s_fFLPIKeyRepeatTimer          = 0.0f;
Asura_Vector_3* ToolsAVP_Mode_Waypointing::s_pxPhysicsVelocities          = NULL;
Asura_Quat*     ToolsAVP_Mode_Waypointing::s_pxPhysicsOrientations        = NULL;
int				ToolsAVP_Mode_Waypointing::s_iMinIslandSize				  = 2;
Asura_Bounding_Box ToolsAVP_Mode_Waypointing::s_xPhysicsBoundingBox;
bool			ToolsAVP_Mode_Waypointing::s_bIncludeLoFData			  = true;
bool			ToolsAVP_Mode_Waypointing::s_bShowGamePlayVolumes		  = false;
Asura_String    ToolsAVP_Mode_Waypointing::s_xNumFailedLinkRoutesString;
u_int           ToolsAVP_Mode_Waypointing::s_uNumFailedLinkRoutes         = 0;
ToolsAVP_Mode_Waypointing::LinkRouteData ToolsAVP_Mode_Waypointing::s_axFailedLinkRoutes[ MAX_FAILED_LINK_ROUTES ];

float ToolsAVP_Mode_Waypointing::s_fAttractorAngleTolerance          = 35.0f;
float ToolsAVP_Mode_Waypointing::s_fWaypointVolumeMinAngle           = 50.0f;
float ToolsAVP_Mode_Waypointing::s_fWaypointVolumeMaxAngle           = 160.0f;
float ToolsAVP_Mode_Waypointing::s_fLinkMaxAngle                     = 110.0f;
float ToolsAVP_Mode_Waypointing::s_fTransitionLinkMinAngle           = 60.0f;
float ToolsAVP_Mode_Waypointing::s_fTransitionLinkMaxAngleToPerfect  = 35.0f;
float ToolsAVP_Mode_Waypointing::s_fSurfaceAngleTolerance            = 15.0f;
float ToolsAVP_Mode_Waypointing::s_fMinTransitionLinkDistance        = 0.5f;
float ToolsAVP_Mode_Waypointing::s_fIdealTransitionLinkDistance      = 2.5f;
float ToolsAVP_Mode_Waypointing::s_fMaxTransitionLinkDistance        = 4.0f;
float ToolsAVP_Mode_Waypointing::s_fDistanceAngleScoreWeighting      = 0.5f;
float ToolsAVP_Mode_Waypointing::s_fTransitionLinkMinAcceptableScore = 0.75f;

float ToolsAVP_Mode_Waypointing::s_fMinFloorCeilingJumpDistance      = 2.95f;
float ToolsAVP_Mode_Waypointing::s_fMaxFloorCeilingJumpDistance      = 4.55f;
float ToolsAVP_Mode_Waypointing::s_fMinCeilingFloorFallDistance      = 2.95f;
float ToolsAVP_Mode_Waypointing::s_fMaxCeilingFloorFallDistance      = 8.05f;
float ToolsAVP_Mode_Waypointing::s_fMinWallWallJumpDistance          = 2.95f;
float ToolsAVP_Mode_Waypointing::s_fMaxWallWallJumpDistance          = 6.05f;
float ToolsAVP_Mode_Waypointing::s_fJumpTargetSurfaceAngleTolerance  = 15.0f;

float                        ToolsAVP_Mode_Waypointing::s_fMinAlienWaypointRadius = 0.35f; // 0.745f; - was causing problems is branch WP generation...
Asura_Physics_Object*        ToolsAVP_Mode_Waypointing::s_pxHumanoidPhysicsObject = NULL;
Asura_Collision_Box          ToolsAVP_Mode_Waypointing::s_xAlienTestCollisionBox( -0.35f, 0.35f, -0.7f, 0.0f, -0.35f, 0.35f );
Asura_Collision_Box          ToolsAVP_Mode_Waypointing::s_xHumanoidTestCollisionBox( -0.3f, 0.3f, -2.0f, 0.0f, -0.3f, 0.3f );
AVP_Tools_Waypoint_Searcher* ToolsAVP_Mode_Waypointing::s_pxWaypointSearcher = NULL;

bool  ToolsAVP_Mode_Waypointing::s_bOutlineModulePolyIndex = false;
u_int ToolsAVP_Mode_Waypointing::s_uOutlineModuleIndex = 0;
u_int ToolsAVP_Mode_Waypointing::s_uOutlinePolyIndex   = 0;

const char* ToolsAVP_Mode_Waypointing::s_aszAVPTabNames[ AVP_TAB_NUM_TAB_NAMES ] =
{
    "Settings",
    "Rendering",
    "Debugging"
};

const char* ToolsAVP_Mode_Waypointing::s_aszAVPSDTNames[ SDT_END_ENUM ] =
{
	"Show Alien Data Only",
	"Show Predator Data Only",
	"Show Humanoid Data Only",
	"Show All Character Data"
};

Asura_LinkList< ToolsAVP_Mode_Waypointing::Gs2WayPointID > ToolsAVP_Mode_Waypointing::s_xAlienInVentGS2;
Asura_LinkList< ToolsAVP_Mode_Waypointing::Gs2WayPointID > ToolsAVP_Mode_Waypointing::s_xAlienOutVentGS2;

void ToolsAVP_Mode_Waypointing::InitialiseMode()
{
	s_pxThis = this;

	PARENT::InitialiseMode();

    s_xNumFailedLinkRoutesString = "There are no failed link routes.";
    s_pxPhysicsVelocities        = new Asura_Vector_3[ MAX_STORED_PHYSICS_POSITIONS ];
    s_pxPhysicsOrientations      = new Asura_Quat[ MAX_STORED_PHYSICS_POSITIONS ];

    if ( s_bShowEnvHooks )
    {
        Tools_Object_Attractor_Node::CreateAttractorObjects();
    }

	if ( s_bShowGS2Objects && !s_bToolsEntitiesCreated )
	{
		CreateToolsEntities();

		for (LIF<Tools_Object*> xObjectLIF(&Tools_Object_System::ObjectList);!xObjectLIF.done();xObjectLIF.next())
		{
			if ( xObjectLIF()->Type == TOT_Entity_AVPGS2Node ||
				xObjectLIF()->Type == TOT_Entity_AVPGS2Spline ||
				xObjectLIF()->Type == TOT_Entity_AVPGS2Attractor ||
				xObjectLIF()->Type == TOT_Entity_GS2Node ||
				xObjectLIF()->Type == TOT_Entity_GS2Spline ||
				xObjectLIF()->Type == TOT_Entity_GS2Attractor )
			{
				xObjectLIF()->HiddenFlags &= ~ObjectHideFlag_Mode;
			}
		}
	}

    CreateAVPOverlay();

	// Ensure the Menu handle is valid as it will crash Batch waypointing if not!
	if ( Tools_Mode_Waypointing::s_iMenuHandle != ASURA_INVALID_HANDLE )
	{
		Asura_Menubar_Title* pxTitle = Asura_Menubar_System::GetTitle( Tools_Mode_Waypointing::s_iMenuHandle, "Overlays" );
		if ( pxTitle )
		{
			pxTitle->AddItem( "AVP Waypointing", ToggleAVPWaypointingOverlay, AVPWaypointingOverlayIsOn );
		}
	}
}

void ToolsAVP_Mode_Waypointing::ShutdownMode()
{
    DestroyAVPOverlay();

    if ( s_bShowEnvHooks )
    {
        Tools_Object_Attractor_Node::DeleteAttractorObjects();
    }

    // Destroy any failed link routes.
    for ( u_int u = 0; u < s_uNumFailedLinkRoutes; ++u )
    {
        s_axFailedLinkRoutes[ u ].ShutdownArrays();
    }
    delete [] s_pxPhysicsVelocities;
    s_pxPhysicsVelocities = NULL;
    delete [] s_pxPhysicsOrientations;
    s_pxPhysicsOrientations = NULL;
    s_uNumFailedLinkRoutes  = 0;

	PARENT::ShutdownMode();

	s_pxThis = NULL;
}

void ToolsAVP_Mode_Waypointing::Update()
{
    PARENT::Update();

    if ( s_bFailedLinkRouteIndexChanged )
    {
        s_bFailedLinkRouteIndexChanged = false;
        s_uFailedLinkPositionIndex     = 0;
    }

    if ( s_bShowEnvHooksChanged )
    {
        s_bShowEnvHooksChanged = false;
        if ( s_bShowEnvHooks )
        {
            Tools_Object_Attractor_Node::CreateAttractorObjects();
        }
        else
        {
            Tools_Object_Attractor_Node::DeleteAttractorObjects();
        }
    }

	if ( s_bShowGS2ObjectsChanged )
	{
		s_bShowGS2ObjectsChanged = false;

		if ( s_bShowGS2Objects && !s_bToolsEntitiesCreated )
		{
			CreateToolsEntities();
		}

		for (LIF<Tools_Object*> xObjectLIF(&Tools_Object_System::ObjectList);!xObjectLIF.done();xObjectLIF.next())
		{
			if ( xObjectLIF()->Type == TOT_Entity_AVPGS2Node ||
				xObjectLIF()->Type == TOT_Entity_AVPGS2Spline ||
				xObjectLIF()->Type == TOT_Entity_AVPGS2Attractor ||
				xObjectLIF()->Type == TOT_Entity_GS2Node ||
				xObjectLIF()->Type == TOT_Entity_GS2Spline ||
				xObjectLIF()->Type == TOT_Entity_GS2Attractor )
			{
				if ( s_bShowGS2Objects )
				{
					xObjectLIF()->HiddenFlags &= ~ObjectHideFlag_Mode;
				}
				else
				{
					xObjectLIF()->HiddenFlags |= ObjectHideFlag_Mode;
				}
			}
		}
	}

    if ( s_bShowFailedLinkRoutes && s_uFailedLinkRouteIndex < s_uNumFailedLinkRoutes )
    {
        if ( Asura_UserInput::DebouncedKeyPressed( 'y' ) )
        {
            if ( s_uFailedLinkRouteIndex > 0 )
            {
                --s_uFailedLinkRouteIndex;
                s_uFailedLinkPositionIndex = 0;
            }
        }
        if ( Asura_UserInput::DebouncedKeyPressed( 'u' ) )
        {
            if ( s_uFailedLinkRouteIndex < s_uNumFailedLinkRoutes - 1 )
            {
                ++s_uFailedLinkRouteIndex;
                s_uFailedLinkPositionIndex = 0;
            }
        }

        if ( Asura_UserInput::DebouncedKeyPressed( 'g' ) )
        {
            s_uFailedLinkPositionIndex = 0;
        }

        bool bDecrementLinkPositionIndex = false;
        bool bIncrementLinkPositionIndex = false;
        // Debounced key stuff.
        if ( Asura_UserInput::DebouncedKeyPressed( 'h' ) )
        {
            bDecrementLinkPositionIndex = true;
        }
        if ( Asura_UserInput::DebouncedKeyPressed( 'j' ) )
        {
            bIncrementLinkPositionIndex = true;
        }
        // Key repeat stuff.
        if ( Asura_UserInput::KeyPressed( 'h' ) )
        {
            s_fFLPIKeyDownTimer   += Asura_Timers::GetGameTimeSlice();
            s_fFLPIKeyRepeatTimer += Asura_Timers::GetGameTimeSlice();

            if ( s_fFLPIKeyDownTimer > 0.5f )
            {
                if ( s_fFLPIKeyRepeatTimer > 0.125f )
                {
                    s_fFLPIKeyRepeatTimer = 0.0f;

                    bDecrementLinkPositionIndex = true;
                }
            }
        }
        else if ( Asura_UserInput::KeyPressed( 'j' ) )
        {
            s_fFLPIKeyDownTimer   += Asura_Timers::GetGameTimeSlice();
            s_fFLPIKeyRepeatTimer += Asura_Timers::GetGameTimeSlice();

            if ( s_fFLPIKeyDownTimer > 0.5f )
            {
                if ( s_fFLPIKeyRepeatTimer > 0.125f )
                {
                    s_fFLPIKeyRepeatTimer = 0.0f;

                    bIncrementLinkPositionIndex = true;
                }
            }
        }
        else
        {
            s_fFLPIKeyDownTimer   = 0.0f;
            s_fFLPIKeyRepeatTimer = 0.0f;
        }

        if ( bDecrementLinkPositionIndex )
        {
            if ( s_uFailedLinkPositionIndex > 0 )
            {
                --s_uFailedLinkPositionIndex;
            }
        }
        else if ( bIncrementLinkPositionIndex )
        {
            if ( s_uFailedLinkPositionIndex < s_axFailedLinkRoutes[ s_uFailedLinkRouteIndex ].m_uEntryCount - 1 )
            {
                ++s_uFailedLinkPositionIndex;
            }
        }

        s_uFailedLinkPositionIndex = Asura_Maths::Min( s_uFailedLinkPositionIndex, s_axFailedLinkRoutes[ s_uFailedLinkRouteIndex ].m_uEntryCount - 1 );
    }
}

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

    PARENT::RenderWorld_PostToneMap();

    // Render the failed link, if necessary.
    if ( s_bShowFailedLinkRoutes && s_uFailedLinkRouteIndex < s_uNumFailedLinkRoutes )
    {
        const LinkRouteData& xLRD = s_axFailedLinkRoutes[ s_uFailedLinkRouteIndex ];

		Asura_Matrix_3x3 xInitialOrientation;
		xLRD.m_pxOrientationArray[ 0 ].ToMatrix( xInitialOrientation );
		const Asura_Vector_3 xInitialForward( xInitialOrientation.e13, xInitialOrientation.e23, xInitialOrientation.e33 );

        Asura_Matrix_3x3 xOrientation;
        xLRD.m_pxOrientationArray[ s_uFailedLinkPositionIndex ].ToMatrix( xOrientation );
        Asura_Render::RenderBB( xLRD.m_xBB, xLRD.m_pxPositionArray[ s_uFailedLinkPositionIndex ], xOrientation );
        Asura_Render::RenderLine(
            xLRD.m_pxPositionArray[ s_uFailedLinkPositionIndex ],
            xLRD.m_pxPositionArray[ s_uFailedLinkPositionIndex ] + xLRD.m_pxVelocityArray[ s_uFailedLinkPositionIndex ],
            0xffffffff );

		Asura_Render::RenderLine( xLRD.m_pxPositionArray[ s_uFailedLinkPositionIndex ],
			xLRD.m_pxPositionArray[ s_uFailedLinkPositionIndex ] + xInitialForward * 2.0f, 0xff00ff00 );
		const Asura_Vector_3 xCurrentForward( xOrientation.e13, xOrientation.e23, xOrientation.e33 );
		Asura_Render::RenderLine( xLRD.m_pxPositionArray[ s_uFailedLinkPositionIndex ],
			xLRD.m_pxPositionArray[ s_uFailedLinkPositionIndex ] + xCurrentForward * 2.0f, 0xff0000ff );

        Render_Sphere( s_axFailedLinkRoutes[ s_uFailedLinkRouteIndex ].m_xStartPosition, 1.0f, 0x1f00ff00 );
        Render_Sphere( s_axFailedLinkRoutes[ s_uFailedLinkRouteIndex ].m_xFinishPosition, 1.0f, 0x1fff0000 );
    }

	if ( s_bOutlineModulePolyIndex )
	{
		const Asura_CollisionMesh* pxCollMesh = Asura_Module_System::GetCollisionMeshForModule( s_uOutlineModuleIndex );
		if ( pxCollMesh && s_uOutlinePolyIndex < pxCollMesh->GetNumberOfPolys() )
		{
			const Asura_Vector_3& xModuleCentre = Asura_Module_System::GetPositionOfModule( s_uOutlineModuleIndex );

			Asura_Vector_3 axVertices[ 4 ];
			pxCollMesh->GetVertices( axVertices, s_uOutlinePolyIndex );
			axVertices[ 0 ] += xModuleCentre;
			axVertices[ 1 ] += xModuleCentre;
			axVertices[ 2 ] += xModuleCentre;

			if ( pxCollMesh->IsQuad( s_uOutlinePolyIndex ) )
			{
				axVertices[ 3 ] += xModuleCentre;

				Tools_Render::RenderCylinder( axVertices[ 0 ], axVertices[ 1 ], 0.1f, 0xffff0000, false );
				Tools_Render::RenderCylinder( axVertices[ 1 ], axVertices[ 2 ], 0.1f, 0xffff0000, false );
				Tools_Render::RenderCylinder( axVertices[ 2 ], axVertices[ 3 ], 0.1f, 0xffff0000, false );
				Tools_Render::RenderCylinder( axVertices[ 3 ], axVertices[ 0 ], 0.1f, 0xffff0000, false );
			}
			else
			{
				Tools_Render::RenderCylinder( axVertices[ 0 ], axVertices[ 1 ], 0.1f, 0xffff0000, false );
				Tools_Render::RenderCylinder( axVertices[ 1 ], axVertices[ 2 ], 0.1f, 0xffff0000, false );
				Tools_Render::RenderCylinder( axVertices[ 2 ], axVertices[ 0 ], 0.1f, 0xffff0000, false );
			}
		}
	}

	if ( s_bShowGamePlayVolumes )
	{
		for (Asura_Hashed_List_It<AVP_ServerEntity_GamePlayVolume> xIt(AVP_ServerEntity_GamePlayVolume::GetList()); !xIt.Done(); xIt.Next())
		{
			AVP_ServerEntity_GamePlayVolume* pxGamePlayVolume = xIt.GetCurrent();

			if ( pxGamePlayVolume )
			{
				pxGamePlayVolume->RenderForWaypointingMode();
			}
		}
	}

#ifdef AVP_INCLUDE_LOF_DEBUG_INFO
	int iSelectedWaypoint;
	if ( Tools_Waypoint_Selection::GetSelectedWaypoint( iSelectedWaypoint ) )
	{
		const Asura_AI_NavVertex* pxWP = Asura_AI_Navigation_System::GetWaypoint( iSelectedWaypoint );

		for ( u_int u = 0; u < AVP_Waypoint_Manager::GetNumSectors(); ++u )
		{
			Asura_Vector_3 xLimitOne, xLimitTwo;
			AVP_Waypoint_Manager::GetSectorLimits( u, xLimitOne, xLimitTwo );

			const Asura_Vector_3 xBasePosition = pxWP->GetPosition() + ( pxWP->GetNormal() * 1.2f );

			Asura_Render::RenderLine( xBasePosition, xBasePosition + xLimitOne * 5.0f, 0xffffffff );
			Asura_Render::RenderLine( xBasePosition, xBasePosition + xLimitTwo * 5.0f, 0xffffffff );

			const u_short usMinLOFWPID = AVP_Waypoint_Manager::GetMinLOFWPID( iSelectedWaypoint, u );
			if ( usMinLOFWPID != ASURA_INVALID_WAYPOINT_ID )
			{
				const Asura_AI_NavVertex* pxMinWP = Asura_AI_Navigation_System::GetWaypoint( usMinLOFWPID );
				if ( pxMinWP )
				{
					Render_Sphere( pxMinWP->GetPosition(), pxMinWP->GetRadius(), 0xffff0000 );

					Asura_Vector_3 xToMinWP = pxMinWP->GetPosition() - xBasePosition;
					xToMinWP.Normalise();
					Asura_Render::RenderLine( xBasePosition,
						xBasePosition + xToMinWP * AVP_Waypoint_Manager::GetMinLOFValue( iSelectedWaypoint, u ),
						0xffff0000 );
				}
			}
			const u_short usMaxLOFWPID = AVP_Waypoint_Manager::GetMaxLOFWPID( iSelectedWaypoint, u );
			if ( usMaxLOFWPID != ASURA_INVALID_WAYPOINT_ID )
			{
				const Asura_AI_NavVertex* pxMaxWP = Asura_AI_Navigation_System::GetWaypoint( usMaxLOFWPID );
				if ( pxMaxWP )
				{
					Render_Sphere( pxMaxWP->GetPosition(), pxMaxWP->GetRadius(), 0xff00ff00 );

					Asura_Vector_3 xToMaxWP = pxMaxWP->GetPosition() - xBasePosition;
					xToMaxWP.Normalise();
					Asura_Render::RenderLine( xBasePosition,
						xBasePosition + xToMaxWP * AVP_Waypoint_Manager::GetMaxLOFValue( iSelectedWaypoint, u ),
						0xff00ff00 );
				}
			}
		}
	}
#endif
}

void ToolsAVP_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 );

		Asura_Vector_3 xToWP = pxBaseWP->m_xPosition - Asura_Camera::GetCameraData( 0 ).m_xPosition;
		const float    fToWP = xToWP.Normalise();

        if ( s_bShowOnlyInZone )
        {
            if ( pxBaseWP->m_usZone != s_iZoneIDToRender )
            {
                continue;
            }
        }
        if ( s_bShowOnlyInRange )
        {
            if ( fToWP > s_fRangeToRender )
            {
                continue;
            }
        }
        if ( s_bOnlyShowForwardFacingData )
        {
            if ( ( pxBaseWP->GetNormal() * xToWP ) > 0.0f )
            {
                continue;
            }
        }

        for ( int c = 0; c < pxBaseWP->m_ucNumConnections; ++c )
        {
			if ( s_iShowDataType == SDT_ALIEN )
			{
				if ( !( pxBaseWP->m_pxConnections[ c ].m_usFlags & AVP_Navigation::AVP_AI_CONNECTION_FLAG_ALIEN ) )
				{
					continue;
				}
			}
			else if ( s_iShowDataType == SDT_PREDATOR )
			{
				if ( !( pxBaseWP->m_pxConnections[ c ].m_usFlags & AVP_Navigation::AVP_AI_CONNECTION_FLAG_PREDATOR ) )
				{
					continue;
				}
			}
			else if ( s_iShowDataType == SDT_HUMANOID )
			{
				if ( !( pxBaseWP->m_pxConnections[ c ].m_usFlags & AVP_Navigation::AVP_AI_CONNECTION_FLAG_HUMANOID ) )
				{
					continue;
				}
			}

            const int iTargetWP = pxBaseWP->m_pxConnections[ c ].m_usIndex;
            const Asura_AI_NavVertex* pxTargetWP = Asura_AI_Navigation_System::GetWaypoint( iTargetWP );

            const Asura_Vector_3& xP1 = pxBaseWP->m_xPosition;
            const Asura_Vector_3& xP2 = pxTargetWP->m_xPosition;

            bool bIsTwoWayLink = false;
            for ( int k = 0; k < pxTargetWP->m_ucNumConnections; ++k )
            {
                if ( pxTargetWP->m_pxConnections[ k ].m_usIndex == i )
                {
					if ( s_iShowDataType == SDT_ALIEN )
					{
						bIsTwoWayLink = ( ( pxTargetWP->m_pxConnections[ k ].m_usFlags & AVP_Navigation::AVP_AI_CONNECTION_FLAG_ALIEN ) != 0 );
					}
					else if ( s_iShowDataType == SDT_PREDATOR )
					{
						bIsTwoWayLink = ( ( pxTargetWP->m_pxConnections[ k ].m_usFlags & AVP_Navigation::AVP_AI_CONNECTION_FLAG_PREDATOR ) != 0 );
					}
					else if ( s_iShowDataType == SDT_HUMANOID )
					{
						bIsTwoWayLink = ( ( pxTargetWP->m_pxConnections[ k ].m_usFlags & AVP_Navigation::AVP_AI_CONNECTION_FLAG_HUMANOID ) != 0 );
					}
					else
					{
						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 ToolsAVP_Mode_Waypointing::RenderWaypoints() const
{
    extern void Render_Sphere( const Asura_Vector_3& xCentre, float fRadius, int iColour );

    const Asura_Vector_3 xCameraForward( Asura_Camera::GetCameraData( 0 ).m_xOrientation.e13,
                                         Asura_Camera::GetCameraData( 0 ).m_xOrientation.e23,
                                         Asura_Camera::GetCameraData( 0 ).m_xOrientation.e33 );

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

        if ( s_bShowOnlyInZone )
        {
            if ( pxWP->m_usZone != s_iZoneIDToRender )
            {
                continue;
            }
        }
        if ( s_bShowOnlyInRange )
        {
            if ( ( pxWP->m_xPosition - Asura_Camera::GetCameraData( 0 ).m_xPosition ).MagnitudeSquared() > s_fRangeToRender * s_fRangeToRender )
            {
                continue;
            }
        }
        if ( s_iShowDataType == SDT_ALIEN )
        {
            if ( !( pxWP->m_usFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN ) )
            {
                continue;
            }
        }
		else if ( s_iShowDataType == SDT_PREDATOR )
		{
			if ( !( pxWP->m_usFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_PREDATOR ) )
			{
				continue;
			}
		}
		else if ( s_iShowDataType == SDT_HUMANOID )
		{
			if ( !( pxWP->m_usFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_HUMANOID ) )
			{
				continue;
			}
		}
        if ( s_bOnlyShowForwardFacingData )
        {
            if ( ( pxWP->GetNormal() * xCameraForward ) > 0.0f )
            {
                continue;
            }
        }

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

void ToolsAVP_Mode_Waypointing::RenderTestLinks() const
{
    const Asura_Vector_3 xCameraForward( Asura_Camera::GetCameraData( 0 ).m_xOrientation.e13,
                                         Asura_Camera::GetCameraData( 0 ).m_xOrientation.e23,
                                         Asura_Camera::GetCameraData( 0 ).m_xOrientation.e33 );

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

		if ( s_bShowOnlyInRange )
		{
			if ( ( pxBaseWP->m_xPosition - Asura_Camera::GetCameraData( 0 ).m_xPosition ).MagnitudeSquared() > s_fRangeToRender * s_fRangeToRender )
			{
				continue;
			}
		}
		if ( s_bOnlyShowForwardFacingData )
		{
			if ( ( pxBaseWP->m_xNormal * xCameraForward ) > 0.0f )
			{
				continue;
			}
		}

		for ( int c = 0; c < pxBaseWP->GetNumLinks(); ++c )
		{
			if ( s_iShowDataType == SDT_ALIEN )
			{
				if ( !( pxBaseWP->m_xLinks[ c ].m_uFlags & AVP_Navigation::AVP_AI_CONNECTION_FLAG_ALIEN ) )
				{
					continue;
				}
			}
			else if ( s_iShowDataType == SDT_PREDATOR )
			{
				if ( !( pxBaseWP->m_xLinks[ c ].m_uFlags & AVP_Navigation::AVP_AI_CONNECTION_FLAG_PREDATOR ) )
				{
					continue;
				}
			}
			else if ( s_iShowDataType == SDT_HUMANOID )
			{
				if ( !( pxBaseWP->m_xLinks[ c ].m_uFlags & AVP_Navigation::AVP_AI_CONNECTION_FLAG_HUMANOID ) )
				{
					continue;
				}
			}

			const int iTargetWP = pxBaseWP->m_xLinks[ c ].m_iIndex;
			const Tools_Nav_Vertex* pxTargetWP = &s_pxTestWaypointStorage[ iTargetWP ];

			const Asura_Vector_3& xP1 = pxBaseWP->m_xPosition;
			const Asura_Vector_3& xP2 = pxTargetWP->m_xPosition;

			// check for a link back to this waypoint
			bool bIsTwoWayLink = false;
			for ( int k = 0; k < pxTargetWP->GetNumLinks(); ++k )
			{
				if ( pxTargetWP->m_xLinks[ k ].m_iIndex == i )
				{
					if ( s_iShowDataType == SDT_ALIEN )
					{
						bIsTwoWayLink = ( ( pxTargetWP->m_xLinks[ k ].m_uFlags & AVP_Navigation::AVP_AI_CONNECTION_FLAG_ALIEN ) != 0 );
					}
					else if ( s_iShowDataType == SDT_PREDATOR )
					{
						bIsTwoWayLink = ( ( pxTargetWP->m_xLinks[ k ].m_uFlags & AVP_Navigation::AVP_AI_CONNECTION_FLAG_PREDATOR ) != 0 );
					}
					else if ( s_iShowDataType == SDT_HUMANOID )
					{
						bIsTwoWayLink = ( ( pxTargetWP->m_xLinks[ k ].m_uFlags & AVP_Navigation::AVP_AI_CONNECTION_FLAG_HUMANOID ) != 0 );
					}
					else
					{
						bIsTwoWayLink = true;
					}

					break;
				}
			}

			const u_short usLinkFlags = static_cast< u_short >( pxBaseWP->m_xLinks[ c ].m_uFlags );
			if ( usLinkFlags == 0 && bIsTwoWayLink )
			{
				// Standard link colour - white.
				Asura_Render::RenderLine( xP1, xP2, 0xffffffff );
			}
			else
			{
				Asura_Vector_3 xStartColour, xEndColour;
				GetLinkColour( pxBaseWP->m_xLinks[ c ].m_uFlags, bIsTwoWayLink, 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( xP1, xP2, uLinkColour );
				}
				else
				{
					RenderLine( xP1, xP2, xStartColour, xEndColour );
				}
			}
		}
	}
}

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

    const Asura_Vector_3 xCameraForward( Asura_Camera::GetCameraData( 0 ).m_xOrientation.e13,
                                         Asura_Camera::GetCameraData( 0 ).m_xOrientation.e23,
                                         Asura_Camera::GetCameraData( 0 ).m_xOrientation.e33 );

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

		Asura_Vector_3 xToWP = pxWP->m_xPosition - Asura_Camera::GetCameraData( 0 ).m_xPosition;
		const float    fToWP = xToWP.Normalise();

		const u_short usFlags = static_cast< u_short >( pxWP->m_iFlags );

		if ( s_bShowOnlyInRange )
		{
			if ( fToWP > s_fRangeToRender )
			{
				continue;
			}
		}
		if ( s_iShowDataType == SDT_ALIEN )
		{
			if ( !( usFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN ) )
			{
				continue;
			}
		}
		else if ( s_iShowDataType == SDT_PREDATOR )
		{
			if ( !( usFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_PREDATOR ) )
			{
				continue;
			}
		}
		else if ( s_iShowDataType == SDT_HUMANOID )
		{
			if ( !( usFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_HUMANOID ) )
			{
				continue;
			}
		}
		if ( s_bOnlyShowForwardFacingData )
		{
			if ( ( pxWP->m_xNormal * xToWP ) > 0.0f )
			{
				continue;
			}
		}

		int iColour = 0x1fffffff;
		GetWaypointColour( usFlags, iColour );
		Render_Sphere( pxWP->m_xPosition, pxWP->m_fRadius, iColour );
	}
}

void ToolsAVP_Mode_Waypointing::RenderWaypointNormals() const
{
    const Asura_Vector_3 xCameraForward( Asura_Camera::GetCameraData( 0 ).m_xOrientation.e13,
                                         Asura_Camera::GetCameraData( 0 ).m_xOrientation.e23,
                                         Asura_Camera::GetCameraData( 0 ).m_xOrientation.e33 );

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

		Asura_Vector_3 xToWP = pxWP->m_xPosition - Asura_Camera::GetCameraData( 0 ).m_xPosition;
		const float    fToWP = xToWP.Normalise();

		bool bShouldRenderWP = true;
		if ( s_bShowOnlyInZone )
		{
			bShouldRenderWP &= ( pxWP->m_usZone == s_iZoneIDToRender );
		}
		if ( s_bShowOnlyInRange )
		{
			bShouldRenderWP &= ( fToWP < s_fRangeToRender );
		}
		if ( s_iShowDataType == SDT_ALIEN )
		{
			if ( !( pxWP->m_usFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN ) )
			{
				continue;
			}
		}
		else if ( s_iShowDataType == SDT_PREDATOR )
		{
			if ( !( pxWP->m_usFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_PREDATOR ) )
			{
				continue;
			}
		}
		else if ( s_iShowDataType == SDT_HUMANOID )
		{
			if ( !( pxWP->m_usFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_HUMANOID ) )
			{
				continue;
			}
		}
		if ( s_bOnlyShowForwardFacingData )
		{
			if ( ( pxWP->GetNormal() * xToWP ) > 0.0f )
			{
				continue;
			}
		}

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

void ToolsAVP_Mode_Waypointing::TransferToolsModeData( const int iMode, Tools_ModeDataFile& xFile )
{
    // Our version first
    int iVersion = CURRENT_MODE_DATA_VERSION;
    xFile.Transfer( iVersion );

    // Then parent data.
    PARENT::TransferToolsModeData( iMode, xFile );

    // Then our data.
    if ( iVersion == CURRENT_MODE_DATA_VERSION )
	{
		xFile.Transfer( s_xAVPOverlayPosition );
		xFile.Transfer( s_bShowEnvHooks );
		xFile.Transfer( s_iShowDataType );
		xFile.Transfer( s_bOnlyShowForwardFacingData );
		xFile.Transfer( s_bShowGS2Objects );
	}
	else if ( iVersion == 1 )
    {
		// This version got corrupted somehow, so we'll chuck it away.
    }
	else if ( iVersion == 0 )
	{
		xFile.Transfer( s_xAVPOverlayPosition );
		xFile.Transfer( s_bShowEnvHooks );
		bool bDummy;
		xFile.Transfer( bDummy );
		xFile.Transfer( bDummy );
		xFile.Transfer( s_bOnlyShowForwardFacingData );
	}
}

Tools_Waypoint_Analysis* ToolsAVP_Mode_Waypointing::CreateWaypointAnalysis()
{
    return new ToolsAVP_Waypoint_Analysis;
}

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

void ToolsAVP_Mode_Waypointing::CreateAVPOverlay()
{
    Asura_Overlay* pxNewOverlay = new Asura_Overlay( "AVP Waypointing Mode", false, &s_xAVPOverlayPosition );
    Asura_Vector_2 xOffset      = Asura_Vector_2( 1.0f, 3.0f );

    pxNewOverlay->m_uID = reinterpret_cast< u_int >( this );

	Asura_Dialog* pxDialogue = new Asura_Function_Dialog( "Generate Extra Waypointing Data", xOffset, GenerateExtraWaypointingData );
	pxNewOverlay->AddDialog( pxDialogue );
	xOffset.y += Asura_Dialog::LargeSpacing();

	pxDialogue = new Asura_Tickbox_Dialog( "Include 'Extra Waypointing Data' In Main Waypointing Pass", xOffset, &s_bIncludeLoFData );
	pxNewOverlay->AddDialog( pxDialogue );
	xOffset.y += Asura_Dialog::LargeSpacing();

	pxDialogue = new Asura_Function_Dialog( "Verify Waypoints For LoF Data", xOffset, VerifyWaypointsForLOF );
    pxNewOverlay->AddDialog( pxDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

    Asura_Tabs_Dialog* pxTabsDialogue = new Asura_Tabs_Dialog( xOffset, s_aszAVPTabNames, sizeof( s_aszAVPTabNames ) / sizeof( s_aszAVPTabNames[ 0 ] ) );
    pxNewOverlay->AddDialog( pxTabsDialogue );

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

    //pxDialogue = new Asura_Numerical_Dialog( "Attractor Link Angle Tolerance", xOffset, &s_fAttractorAngleTolerance, 5.0f, 50.0f, 1.0f, "%1.1f" );
    //pxDialogue->SetHelpText( "How far waypoint normals can be from the ideal and still be considered for linking across an attractor." );
    //pxNewOverlay->AddDialog( pxDialogue );
    //xOffset.y += Asura_Dialog::DefaultSpacing();

    pxDialogue = new Asura_Numerical_Dialog( "Waypoint Volume Poly Angle Tolerance Min", xOffset, &s_fWaypointVolumeMinAngle, 5.0f, 180.0f, 1.0f, "%1.1f" );
    pxDialogue->SetHelpText( "Polys greater than this angle away from the waypoint poly will act to constrict the waypoint volume." );
    pxTabsDialogue->AddDialog( pxDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxDialogue = new Asura_Numerical_Dialog( "Waypoint Volume Poly Angle Tolerance Max", xOffset, &s_fWaypointVolumeMaxAngle, 5.0f, 180.0f, 1.0f, "%1.1f" );
    pxDialogue->SetHelpText( "Polys less than this angle away from the waypoint poly will act to constrict the waypoint volume." );
    pxTabsDialogue->AddDialog( pxDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxDialogue = new Asura_Numerical_Dialog( "Waypoint Link Angle Tolerance", xOffset, &s_fLinkMaxAngle, 5.0f, 180.0f, 1.0f, "%1.1f" );
    pxDialogue->SetHelpText( "Links to a poly greater than this angle away from the link start poly will be culled." );
    pxTabsDialogue->AddDialog( pxDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxDialogue = new Asura_Numerical_Dialog( "Transition Link Angle Min", xOffset, &s_fTransitionLinkMinAngle, 5.0f, 180.0f, 1.0f, "%1.1f" );
    pxDialogue->SetHelpText( "Links whose start and end normals are greater than this angle will be considered transition links." );
    pxTabsDialogue->AddDialog( pxDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxDialogue = new Asura_Numerical_Dialog( "Transition Link Angle To Perfect Max", xOffset, &s_fTransitionLinkMaxAngleToPerfect, 5.0f, 180.0f, 1.0f, "%1.1f" );
    pxDialogue->SetHelpText( "The maximum angle a transition link can be from the 'perfect' link between two surfaces." );
    pxTabsDialogue->AddDialog( pxDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxDialogue = new Asura_Numerical_Dialog( "Surface Plane Angle Tolerance", xOffset, &s_fSurfaceAngleTolerance, 5.0f, 180.0f, 1.0f, "%1.1f" );
    pxDialogue->SetHelpText( "Waypoints whose normals differ by less than this angle will be considered to be on the same surface." );
    pxTabsDialogue->AddDialog( pxDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxDialogue = new Asura_Numerical_Dialog( "Min Transition Link Corner Distance", xOffset, &s_fMinTransitionLinkDistance, 0.5f, 15.0f, 0.1f, "%1.2f" );
    pxDialogue->SetHelpText( "The minimum distance a transition link can start/end from the transition corner." );
    pxTabsDialogue->AddDialog( pxDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

#ifndef SCORE_DISTANCE_LINEARLY
    pxDialogue = new Asura_Numerical_Dialog( "Ideal Transition Link Distance", xOffset, &s_fIdealTransitionLinkDistance, 0.5f, 15.0f, 0.1f, "%1.2f" );
    pxDialogue->SetHelpText( "The ideal distance a transition link should start/end from the transition corner." );
    pxTabsDialogue->AddDialog( pxDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();
#endif

    pxDialogue = new Asura_Numerical_Dialog( "Max Transition Link Corner Distance", xOffset, &s_fMaxTransitionLinkDistance, 0.5f, 15.0f, 0.1f, "%1.2f" );
    pxDialogue->SetHelpText( "The maximum distance a transition link can start/end from the transition corner." );
    pxTabsDialogue->AddDialog( pxDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

    //pxDialogue = new Asura_Numerical_Dialog( "Distance <--> Angle Score Weighting", xOffset, &s_fDistanceAngleScoreWeighting, 0.0f, 1.0f, 0.05f, "%1.2f" );
    //pxTabsDialogue->AddDialog( pxDialogue );
    //xOffset.y += Asura_Dialog::DefaultSpacing();

    //pxDialogue = new Asura_Numerical_Dialog( "Transition Link Min Acceptable Score", xOffset, &s_fTransitionLinkMinAcceptableScore, 0.0f, 1.0f, 0.05f, "%1.2f" );
    //pxTabsDialogue->AddDialog( pxDialogue );
    //xOffset.y += Asura_Dialog::DefaultSpacing();

    pxDialogue = new Asura_Numerical_Dialog( "Min Floor->Ceiling Jump Link Distance", xOffset, &s_fMinFloorCeilingJumpDistance, 0.5f, 15.0f, 0.1f, "%1.2f" );
    pxTabsDialogue->AddDialog( pxDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxDialogue = new Asura_Numerical_Dialog( "Max Floor->Ceiling Jump Link Distance", xOffset, &s_fMaxFloorCeilingJumpDistance, 0.5f, 15.0f, 0.1f, "%1.2f" );
    pxTabsDialogue->AddDialog( pxDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxDialogue = new Asura_Numerical_Dialog( "Min Ceiling->Floor Fall Link Distance", xOffset, &s_fMinCeilingFloorFallDistance, 0.5f, 30.0f, 0.1f, "%1.2f" );
    pxTabsDialogue->AddDialog( pxDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxDialogue = new Asura_Numerical_Dialog( "Max Ceiling->Floor Fall Link Distance", xOffset, &s_fMaxCeilingFloorFallDistance, 0.5f, 30.0f, 0.1f, "%1.2f" );
    pxTabsDialogue->AddDialog( pxDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxDialogue = new Asura_Numerical_Dialog( "Min Wall->Wall Jump Link Distance", xOffset, &s_fMinWallWallJumpDistance, 0.5f, 15.0f, 0.1f, "%1.2f" );
    pxTabsDialogue->AddDialog( pxDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxDialogue = new Asura_Numerical_Dialog( "Max Wall->Wall Jump Link Distance", xOffset, &s_fMaxWallWallJumpDistance, 0.5f, 15.0f, 0.1f, "%1.2f" );
    pxTabsDialogue->AddDialog( pxDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxDialogue = new Asura_Numerical_Dialog( "Jump Target Surface Angle Tolerance", xOffset, &s_fJumpTargetSurfaceAngleTolerance, 0.5f, 50.0f, 0.1f, "%1.2f" );
    pxDialogue->SetHelpText( "Jumps will only be considered onto surfaces that are within the target surface angle tolerance of the expected angle (roughly 180 degrees)." );
    pxTabsDialogue->AddDialog( pxDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

	pxDialogue = new Asura_Numerical_Dialog( "Minimum Waypoint Island Size", xOffset, &s_iMinIslandSize, 0, 100, 1, "%d" );
	pxTabsDialogue->AddDialog( pxDialogue );
	xOffset.y += Asura_Dialog::DefaultSpacing();


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

    pxDialogue = new Asura_Tickbox_Dialog( "Show Env Hooks", xOffset, &s_bShowEnvHooks, &s_bShowEnvHooksChanged );
    pxTabsDialogue->AddDialog( pxDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

	pxDialogue = new Asura_Tickbox_Dialog( "Show GS2 Objects", xOffset, &s_bShowGS2Objects, &s_bShowGS2ObjectsChanged );
	pxTabsDialogue->AddDialog( pxDialogue );
	xOffset.y += Asura_Dialog::DefaultSpacing();

	pxDialogue = new Asura_DropDown_List_Dialog( xOffset, s_aszAVPSDTNames, &s_iShowDataType, SDT_END_ENUM, SDT_END_ENUM, NULL );
	pxTabsDialogue->AddDialog( pxDialogue );
	xOffset.y += Asura_Dialog::DefaultSpacing();

    pxDialogue = new Asura_Tickbox_Dialog( "Only Show Waypoints/Links Facing The Camera", xOffset, &s_bOnlyShowForwardFacingData );
    pxTabsDialogue->AddDialog( pxDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

	pxDialogue = new Asura_Tickbox_Dialog( "Show Game Play Volumes", xOffset, &s_bShowGamePlayVolumes );
    pxTabsDialogue->AddDialog( pxDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

    //pxDialogue = new Asura_Tickbox_Dialog( "Render Colours As Floor/Wall/Ceiling", xOffset, &s_bRenderColoursAsFWC );
    //pxTabsDialogue->AddDialog( pxDialogue );
    //xOffset.y += Asura_Dialog::DefaultSpacing();


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

    pxDialogue = new Asura_Tickbox_Dialog( "Show Failed Link Routes", xOffset, &s_bShowFailedLinkRoutes );
    pxTabsDialogue->AddDialog( pxDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxDialogue = new Asura_Dynamic_Dialog( &s_xNumFailedLinkRoutesString, xOffset );
    pxTabsDialogue->AddDialog( pxDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxDialogue = new Asura_Numerical_Dialog( "Failed Link Route Index   <-Y, U->", xOffset, &s_uFailedLinkRouteIndex, 0, MAX_FAILED_LINK_ROUTES, 1, "%u", &s_bFailedLinkRouteIndexChanged );
    pxTabsDialogue->AddDialog( pxDialogue );
    xOffset.y += Asura_Dialog::DefaultSpacing();

    pxDialogue = new Asura_Numerical_Dialog( "Failed Link Route Position Index   ||<-G, <-H, J->", xOffset, &s_uFailedLinkPositionIndex, 0, 10000, 1, "%u" );
    pxTabsDialogue->AddDialog( pxDialogue );
    xOffset.y += Asura_Dialog::LargeSpacing();

	pxDialogue = new Asura_Tickbox_Dialog( "Outline Module/Poly Index", xOffset, &s_bOutlineModulePolyIndex );
	pxTabsDialogue->AddDialog( pxDialogue );
	xOffset.y += Asura_Dialog::DefaultSpacing();

	pxDialogue = new Asura_Numerical_Dialog( "Module", xOffset, &s_uOutlineModuleIndex, 0, 9999, 1, "%u" );
	pxTabsDialogue->AddDialog( pxDialogue );
	xOffset.y += Asura_Dialog::DefaultSpacing();

	pxDialogue = new Asura_Numerical_Dialog( "Poly", xOffset, &s_uOutlinePolyIndex, 0, 99999, 1, "%u" );
	pxTabsDialogue->AddDialog( pxDialogue );
	xOffset.y += Asura_Dialog::LargeSpacing();

    Asura_Overlay_System::SubmitNewOverlay( pxNewOverlay, false );
}

void ToolsAVP_Mode_Waypointing::DestroyAVPOverlay()
{
    Asura_Overlay_System::CloseOverlays( reinterpret_cast< u_int >( this ) );
}

void ToolsAVP_Mode_Waypointing::ToggleAVPWaypointingOverlay()
{
	if ( s_pxThis )
	{
		if ( AVPWaypointingOverlayIsOn() )
		{
			s_pxThis->DestroyAVPOverlay();
		}
		else
		{
			s_pxThis->CreateAVPOverlay();
		}
	}
}

bool ToolsAVP_Mode_Waypointing::AVPWaypointingOverlayIsOn()
{
	if ( s_pxThis )
	{
		return ( Asura_Overlay_System::GetOverlay( reinterpret_cast< u_int >( s_pxThis ) ) != NULL );
	}
	else
	{
		return false;
	}
}

void ToolsAVP_Mode_Waypointing::GenerateExtraWaypointingData()
{
	// Make sure that we have created the container tools entities,
	// which are needed for calculating luminance values for the containers.
	CreateToolsEntities();

	AVP_Waypoint_Manager::DestroyExtraWaypointingData();
	bool bTouchedEntityData = false;
	if ( AVP_Waypoint_Manager::CalculateExtraWaypointingData( bTouchedEntityData ) )
	{
		Tools_SaveAsura::SetWaypointingModified();
		if ( bTouchedEntityData )
		{
			Tools_SaveAsura::SetEntityModified();
		}

		Tools_Timed_Message_System::AddMessage( Tools_Timed_Message_System::COLOUR_SUCCESS, "Extra Waypointing Data Generated" );
	}
	else
	{
		AVP_Waypoint_Manager::DestroyExtraWaypointingData();
		Tools_Timed_Message_System::AddMessage( Tools_Timed_Message_System::COLOUR_ERROR, "Extra Waypointing Data Generation Failed" );
	}
}

void ToolsAVP_Mode_Waypointing::VerifyWaypointsForLOF()
{
	AVP_Waypoint_Manager::VerifyWaypointsForLOF();
}

bool ToolsAVP_Mode_Waypointing::GenerateWaypointsSingleStep( int iPhase, bool& bFinished )
{
	if ( iPhase == 121 )
	{
		return AddGS2Waypoints();
	}
	else if ( iPhase == 122 )
	{
		return AddEnvHookWaypoints();
	}
	else if ( iPhase == 123 )
	{
		return AddPredatorJumpWaypoints();
	}
	else if ( iPhase == 124 )
	{
		return AddAlienJumpWaypoints();
	}
	else if ( iPhase == 151 )
    {
        return FlagWaypointsForActorType();
    }
    else if ( iPhase == 179 )
    {
        return FlagTransitionLinks();
    }
    else if ( iPhase == 201 )
    {
        return FinalFlagTransitionLinks();
    }
    else if ( iPhase == 202 )
    {
        return UnflagTransitionLinks();
    }
    else if ( iPhase == 222 )
    {
        return AddJumpLinks();
    }
	else if ( iPhase == 223 )
	{
		return AddGS2Links();
	}
	else if ( iPhase == 224 )
	{
		return AddEnvHookLinks();
	}
	else if ( iPhase == 225 )
	{
		return AddPredatorJumpLinks();
	}
	else if ( iPhase == 226 )
	{
		return AddAlienJumpLinks();
	}
	else if ( iPhase == 227 )
	{
		return AddLiftUDWLinks();
	}
	else if ( iPhase == 281 )
	{
		return RemoveHumanoidFlagFromInvalidOneWayLinks();
	}
	else if ( iPhase == 331 )
	{
		if ( s_bIsBatchWaypointing )
		{
			bool bTouchedEntityData = false;
			const bool bOk = AVP_Waypoint_Manager::CalculateExtraWaypointingData( bTouchedEntityData );
			if ( bTouchedEntityData )
			{
				Tools_SaveAsura::SetEntityModified();
			}
			if ( bOk )
			{
				return true;
			}
			else
			{
				AVP_Waypoint_Manager::DestroyExtraWaypointingData();
				return false;
			}
		}
		else
		{
			return true;
		}
	}
	else if ( iPhase == 332 )
	{
		if ( s_bIncludeLoFData )
		{
			GenerateExtraWaypointingData();
		}

		return true;
	}
    else
    {
        return PARENT::GenerateWaypointsSingleStep( iPhase, bFinished );
    }
}

bool ToolsAVP_Mode_Waypointing::InitialiseAndAllocateStorage()
{
    bool bOk = PARENT::InitialiseAndAllocateStorage();

    // Destroy any failed link routes.
    for ( u_int u = 0; u < s_uNumFailedLinkRoutes; ++u )
    {
        s_axFailedLinkRoutes[ u ].ShutdownArrays();
    }
    s_uNumFailedLinkRoutes = 0;
    s_xNumFailedLinkRoutesString = "There are no failed link routes.";

    // Set our angles again in case they've changed.
    SetMaxDotProductBetweenWaypointPolygonNormals( Asura_Maths::Cosine( Asura_Maths::DegToRad( s_fWaypointVolumeMinAngle ) ) );
    SetMaxDotProductForLinkBetweenWaypointNormals( Asura_Maths::Cosine( Asura_Maths::DegToRad( s_fLinkMaxAngle ) ) );

    bOk &= InitialiseStandardBB();

	AVP_Waypoint_Manager::DestroyExtraWaypointingData();

	s_xAlienInVentGS2.Clear();
	s_xAlienOutVentGS2.Clear();

    return bOk;
}

bool ToolsAVP_Mode_Waypointing::CreateWaypointingPhysicsObjects()
{
    const bool bParentOk = PARENT::CreateWaypointingPhysicsObjects();

    // Standard physics object is the Alien.
    if ( s_pxStandardPhysicsObject )
    {
        u_int uFlagsToIgnore = s_pxStandardPhysicsObject->Tools_GetFlagsOfPolysToIgnore();
        uFlagsToIgnore &= ~AsuraPolygonFlag_IgnoreWhenWaypointing;
        uFlagsToIgnore |= AVP_PolygonFlag_IgnoreForAlienCollision;
        s_pxStandardPhysicsObject->Tools_SetFlagsOfPolysToIgnore( uFlagsToIgnore );
        s_pxStandardPhysicsObject->SetCollisionObject( &s_xAlienTestCollisionBox );
		s_pxStandardPhysicsObject->SetCollisionProxy( AVP_Actor_PhysicsObjectHandler::GetActorCollisionProxy(AVP_ENTITYCLASS_ALIEN) );
    }

    s_pxHumanoidPhysicsObject = Asura_Physics_Object::Create( ASURA_PHYSICS_OBJECT_ACTOR );
    if ( s_pxHumanoidPhysicsObject )
    {
        s_pxHumanoidPhysicsObject->SetLinVelocity( Asura_Maths::ZeroVector3 );
        s_pxHumanoidPhysicsObject->SetAxisConstraint( Asura_Physics_Object::AXIS_CONSTRAINT_NONE );
        s_pxHumanoidPhysicsObject->SetFeelsGravity( true );
        s_pxHumanoidPhysicsObject->SetOnlyCollideWithEnvironmentAndStaticObjects( true );
        s_pxHumanoidPhysicsObject->SetCollisionObject( &s_xHumanoidTestCollisionBox );
        s_pxHumanoidPhysicsObject->SetCollisionProxy( AVP_Actor_PhysicsObjectHandler::GetActorCollisionProxy(AVP_ENTITYCLASS_MARINE) );
        s_pxHumanoidPhysicsObject->SetIgnore( true );
        s_pxHumanoidPhysicsObject->SetReportObjectCollisions( true );
        s_pxHumanoidPhysicsObject->SetReportEnvironmentCollisions( true );

        u_int uFlagsToIgnore = s_pxHumanoidPhysicsObject->Tools_GetFlagsOfPolysToIgnore();
        uFlagsToIgnore |= AVP_PolygonFlag_IgnoreForHumanCollision;
        s_pxHumanoidPhysicsObject->Tools_SetFlagsOfPolysToIgnore( uFlagsToIgnore );
    }

    return ( bParentOk && s_pxHumanoidPhysicsObject );
}

void ToolsAVP_Mode_Waypointing::DestroyWaypointingPhysicsObjects()
{
    if ( s_pxHumanoidPhysicsObject )
    {
        Asura_Physics_Object::Destroy( s_pxHumanoidPhysicsObject );
        s_pxHumanoidPhysicsObject = NULL;
    }

    PARENT::DestroyWaypointingPhysicsObjects();
}

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

	const ToolsAVP_Waypoint_Object* const pxAVPUDW = static_cast< const ToolsAVP_Waypoint_Object* const >( pxUserWaypoint );

	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;

			if ( pxAVPUDW->GetAVPUDW().IsLiftWaypoint() )
			{
				// Lift UDWs specify their own radius.
				s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_fRadius = pxAVPUDW->GetAVPUDW().m_fRadius;
			}

			++iNumWPsInBin;

			return true;
		}
	}

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

bool ToolsAVP_Mode_Waypointing::CullWaypointsBySize()
{
    // Cull any waypoints that are less than the minimum radius.
    // Also, cull any wall and ceiling waypoints that are less than the Alien waypoint radius.
    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 ]; )
            {
                Tools_Nav_Vertex& xWaypoint = s_apxWaypointStorage[ iBin ][ iWaypointIndex ];
                bool              bCull     = false;

                if ( xWaypoint.IsCeiling() || xWaypoint.IsWall() )
                {
                    if ( xWaypoint.m_fRadius < s_fMinAlienWaypointRadius )
                    {
                        bCull = true;
                    }
                }
                else if ( RadiusIsLessThanMinRadius( xWaypoint.m_iFlags, xWaypoint.m_fRadius ) )
                {
                    bCull = true;
                }

                if ( bCull )
                {
                    xWaypoint = s_apxWaypointStorage[ iBin ][ --s_aiNumWaypoints[ iBin ] ];
                }
                else
                {
                    ++iWaypointIndex;
                }
            }

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

    return SomeBinWaypointsExist();
}

bool ToolsAVP_Mode_Waypointing::CullRedundantWaypoints()
{
    ASURA_CPU_PROFILE_SCOPED_TIMER("ToolsAVP_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, AVPAABBTreePerBinSizeTestCallback );

        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, AVPAABBTreePerBinSizeTestCallback );

        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 ToolsAVP_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;
        }

		// Don't cull GS2 waypoints.
		if ( s_pxWaypointStorage[ i ].m_iFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_GS2 )
		{
			++i;
			continue;
		}

		// Don't cull env hook waypoints.
		if ( s_pxWaypointStorage[ i ].m_iFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ENV_HOOK )
		{
			++i;
			continue;
		}

		// Don't cull env predator jump waypoints.
		if ( s_pxWaypointStorage[ i ].m_iFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_PREDATOR_JUMP )
		{
			++i;
			continue;
		}

		if ( s_pxWaypointStorage[ i ].m_iFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN_JUMP )
		{
			++i;
			continue;
		}


		// Test to see if we can fit a box on the waypoint.
		bool bIsAlienWaypoint = ( ( s_pxWaypointStorage[ i ].m_iFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN ) != 0 );
		if ( bIsAlienWaypoint )
		{
			const Asura_Collision_Object& xCollisionObject = *s_pxStandardPhysicsObject->GetCollisionObject();

			Asura_Bounding_Box xMaximumBB = xCollisionObject.GetBoundingBox();
			const float fMaxX = Asura_Maths::Max( Asura_Maths::Abs( xMaximumBB.MinX ), Asura_Maths::Abs( xMaximumBB.MaxX ) );
			const float fMaxZ = Asura_Maths::Max( Asura_Maths::Abs( xMaximumBB.MinZ ), Asura_Maths::Abs( xMaximumBB.MaxZ ) );
			const float fMaximumExtent = Asura_Maths::Max( fMaxX, fMaxZ );
			xMaximumBB.MinX = -fMaximumExtent;
			xMaximumBB.MaxX = fMaximumExtent;
			xMaximumBB.MinZ = -fMaximumExtent;
			xMaximumBB.MaxZ = fMaximumExtent;
			const Asura_Collision_Box xMaximumCollisionObject( xMaximumBB );

			Asura_Matrix_3x3 xOrientation;
			Asura_Maths::OrientationFromNormal(xOrientation, s_pxWaypointStorage[i].m_xNormal);

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

			Asura_Vector_3 xFinalPosition;
			if ( !PlaceObjectAt( s_pxWaypointStorage[ i ].m_xPosition, xUpMovement,
				s_fPlacedObjectAllowedMovement * s_fPlacedObjectAllowedMovement,
				xMaximumCollisionObject,
				( bIsAlienWaypoint ? s_pxStandardPhysicsObject->GetFlagsOfPolysToIgnore() : s_pxHumanoidPhysicsObject->GetFlagsOfPolysToIgnore() ),
				xOrientation, xFinalPosition, ( bIsAlienWaypoint ? s_pxStandardPhysicsObject->GetCollisionProxy() : s_pxHumanoidPhysicsObject->GetCollisionProxy() ) ) )
			{
				// Remove the Alien flag.
				s_pxWaypointStorage[ i ].m_iFlags &= ~AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN;
			}
		}

		bool bIsHumanoidWaypoint = ( ( s_pxWaypointStorage[ i ].m_iFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_HUMANOID ) != 0 );
		if ( bIsHumanoidWaypoint )
		{
			const Asura_Collision_Object& xCollisionObject = *s_pxHumanoidPhysicsObject->GetCollisionObject();

			Asura_Bounding_Box xMaximumBB = xCollisionObject.GetBoundingBox();
			const float fMaxX = Asura_Maths::Max( Asura_Maths::Abs( xMaximumBB.MinX ), Asura_Maths::Abs( xMaximumBB.MaxX ) );
			const float fMaxZ = Asura_Maths::Max( Asura_Maths::Abs( xMaximumBB.MinZ ), Asura_Maths::Abs( xMaximumBB.MaxZ ) );
			const float fMaximumExtent = Asura_Maths::Max( fMaxX, fMaxZ );
			xMaximumBB.MinX = -fMaximumExtent;
			xMaximumBB.MaxX = fMaximumExtent;
			xMaximumBB.MinZ = -fMaximumExtent;
			xMaximumBB.MaxZ = fMaximumExtent;
			const Asura_Collision_Box xMaximumCollisionObject( xMaximumBB );

			Asura_Matrix_3x3 xOrientation;
			Asura_Maths::OrientationFromNormal(xOrientation, s_pxWaypointStorage[i].m_xNormal);

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

			Asura_Vector_3 xFinalPosition;
			if ( !PlaceObjectAt( s_pxWaypointStorage[ i ].m_xPosition, xUpMovement,
				s_fPlacedObjectAllowedMovement * s_fPlacedObjectAllowedMovement,
				xMaximumCollisionObject,
				( bIsAlienWaypoint ? s_pxStandardPhysicsObject->GetFlagsOfPolysToIgnore() : s_pxHumanoidPhysicsObject->GetFlagsOfPolysToIgnore() ),
				xOrientation, xFinalPosition, ( bIsAlienWaypoint ? s_pxStandardPhysicsObject->GetCollisionProxy() : s_pxHumanoidPhysicsObject->GetCollisionProxy() ) ) )
			{
				// Remove the humanoid flag.
				s_pxWaypointStorage[ i ].m_iFlags &= ~AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_HUMANOID;
			}
		}

        bIsAlienWaypoint    = ( ( s_pxWaypointStorage[ i ].m_iFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN ) != 0 );
		bIsHumanoidWaypoint = ( ( s_pxWaypointStorage[ i ].m_iFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_HUMANOID ) != 0 );
        if ( !bIsAlienWaypoint && !bIsHumanoidWaypoint )
        {
#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;
        }
    }

	return ( s_iNumWaypoints > 0 );
}

// 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 ToolsAVP_Mode_Waypointing::GenerateWaypointLinks()
{
	ASURA_CPU_PROFILE_SCOPED_TIMER("ToolsAVP_Mode_Waypointing::GenerateWaypointLinks");

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

		bool bSpecialFromWP = 
			( s_pxWaypointStorage[ i ].m_iFlags & 
			(AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ENV_HOOK | 
			 AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_GS2 | 
			 AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_PREDATOR_JUMP | 
			 AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN_JUMP) ) != 0;

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

		

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

		const bool bSourceIsLiftUDW = ( ( s_pxWaypointStorage[ i ].m_iFlags & TOOLS_NAVVERTEX_FLAG_LIFT_UDW ) != 0 );

		// Add links to other waypoints.
        for ( int j = 0; j < s_iNumWaypoints; ++j )
        {
            if ( i == j )
            {
                continue;
            }

			bool bSpecialToWP = (s_pxWaypointStorage[ j ].m_iFlags & 
				(AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ENV_HOOK | 
				AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_GS2 | 
				AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_PREDATOR_JUMP | 
				AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN_JUMP) ) != 0;

			bool bSpecialWP = bSpecialFromWP || bSpecialToWP;


			// Don't generate links between lift user-defined waypoints.
			const bool bTargetIsLiftUDW = ( ( s_pxWaypointStorage[ j ].m_iFlags & TOOLS_NAVVERTEX_FLAG_LIFT_UDW ) != 0 );
			if ( bSourceIsLiftUDW && bTargetIsLiftUDW )
			{
				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 ( !bSpecialWP && 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, but only if it's a ceiling or wall link.
            const bool bIsCeilingWallLink = ( s_pxWaypointStorage[ i ].IsCeiling() || s_pxWaypointStorage[ i ].IsWall() ||
                                              s_pxWaypointStorage[ j ].IsCeiling() || s_pxWaypointStorage[ j ].IsWall() );

            const float fNormalDot = s_pxWaypointStorage[ i ].m_xNormal * s_pxWaypointStorage[ j ].m_xNormal;

            if ( bIsCeilingWallLink && fNormalDot < s_fMaxDotProductForLinkBetweenWaypointNormals )
            {
                continue;
            }

			if (s_pxWaypointStorage[i].m_iFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_GS2)
			{
				bool found = false;
				for (Asura_LinkList_ConstIt< Gs2WayPointID > it(s_xAlienOutVentGS2); !found && !it.Done(); it.Next())
				{
					const Gs2WayPointID* pxWPID = it.GetCurrent();
					if (s_pxWaypointStorage[i].m_xPosition.ApproximatelyEquals(pxWPID->m_xPosition) &&
						s_pxWaypointStorage[i].m_xNormal.ApproximatelyEquals(pxWPID->m_xNormal))
					{
						found = true;
					}
				}
				if (!found)
				{
					continue;
				}
			}

			if (s_pxWaypointStorage[j].m_iFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_GS2)
			{
				bool found = false;
				for (Asura_LinkList_ConstIt< Gs2WayPointID > it(s_xAlienInVentGS2); !found && !it.Done(); it.Next())
				{
					const Gs2WayPointID* pxWPID = it.GetCurrent();
					if (s_pxWaypointStorage[j].m_xPosition.ApproximatelyEquals(pxWPID->m_xPosition) &&
						s_pxWaypointStorage[j].m_xNormal.ApproximatelyEquals(pxWPID->m_xNormal))
					{
						found = true;
					}
				}
				if (!found)
				{
					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 ToolsAVP_Mode_Waypointing::CullRedundantLinks()
{
    // First cull non-transition links that overlap other waypoints.
    if ( !CullOverlappingNonTransitionLinks() )
    {
        return false;
    }

    // Now make sure that all transition links from a waypoint meet certain criteria.
    // 1) Make sure that transition links start and finish an appropriate distance from the relevant planes.
    if ( !CullTransitionLinksUsingDistanceFromPlanes() )
    {
        return false;
    }

    // 2) Only keep the most direct link from the waypoint onto the other surface if there is more than one.
    if ( !CullLessDirectTransitionLinks() )
    {
        return false;
    }

    return true;
}

bool ToolsAVP_Mode_Waypointing::MakeAllLinksTwoWay()
{
    // This has been overridden so it only makes non-transition links two way, as we've already done some selective culling on
    // transition links.
    for ( int i = 0; i < s_iNumWaypoints; ++i )
    {   
        for ( int iLinkIndex = 0; iLinkIndex < s_pxWaypointStorage[ i ].GetNumLinks(); ++iLinkIndex )
        {
            if ( ( s_pxWaypointStorage[ i ].m_xLinks[ iLinkIndex ].m_uFlags ) & TOOLSAVP_CONNECTION_FLAG_TRANSITION )
            {
                continue;
            }

            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 ToolsAVP_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;

    // We do two lots of tests here. Firstly we do the standard (Alien) physics object against all potential Alien links.
    // Then we do the humanoid physics object against all potential humanoid links.
	bool bIsToolsLinkAnalysis = false;
    bool bIsTransitionLink    = false;
    bool bIsConcave           = false;
    bool bStartWPIsHumanoid   = false;
    bool bFinishWPIsHumanoid  = false;
    bool bStartWPIsAlien      = false;
    bool bFinishWPIsAlien     = false;
    bool bIsGS2	              = false;
    bool bIsEnvHook	          = false;
    float fStartWPRadius      = 0.0f;
    float fFinishWPRadius     = 0.0f;
    if ( s_pxWaypointStorage )
    {
        const Tools_Nav_Vertex& xStartWP  = s_pxWaypointStorage[ iStartWPIndex ];
        const Tools_Nav_Vertex& xFinishWP = s_pxWaypointStorage[ iFinishWPIndex ];

		bIsEnvHook			     = (  (uFlags & AVP_Navigation::AVP_AI_CONNECTION_FLAG_ENV_HOOK ) != 0);
		bIsGS2			     = (  (uFlags & AVP_Navigation::AVP_AI_CONNECTION_FLAG_GS2 ) != 0);
        bIsTransitionLink    = ( ( uFlags & TOOLSAVP_CONNECTION_FLAG_TRANSITION ) != 0 );
        bIsConcave           = ( ( ( xFinishWP.m_xPosition - xStartWP.m_xPosition ) * xFinishWP.m_xNormal ) < 0.0f );
        bStartWPIsHumanoid   = ( ( xStartWP.m_iFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_HUMANOID ) != 0 );
		bStartWPIsHumanoid  |= ( ( xStartWP.m_iFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_PREDATOR ) != 0 );
		bFinishWPIsHumanoid  = ( ( xFinishWP.m_iFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_HUMANOID ) != 0 );
        bFinishWPIsHumanoid |= ( ( xFinishWP.m_iFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_PREDATOR ) != 0 );
        bStartWPIsAlien      = ( ( xStartWP.m_iFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN ) != 0 );
        bFinishWPIsAlien     = ( ( xFinishWP.m_iFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN ) != 0 );
		fStartWPRadius		 = s_pxWaypointStorage[ iStartWPIndex ].m_fRadius;
		fFinishWPRadius		 = s_pxWaypointStorage[ iFinishWPIndex ].m_fRadius;
    }
    else
    {
		bIsToolsLinkAnalysis = true;

        const Asura_AI_NavVertex* const pxStartWaypoint  = Asura_AI_Navigation_System::GetWaypoint( iStartWPIndex );
        const Asura_AI_NavVertex* const pxFinishWaypoint = Asura_AI_Navigation_System::GetWaypoint( iFinishWPIndex );
        Asura_Assert( pxStartWaypoint && pxFinishWaypoint, "ToolsAVP_Mode_Waypointing::CullAndFlagLinkWithPhysics - could not find start and finish waypoints" );
        if ( pxStartWaypoint && pxFinishWaypoint )
        {
            bIsTransitionLink = false;
            u_char ucLink = 0;
            for ( ucLink = 0; ucLink < pxStartWaypoint->m_ucNumConnections; ++ucLink )
            {
                if ( pxStartWaypoint->m_pxConnections[ ucLink ].m_usIndex == iFinishWPIndex )
                {
                    bIsTransitionLink = AVP_Navigation::IsTransitionLink( pxStartWaypoint->m_pxConnections[ ucLink ].m_usFlags );
					bIsEnvHook		  = (  (pxStartWaypoint->m_pxConnections[ ucLink ].m_usFlags & AVP_Navigation::AVP_AI_CONNECTION_FLAG_ENV_HOOK ) != 0);
					bIsGS2			  = (  (pxStartWaypoint->m_pxConnections[ ucLink ].m_usFlags & AVP_Navigation::AVP_AI_CONNECTION_FLAG_GS2 ) != 0);
                    break;
                }
            }
            if ( ucLink == pxStartWaypoint->m_ucNumConnections )
            {
                const bool bStartWPIsCeiling  = IsCeilingPoly( xStartNormal );
                const bool bStartWPIsWall     = IsWallPoly( xStartNormal );
                const bool bFinishWPIsCeiling = IsCeilingPoly( xFinishNormal );
                const bool bFinishWPIsWall    = IsWallPoly( xFinishNormal );

                // Connection doesn't already exist - we'll have to check the waypoint flags.
                if ( ( bStartWPIsCeiling && !bFinishWPIsCeiling ) ||
                    ( !bStartWPIsCeiling && bFinishWPIsCeiling ) ||
                    ( bStartWPIsWall && !bFinishWPIsWall ) ||
                    ( !bStartWPIsWall && bFinishWPIsWall ) )
                {
                    bIsTransitionLink = true;
                }
            }

            bIsConcave           = ( ( ( pxFinishWaypoint->m_xPosition - pxStartWaypoint->m_xPosition ) * xFinishNormal ) < 0.0f );
            bStartWPIsHumanoid   = ( ( pxStartWaypoint->m_usFlags  & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_HUMANOID ) != 0 );
			bStartWPIsHumanoid  |= ( ( pxStartWaypoint->m_usFlags  & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_PREDATOR ) != 0 );
            bFinishWPIsHumanoid  = ( ( pxFinishWaypoint->m_usFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_HUMANOID ) != 0 );
			bFinishWPIsHumanoid |= ( ( pxFinishWaypoint->m_usFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_PREDATOR ) != 0 );
            bStartWPIsAlien      = ( ( pxStartWaypoint->m_usFlags  & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN ) != 0 );
            bFinishWPIsAlien     = ( ( pxFinishWaypoint->m_usFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN ) != 0 );
			fStartWPRadius		 = pxStartWaypoint->GetRadius();
			fFinishWPRadius		 = pxFinishWaypoint->GetRadius();
        }
    }

	// Do the standard (Alien) stuff first.
	const bool bAlienTestNeeded = ( bStartWPIsAlien && bFinishWPIsAlien && !bIsGS2 && !bIsEnvHook &&
		( !bIsToolsLinkAnalysis || ToolsAVP_Waypoint_Analysis::IsInterestedInAlienRouteFailure() ) );
	bool bAlienOk				= false;
	if ( bAlienTestNeeded )
	{
		if ( s_bGravityFollowsSurfaceNormals )
		{
			s_pxStandardPhysicsObject->SetOwnGravityDirection( Asura_AVector3( -xStartNormal ) );
		}

		// Generate the orientation matrix based on the normal and direction...
		CalculateOrientationFromNormalAndDirection( xStartNormal, (xFinish - xStart), xOrientation );
		xUpMovement.x = xOrientation.e12 * -s_fPlacedObjectMovementScale;
		xUpMovement.y = xOrientation.e22 * -s_fPlacedObjectMovementScale;
		xUpMovement.z = xOrientation.e32 * -s_fPlacedObjectMovementScale;

		s_uNumPhysicsPositions = 0;
		bAlienOk = PlaceObjectAt( xStart, xUpMovement, s_fPlacedObjectAllowedMovement * s_fPlacedObjectAllowedMovement,
			*s_pxStandardPhysicsObject->GetCollisionObject(), s_pxStandardPhysicsObject->GetFlagsOfPolysToIgnore(),
			xOrientation, xStartPosition, s_pxStandardPhysicsObject->GetCollisionProxy() );
		if ( !bAlienOk )
		{
			s_uNumPhysicsPositions = 1;
			s_pxPhysicsVelocities[ 0 ] = Asura_Maths::ZeroVector3;
			s_pxPhysicsPositions[ 0 ] = xStartPosition;
			s_pxPhysicsOrientations[ 0 ].FromMatrix( xOrientation );
			s_xPhysicsBoundingBox = s_pxStandardPhysicsObject->GetCollisionObject()->GetBoundingBox();
		}
	}
	if ( bAlienTestNeeded && bAlienOk )
	{
		// Now move to the destination.
		s_uNumPhysicsPositions = 0;
		float fAlienFallDistance = 0.0f;
		s_pxStandardPhysicsObject->SetIgnore( false );
		s_pxStandardPhysicsObject->SetOrientation( xOrientation );
		bAlienOk = AVPMoveToDestination( xStartPosition, xFinish, xFinishNormal,
			s_xMovedObjectAllowedOffset, true, bIsTransitionLink, bIsConcave, s_pxStandardPhysicsObject,
			fAlienFallDistance );
		s_pxStandardPhysicsObject->SetIgnore( true );

		s_xPhysicsBoundingBox = s_pxStandardPhysicsObject->GetCollisionObject()->GetBoundingBox();

		if ( bAlienOk )
		{
			// If this is not a transition link, but the physics object went through a transition (its orientation changed by a large enough
			// amount) then this is not a valid link.
			const float fCosTransitionLinkAngleMin = Asura_Maths::Cosine( Asura_Maths::DegToRad( s_fTransitionLinkMinAngle ) );
			const bool bIsTransitionLink = ( ( xStartNormal * xFinishNormal ) <= fCosTransitionLinkAngleMin );
			if ( bAlienOk && !bIsTransitionLink )
			{
				const float fCosWaypointVolumeMaxAngle = Asura_Maths::Cosine( Asura_Maths::DegToRad( s_fWaypointVolumeMinAngle ) );

				Asura_Matrix_3x3 xWorkingMatrix;
				Asura_Vector_3   xWorkingVector;
				s_pxPhysicsOrientations[ 0 ].ToMatrix( xWorkingMatrix );
				const Asura_Vector_3 xInitialForward( xWorkingMatrix.e13, xWorkingMatrix.e23, xWorkingMatrix.e33 );
				for ( u_int uIndex = 0; uIndex < s_uNumPhysicsPositions; ++uIndex )
				{
					s_pxPhysicsOrientations[ uIndex ].ToMatrix( xWorkingMatrix );
					xWorkingVector.x = xWorkingMatrix.e13;
					xWorkingVector.y = xWorkingMatrix.e23;
					xWorkingVector.z = xWorkingMatrix.e33;

					if ( xInitialForward * xWorkingVector < fCosWaypointVolumeMaxAngle )
					{
						bAlienOk = false;
						s_uNumPhysicsPositions = uIndex + 1;
						break;
					}
				}
			}
		}

		if ( bAlienOk )
		{
			// Flag this as an Alien link.
			uFlags |= AVP_Navigation::AVP_AI_CONNECTION_FLAG_ALIEN;
		}
	}
	if ( bAlienTestNeeded && !bAlienOk )
	{
		AddLastRouteToFailedRoutes( xStart, xFinish );
	}

	const bool bNonAlienTestNeeded = ( bStartWPIsHumanoid && bFinishWPIsHumanoid &&
		( !bIsToolsLinkAnalysis || ToolsAVP_Waypoint_Analysis::IsInterestedInNonAlienRouteFailure() ) );
    bool  bHumanoidOk           = bNonAlienTestNeeded;
    float fHumanoidFallDistance = 0.0f;
    if ( bNonAlienTestNeeded )
    {
		CalculateOrientationFromNormalAndDirection( Asura_Vector_3( 0.0f, -1.0f, 0.0f ), (xFinish - xStart), xOrientation );
		xUpMovement.x = xOrientation.e12 * -s_fPlacedObjectMovementScale;
		xUpMovement.y = xOrientation.e22 * -s_fPlacedObjectMovementScale;
		xUpMovement.z = xOrientation.e32 * -s_fPlacedObjectMovementScale;

        s_uNumPhysicsPositions = 0;
		if ( bIsTransitionLink )
		{
			Asura_Assertf( false, ( "humanoid link that is also a transition link (%d->%d)!", iStartWPIndex, iFinishWPIndex ) );
			Asura_Assertf( false, ( "(%f, %f, %f) -> (%f, %f, %f)", xStart.x, xStart.y, xStart.z, xFinish.x, xFinish.y, xFinish.z ) );
		}

        // Do the humanoid stuff.
		// We do three tests - one down the centre of the link and one either side to ensure that all links have a little breathing room.
		if ( bHumanoidOk )
		{
			// First place the box to ensure we start correctly.
			bHumanoidOk = PlaceObjectAt( xStart, xUpMovement, s_fPlacedObjectAllowedMovement * s_fPlacedObjectAllowedMovement,
				*s_pxHumanoidPhysicsObject->GetCollisionObject(), s_pxHumanoidPhysicsObject->GetFlagsOfPolysToIgnore(),
				xOrientation, xStartPosition, s_pxHumanoidPhysicsObject->GetCollisionProxy() );
			if ( !bHumanoidOk )
			{
				s_uNumPhysicsPositions = 1;
				s_pxPhysicsVelocities[ 0 ] = Asura_Maths::ZeroVector3;
				s_pxPhysicsPositions[ 0 ] = xStartPosition;
				s_pxPhysicsOrientations[ 0 ].FromMatrix( xOrientation );
				s_xPhysicsBoundingBox = s_pxHumanoidPhysicsObject->GetCollisionObject()->GetBoundingBox();
			}
			else
			{
				// Now move to the destination.
				s_uNumPhysicsPositions = 0;
				s_pxHumanoidPhysicsObject->SetIgnore( false );
				s_pxHumanoidPhysicsObject->SetOrientation( xOrientation );
				bHumanoidOk = AVPMoveToDestination( xStartPosition, xFinish, xFinishNormal,
					s_xMovedObjectAllowedOffset, false, bIsTransitionLink, bIsConcave, s_pxHumanoidPhysicsObject,
					fHumanoidFallDistance );
				s_pxHumanoidPhysicsObject->SetIgnore( true );

				s_xPhysicsBoundingBox = s_pxHumanoidPhysicsObject->GetCollisionObject()->GetBoundingBox();
			}
		}
		// First sideways test.
		const Asura_Vector_3 xSideways( xOrientation.e11, xOrientation.e21, xOrientation.e31 );
		if ( bHumanoidOk )
		{
			// First place the box to ensure we start correctly.
			bHumanoidOk = PlaceObjectAt( xStart + xSideways * 0.25f, xUpMovement, s_fPlacedObjectAllowedMovement * s_fPlacedObjectAllowedMovement,
				*s_pxHumanoidPhysicsObject->GetCollisionObject(), s_pxHumanoidPhysicsObject->GetFlagsOfPolysToIgnore(),
				xOrientation, xStartPosition, s_pxHumanoidPhysicsObject->GetCollisionProxy() );
			if ( !bHumanoidOk )
			{
				s_uNumPhysicsPositions = 1;
				s_pxPhysicsVelocities[ 0 ] = Asura_Maths::ZeroVector3;
				s_pxPhysicsPositions[ 0 ] = xStartPosition;
				s_pxPhysicsOrientations[ 0 ].FromMatrix( xOrientation );
				s_xPhysicsBoundingBox = s_pxHumanoidPhysicsObject->GetCollisionObject()->GetBoundingBox();
			}
			else
			{
				// Now move to the destination.
				s_uNumPhysicsPositions = 0;
				s_pxHumanoidPhysicsObject->SetIgnore( false );
				s_pxHumanoidPhysicsObject->SetOrientation( xOrientation );
				bHumanoidOk = AVPMoveToDestination( xStartPosition, xFinish + xSideways * 0.25f, xFinishNormal,
					s_xMovedObjectAllowedOffset, false, bIsTransitionLink, bIsConcave, s_pxHumanoidPhysicsObject,
					fHumanoidFallDistance );
				s_pxHumanoidPhysicsObject->SetIgnore( true );

				s_xPhysicsBoundingBox = s_pxHumanoidPhysicsObject->GetCollisionObject()->GetBoundingBox();
			}
		}
		// Second sideways test.
		if ( bHumanoidOk )
		{
			// First place the box to ensure we start correctly.
			bHumanoidOk = PlaceObjectAt( xStart - xSideways * 0.25f, xUpMovement, s_fPlacedObjectAllowedMovement * s_fPlacedObjectAllowedMovement,
				*s_pxHumanoidPhysicsObject->GetCollisionObject(), s_pxHumanoidPhysicsObject->GetFlagsOfPolysToIgnore(),
				xOrientation, xStartPosition, s_pxHumanoidPhysicsObject->GetCollisionProxy() );
			if ( !bHumanoidOk )
			{
				s_uNumPhysicsPositions = 1;
				s_pxPhysicsVelocities[ 0 ] = Asura_Maths::ZeroVector3;
				s_pxPhysicsPositions[ 0 ] = xStartPosition;
				s_pxPhysicsOrientations[ 0 ].FromMatrix( xOrientation );
				s_xPhysicsBoundingBox = s_pxHumanoidPhysicsObject->GetCollisionObject()->GetBoundingBox();
			}
			else
			{
				// Now move to the destination.
				s_uNumPhysicsPositions = 0;
				s_pxHumanoidPhysicsObject->SetIgnore( false );
				s_pxHumanoidPhysicsObject->SetOrientation( xOrientation );
				bHumanoidOk = AVPMoveToDestination( xStartPosition, xFinish - xSideways * 0.25f, xFinishNormal,
					s_xMovedObjectAllowedOffset, false, bIsTransitionLink, bIsConcave, s_pxHumanoidPhysicsObject,
					fHumanoidFallDistance );
				s_pxHumanoidPhysicsObject->SetIgnore( true );

				s_xPhysicsBoundingBox = s_pxHumanoidPhysicsObject->GetCollisionObject()->GetBoundingBox();
			}
		}
    }
    if ( bNonAlienTestNeeded && bHumanoidOk )
    {
        // Also, flag the link for the correct fall distance.
        bool bFurtherCullingRequired = false;
        if ( fHumanoidFallDistance > s_fMaxLinkFallDistance )
        {
            bHumanoidOk = false;
        }
        else if ( fHumanoidFallDistance >= s_fLongFallLinkDistance )
        {
            uFlags |= ASURA_AI_CONNECTION_FLAGS_LONGFALL;
            bFurtherCullingRequired = true;
        }
        else if ( fHumanoidFallDistance >= 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 dynamics block.
            //     2) if it does not land in its target waypoint after the fall in the dynamics 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 )
                    {
                        bHumanoidOk = false;
                    }

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

        if ( bHumanoidOk )
        {
            // Flag this as a humanoid link.
            uFlags |= AVP_Navigation::AVP_AI_CONNECTION_FLAG_HUMANOID;
			uFlags |= AVP_Navigation::AVP_AI_CONNECTION_FLAG_PREDATOR;
        }
    }
    if ( bNonAlienTestNeeded && !bHumanoidOk )
    {
        AddLastRouteToFailedRoutes( xStart, xFinish );
    }

	if ( bIsToolsLinkAnalysis )
	{
		if ( ( ToolsAVP_Waypoint_Analysis::IsInterestedInAlienRouteFailure() && !bAlienOk ) ||
			( ToolsAVP_Waypoint_Analysis::IsInterestedInNonAlienRouteFailure() && !bHumanoidOk ) )
		{
			return false;
		}
		else
		{
			return true;
		}
	}
	else
    {
        return ( bAlienOk || bHumanoidOk );
    }
}

bool ToolsAVP_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;
        }

		Asura_AI_NavVertex* pxWP = Asura_AI_Navigation_System::GetWaypoint( i );

		if (pxWP->GetFlags() & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN_JUMP )
		{
			continue;
		}

        for ( int c = 0; c < pxWP->m_ucNumConnections; )
        {
			// Don't cull GS2 links.
			if ( pxWP->m_pxConnections[ c ].GetFlags() & AVP_Navigation::AVP_AI_CONNECTION_FLAG_GS2 )
			{
				++c;
				continue;
			}

			// Don't cull env hook links.
			if ( pxWP->m_pxConnections[ c ].GetFlags() & AVP_Navigation::AVP_AI_CONNECTION_FLAG_ENV_HOOK )
			{
				++c;
				continue;
			}

			Asura_AI_NavVertex* pxNextWP = Asura_AI_Navigation_System::GetWaypoint( pxWP->m_pxConnections[ c ].GetIndex() );

			if (pxNextWP->GetFlags() & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN_JUMP )
			{
				++c;
				continue;
			}


            bool bFoundReverseLink = false;

            int iOtherWaypoint = pxWP->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 ToolsAVP_Mode_Waypointing::CalculateWaypointZoneIDs()
{
	UpdateProgress( 0, Asura_AI_Navigation_System::GetNumberOfWaypoints(), "Culling under sized waypoint islands and calculating zone IDs..." );

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

	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
	u_short usFillZone = 0;
	for ( int i = 0; i < Asura_AI_Navigation_System::GetNumberOfWaypoints() && bShouldAbort == false;   )
	{
		if ( UpdateProgress( i, Asura_AI_Navigation_System::GetNumberOfWaypoints(), "Culling under sized waypoint islands and calculating zone IDs..." ) )
		{
			bShouldAbort = true;
		}

		// Skip if not a humanoid waypoint.
		if ( ( Asura_AI_Navigation_System::GetWaypoint( i )->m_usFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_HUMANOID ) == 0 )
		{
			++i;
			continue;
		}

		if ( Asura_AI_Navigation_System::GetWaypoint( i )->m_usZone == ASURA_NAVVERTEX_MAX_ZONE_ID )
		{
			if ( !FloodFillWaypointGraph( i, usFillZone ) )
			{
				bShouldAbort = true;
			}

			// Now check how many waypoints are in this zone. If it's less than the minimum island size cull all the waypoints in this zone.
			int iWPCount = 0;
			for ( int j = 0; j < Asura_AI_Navigation_System::GetNumberOfWaypoints(); ++j )
			{
				if ( Asura_AI_Navigation_System::GetWaypoint( j )->m_usZone == usFillZone )
				{
					++iWPCount;
				}
			}

			if ( iWPCount < s_iMinIslandSize )
			{
				u_short usWPMask = static_cast<u_short>(~AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_HUMANOID);
				u_short usLinkMask = static_cast<u_short>(~AVP_Navigation::AVP_AI_CONNECTION_FLAG_HUMANOID);
				for ( int j = i; j < Asura_AI_Navigation_System::GetNumberOfWaypoints();   )
				{
					Asura_AI_NavVertex* pxWaypoint = Asura_AI_Navigation_System::GetWaypoint( j );
					if ( pxWaypoint->m_usZone == usFillZone &&
						!( pxWaypoint->GetFlags() & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_GS2 ) &&
						!( pxWaypoint->GetFlags() & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ENV_HOOK ) &&
						!( pxWaypoint->GetFlags() & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_PREDATOR_JUMP ) &&
						!( pxWaypoint->GetFlags() & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN_JUMP ))
					{
						// just remove the human flags and mark the zone to non
						pxWaypoint->m_usZone = ASURA_NAVVERTEX_MAX_ZONE_ID;
						pxWaypoint->m_usFlags &= usWPMask;

						// remove human from all the links leading from this waypoint. Since this is an isle - there is no need to look for incoming links
						// they will be cleared when the other waypoints get the same treatment
						for (u_int u = 0; u < pxWaypoint->GetNumLinks(); ++u )
						{
							Asura_AI_NavConnection xLink = pxWaypoint->GetLink(u);
							xLink.m_usFlags &= usLinkMask;
						}
					}
					++j;
				}

			}
			else
			{
				// If we didn't delete any waypoints we need to increment the fill zone to avoid clashes.
				++usFillZone;
			}
		}

		++i;
	}

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

	return true;
}

void ToolsAVP_Mode_Waypointing::RenderLinksHelpText( float& fX, float& fY ) const
{
    Asura_Text_Output::Print( fX, fY++, 0xffffffff, 0, "Link Colours:" );
	switch ( s_iShowDataType )
	{
		case SDT_ALIEN:
		{
			Asura_Text_Output::Print( fX, fY++, 0xffffffff, 0, "Red          - Alien link/waypoint" );
			Asura_Text_Output::Print( fX, fY++, 0xffffffff, 0, "Green        - start of one way link" );
			Asura_Text_Output::Print( fX, fY++, 0xffffffff, 0, "Pink         - transition link" );
			break;
		}

		case SDT_PREDATOR:
		{
			Asura_Text_Output::Print( fX, fY++, 0xffffffff, 0, "White        - predator link/waypoint" );
			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" );
			break;
		}

		case SDT_HUMANOID:
		{
			Asura_Text_Output::Print( fX, fY++, 0xffffffff, 0, "White        - humanoid link/waypoint" );
			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" );
			break;
		}

		default:
		case SDT_ALL:
		{
			break;
		}
	}
}

void ToolsAVP_Mode_Waypointing::GetLinkColour( const unsigned int uFlags, const bool bTwoWayLink, Asura_Vector_3& xStartColour, Asura_Vector_3& xEndColour ) const
{
    if ( bTwoWayLink )
    {
		switch ( s_iShowDataType )
		{
			case SDT_ALIEN:
			{
				const bool bIsTransitionLink = AVP_Navigation::IsTransitionLink( static_cast< u_short >( uFlags ) );
				if ( bIsTransitionLink )
				{
					xStartColour.x = 1.0f;
					xStartColour.y = 0.0f;
					xStartColour.z = 1.0f;
				}
				else
				{
					xStartColour.x = 1.0f;
					xStartColour.y = 0.0f;
					xStartColour.z = 0.0f;
				}
				break;
			}

			case SDT_PREDATOR:
			{
				xStartColour.x = 1.0f;
				xStartColour.y = 1.0f;
				xStartColour.z = 1.0f;
				break;
			}

			case SDT_HUMANOID:
			{
				xStartColour.x = 1.0f;
				xStartColour.y = 1.0f;
				xStartColour.z = 1.0f;
				break;
			}

			default:
			case SDT_ALL:
			{
				xStartColour.x = 1.0f;
				xStartColour.y = 1.0f;
				xStartColour.z = 1.0f;
				break;
			}
		}

        xEndColour = xStartColour;
    }
    else
    {
		switch ( s_iShowDataType )
		{
			case SDT_ALIEN:
			{
				// One way link colour.
				// 0xff00ff00
				xStartColour.x = 0.0f;
				xStartColour.y = 1.0f;
				xStartColour.z = 0.0f;

				const bool bIsTransitionLink = AVP_Navigation::IsTransitionLink( static_cast< u_short >( uFlags ) );
				if ( bIsTransitionLink )
				{
					xEndColour.x = 1.0f;
					xEndColour.y = 0.0f;
					xEndColour.z = 1.0f;
				}
				else
				{
					xEndColour.x = 1.0f;
					xEndColour.y = 0.0f;
					xEndColour.z = 0.0f;
				}
				break;
			}

			case SDT_PREDATOR:
			{
				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
				{
					// One way link colour.
					// 0xff00ff00
					xStartColour.x = 0.0f;
					xStartColour.y = 1.0f;
					xStartColour.z = 0.0f;
				}

				xEndColour.x = 1.0f;
				xEndColour.y = 1.0f;
				xEndColour.z = 1.0f;
				break;
			}

			case SDT_HUMANOID:
			{
				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
				{
					// One way link colour.
					// 0xff00ff00
					xStartColour.x = 0.0f;
					xStartColour.y = 1.0f;
					xStartColour.z = 0.0f;
				}

				xEndColour.x = 1.0f;
				xEndColour.y = 1.0f;
				xEndColour.z = 1.0f;
				break;
			}

			default:
			case SDT_ALL:
			{
				xStartColour.x = 0.0f;
				xStartColour.y = 1.0f;
				xStartColour.z = 0.0f;

				xEndColour.x = 1.0f;
				xEndColour.y = 1.0f;
				xEndColour.z = 1.0f;
				break;
			}
		}
	}
}

void ToolsAVP_Mode_Waypointing::GetWaypointColour( const unsigned int uFlags, int& iColour ) const
{
    if ( s_bRenderColoursAsFWC )
    {
        const bool bIsCeilingWP = ( ( uFlags & ASURA_AI_NAVVERTEX_FLAG_CEILING ) != 0 );
        const bool bIsWallWP    = ( ( uFlags & ASURA_AI_NAVVERTEX_FLAG_WALL )    != 0 );
        Asura_Assert( !( bIsCeilingWP && bIsWallWP ), "ToolsAVP_Mode_Waypointing::GetWaypointColour - bad waypoint flags" );
        if ( bIsCeilingWP )
        {
            iColour = 0x1fff0000;
        }
        else if ( bIsWallWP )
        {
            iColour = 0x1fff8000;
        }
        else
        {
            iColour = 0x1f0000ff;
        }
    }
    else
    {
		switch ( s_iShowDataType )
		{
			case SDT_ALIEN:
			{
				iColour = 0x1fff0000;
				break;
			}

			case SDT_PREDATOR:
			{
				iColour = 0x1fffffff;
				break;
			}

			case SDT_HUMANOID:
			{
				iColour = 0x1fffffff;
				break;
			}

			default:
			case SDT_ALL:
			{
				iColour = 0x1fffffff;
				break;
			}
		}
    }
}

void ToolsAVP_Mode_Waypointing::RenderWaypointInfo( const int iWPIndex, float& fPrintX, float& fPrintY ) const
{
	PARENT::RenderWaypointInfo( iWPIndex, fPrintX, fPrintY );

	const Asura_AI_NavVertex* const pxWaypoint = Asura_AI_Navigation_System::GetWaypoint( iWPIndex );

	Asura_String xFlagsString;
	if ( pxWaypoint->m_usFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_HUMANOID )
	{
		xFlagsString += "Humanoid. ";
	}
	if ( pxWaypoint->m_usFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN )
	{
		xFlagsString += "Alien. ";
	}
	if ( pxWaypoint->m_usFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_PREDATOR )
	{
		xFlagsString += "Predator. ";
	}
	Asura_Text_Output::Print( fPrintX, fPrintY, 0xffffffff, 0, "Type : %s", xFlagsString.GetString() );
	fPrintY += 2.0f;

	for	( u_int u = 0; u < AVP_Waypoint_Manager::GetNumSectors(); ++u )
	{
		Asura_Text_Output::Print( fPrintX, fPrintY, 0xffffffff, 0, "Direction %u : min %.1f, max %.1f",
			u,
			AVP_Waypoint_Manager::GetMinLOFValue( static_cast< u_short >( iWPIndex ), u ),
			AVP_Waypoint_Manager::GetMaxLOFValue( static_cast< u_short >( iWPIndex ), u ) );
		fPrintY += 1.0f;
	}
}

int ToolsAVP_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 ( StaticIsFloorPoly( iPolyFlags, xPolyNormal ) )
	{
		iWaypointFlags |= TOOLS_NAVVERTEX_FLAG_FLOOR;
	}
	else if ( IsWallPoly( xPolyNormal ) )
	{
		iWaypointFlags |= ASURA_AI_NAVVERTEX_FLAG_WALL;
	}
	else if ( IsCeilingPoly( xPolyNormal ) )
	{
		iWaypointFlags |= ASURA_AI_NAVVERTEX_FLAG_CEILING;
	}
	else
	{
		Asura_Assert( false, "ToolsAVP_Mode_Waypointing::GetFlagsForNewWaypoint - unclassified poly" );
	}
	if ( iPolyFlags & AsuraPolygonFlag_DenseWaypoints )
	{
		iWaypointFlags |= TOOLS_NAVVERTEX_FLAG_DENSE;
	}
	return iWaypointFlags;
}

bool ToolsAVP_Mode_Waypointing::StaticIsFloorPoly( const int iPolyFlags, const Asura_Vector_3& xFaceNormal )
{
	return ( ( iPolyFlags & AsuraPolygonFlag_Physics_WalkableSurface ) || ( xFaceNormal.y < -0.707107 ) );
}

bool ToolsAVP_Mode_Waypointing::StaticIsCeilingPoly( const Asura_Vector_3& xFaceNormal )
{
	// Allow anything up to 45° from horizontal.
	return ( xFaceNormal.y > 0.707107f );
}

bool ToolsAVP_Mode_Waypointing::StaticIsWallPoly( const int iPolyFlags, const Asura_Vector_3& xFaceNormal )
{
	// Allow anything between the two 45° from horizontal limits.
	return ( !( iPolyFlags & AsuraPolygonFlag_Physics_WalkableSurface ) && ( -0.707107f <= xFaceNormal.y && xFaceNormal.y <= 0.707107f ) );
}

bool ToolsAVP_Mode_Waypointing::IsCeilingPoly( const Asura_Vector_3& xFaceNormal ) const
{
    // Allow anything up to 45° from horizontal.
    return ( xFaceNormal.y > 0.707107f );
}

bool ToolsAVP_Mode_Waypointing::IsWallPoly( const Asura_Vector_3& xFaceNormal ) const
{
    // Allow anything between the two 45° from horizontal limits.
    return ( -0.707107f <= xFaceNormal.y && xFaceNormal.y <= 0.707107f );
}

Asura_Char* ToolsAVP_Mode_Waypointing::GetPhaseName( const int iPhase )
{
	if ( iPhase == 121 )
    {
        return "Add GS2 Node Waypoints";
    }
	else if ( iPhase == 122 )
	{
		return "Add Env Hook Waypoints";
	}
	else if ( iPhase == 123 )
	{
		return "Add Predator Jump Waypoints";
	}
	else if ( iPhase == 124 )
	{
		return "Add Alien Jump Waypoints";
	}
	else if ( iPhase == 151 )
	{
		return "Flag Waypoints For Actor Type";
	}
    else if ( iPhase == 179 )
    {
        return "Flag Transition Links";
    }
    else if ( iPhase == 201 )
    {
        return "Final Flag Transition Links";
    }
    else if ( iPhase == 202 )
    {
        return "Unflag Transition Links";
    }
    else if ( iPhase == 222 )
    {
        return "Add Jump Links";
    }
	else if ( iPhase == 223 )
	{
		return "Add GS2 Spline Links";
	}
	else if ( iPhase == 224 )
	{
		return "Add Env Hook Links";
	}
	else if ( iPhase == 225 )
	{
		return "Add Predator Jump Links";
	}
	else if ( iPhase == 226 )
	{
		return "Add Alien Jump Links";
	}
	else if ( iPhase == 227 )
	{
		return "Add Lift User-Defined Waypoint Links";
	}
	else if ( iPhase == 281 )
	{
		return "Remove Humanoid Flag From Invalid One Way Links";
	}
	else if ( iPhase == 331 )
	{
		return "Generating Extra Waypointing Data";
	}
	else if ( iPhase == 332)
	{
		if (s_bIncludeLoFData)
		{
			return "Generating LOF";
		}
		else
		{
			return NULL;
		}
	}
    else
    {
        return PARENT::GetPhaseName( iPhase );
    }
}

////////////////////////////////////////////////////////////////////////////////
// Function     : ToolsAVP_Mode_Waypointing::ToolsAVP_Mode_Waypointing
// Author       : Andy Tate
// Date         : 4-Jun-2007
////////////////////////////////////////////////////////////////////////////////

ToolsAVP_Mode_Waypointing::ToolsAVP_Mode_Waypointing()
{
	// Initialise the waypoint-generation values...
    InitialiseStandardBB();
	SetShouldGenerateCoverpoints( false );
	SetGravityFollowsSurfaceNormals( true );
    SetWaypointVolumeCalcThreadFunction( AVPCalculateWaypointVolumeThreadFunction );

    Asura_Vector_3 xMovedObjectAllowedOffset = GetMovedObjectAllowedOffset();
    xMovedObjectAllowedOffset.y = -GetPlacedObjectAllowedMovement();
    SetMovedObjectAllowedOffset( xMovedObjectAllowedOffset );
}

bool ToolsAVP_Mode_Waypointing::InitialiseStandardBB()
{
	const AVP_Collision_Descriptor* pxCD = AVP_Collision_Descriptor::GetDataByHash( 0x4d1c76ec );	// Alien Posture Crawling
	Asura_Assert( pxCD, "ToolsAVP_Mode_Waypointing::InitialiseStandardBB - could not get Alien collision descriptor" );
	if ( !pxCD ) { return false; }

	Asura_Bounding_Box xAlienBox = pxCD->GetCollisionBox()->GetBoundingBox();

    // Tweak the box so it is symmetrical in the XY plane.
    const Asura_Vector_3 xHalfRange = ( xAlienBox.GetMax() - xAlienBox.GetMin() ) * 0.5f;
    xAlienBox.MinZ = -xHalfRange.z;
    xAlienBox.MaxZ = xHalfRange.z;

    SetTestCollisionBox( xAlienBox );

    return true;
}

bool ToolsAVP_Mode_Waypointing::FlagWaypointsForActorType()
{
    // Flag the waypoints for humanoid, Alien, and predator based on normals and radii.
    for ( int i = 0; i < s_iNumWaypoints; ++i )
    {
		// ignore env hooks, predator jumps and gs2 
		if (s_pxWaypointStorage[ i ].m_iFlags & 
			( AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_GS2 | 
			  AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ENV_HOOK | 
			  AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_PREDATOR_JUMP |
			  AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN_JUMP) )
		{
			continue;
		}

        if ( s_pxWaypointStorage[ i ].IsCeiling() || s_pxWaypointStorage[ i ].IsWall() )
        {
            s_pxWaypointStorage[ i ].m_iFlags |= AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN;
        }
        else if ( s_pxWaypointStorage[ i ].m_fRadius > s_fMinAlienWaypointRadius )
        {
            s_pxWaypointStorage[ i ].m_iFlags |= AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN;
			s_pxWaypointStorage[ i ].m_iFlags |= AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_HUMANOID;
            s_pxWaypointStorage[ i ].m_iFlags |= AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_PREDATOR;
        }
        else
        {
            s_pxWaypointStorage[ i ].m_iFlags |= AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_HUMANOID;
			s_pxWaypointStorage[ i ].m_iFlags |= AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_PREDATOR;
        }

		if ( s_pxWaypointStorage[ i ].m_iFlags & TOOLS_NAVVERTEX_FLAG_LIFT_UDW )
		{
			// Lift UDWs can always be used by humanoids.
			s_pxWaypointStorage[ i ].m_iFlags |= AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_HUMANOID;
		}
    }

    return true;
}

bool ToolsAVP_Mode_Waypointing::AddGS2Waypoints()
{
	// Starting at each initial node (a node with the correct structure type set) add the waypoints for the whole structure.
	// This uses the same flood fill algorithm used for zoning.
	// First set all nodes as white.
	int iNumNodes = 0;
	for ( Asura_Hashed_List_It< AVP_ServerNode_GS2Node > xGS2NodeIter( AVP_ServerNode_GS2Node::GetList() );
		!xGS2NodeIter.Done();
		xGS2NodeIter.Next() )
	{
		xGS2NodeIter.GetCurrent()->m_uTraverseHelper = s_usWhiteZoneID;
		++iNumNodes;
	}

	int iLoopCount = 0;
	for ( Asura_Hashed_List_It< AVP_ServerNode_GS2Node > xGS2NodeIter( AVP_ServerNode_GS2Node::GetList() );
		!xGS2NodeIter.Done();
		xGS2NodeIter.Next(), ++iLoopCount )
	{
		if ( UpdateProgress( iLoopCount, iNumNodes, "Adding GS2 Waypoints..." ) )
		{
			return false;
		}

		AVP_ServerNode_GS2Node* pxGS2InitialNode = xGS2NodeIter.GetCurrent();
		if ( pxGS2InitialNode->GetStructureType() != AVP_GS_ST_ALIEN_VENT && pxGS2InitialNode->GetStructureType() != AVP_GS_ST_PREDATOR_JUMP  )
		{
			continue;
		}
		if ( !pxGS2InitialNode->ShouldIntegrateWithNavData() )
		{
			continue;
		}
		if ( pxGS2InitialNode->m_uTraverseHelper != s_usWhiteZoneID )
		{
			// Must have touched this node already during traversal.
			continue;
		}

		// Add the whole of the rest of this structure.
		Asura_LinkList< Asura_Guid > xPendingQueue;
		pxGS2InitialNode->m_uTraverseHelper = s_usGrayZoneID;
		xPendingQueue.Push_Back( pxGS2InitialNode->GetGuid() );
		while ( xPendingQueue.Front() != NULL )
		{
			Asura_ListElt< Asura_Guid >* pxElt = xPendingQueue.Front();
			const Asura_Guid uSourceNodeGuid = pxElt->Data();
			xPendingQueue.Remove( pxElt );

			AVP_ServerNode_GS2Node* pxSourceNode = AVP_ServerNode_GS2Node::FindEntity( uSourceNodeGuid );
			if ( !pxSourceNode ) { return false; }

			// Add the node.
			AddGS2Waypoint( *pxSourceNode, pxGS2InitialNode->GetStructureType() );

			// Traverse all of this node's links, adding each target node.
			for ( u_int uLink = 0; uLink < pxSourceNode->GetNumberLinks(); ++uLink )
			{
				const AVP_ServerNode_GS2Spline* pxLink =
					static_cast< const AVP_ServerNode_GS2Spline* >( Asura_ServerNode_GS2Spline::FindEntity( pxSourceNode->GetLink( uLink ) ) );
				if ( !pxLink || !pxLink->ShouldIntegrateWithNavData() )
				{
					// If the link doesn't exist or shouldn't be added continue.
					continue;
				}
				AVP_ServerNode_GS2Node* pxTargetNode = AVP_ServerNode_GS2Node::FindEntity( pxLink->GetTargetGuid() );
				if ( !pxTargetNode || pxTargetNode->m_uTraverseHelper != s_usWhiteZoneID )
				{
					// If the target node doesn't exist or has already been dealth with continue.
					continue;
				}

				// Add the target node to the pending queue.
				pxTargetNode->m_uTraverseHelper = s_usGrayZoneID;
				xPendingQueue.Push_Back( pxTargetNode->GetGuid() );
			}

			// Flag the source node as black, to show it has been finished with.
			pxSourceNode->m_uTraverseHelper = s_usBlackZoneID;
		}
	}

	return true;
}

bool ToolsAVP_Mode_Waypointing::AddGS2Waypoint( const AVP_ServerNode_GS2Node& xGS2Node, const int iStructureType )
{
	Tools_Nav_Vertex* pxWP = NULL;

	const Asura_Vector_3& xWPPosition = xGS2Node.GetPosition();
	Asura_Matrix_3x3 xGSNodeOrient;
	xGS2Node.GetOrientation( xGSNodeOrient );
	const Asura_Vector_3 xWPNormal = Asura_Vector_3( 0.0f, -1.0f, 0.0f ) * xGSNodeOrient;

	// See if this waypoint already exists.
// 	for ( int iBin = 0; iBin < NUM_WAYPOINT_BINS; ++iBin )
// 	{
// 		for ( int iWP = 0; iWP < s_aiNumWaypoints[ iBin ]; ++iWP )
// 		{
// 			if ( s_apxWaypointStorage[ iBin ][ iWP ].m_xPosition.ApproximatelyEquals( xWPPosition ) &&
// 				s_apxWaypointStorage[ iBin ][ iWP ].m_xNormal.ApproximatelyEquals( xWPNormal ) )
// 			{
// 				pxWP = &s_apxWaypointStorage[ iBin ][ iWP ];				
// 				break;
// 			}
// 		}
// 	}

	if ( !pxWP )
	{
		// 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 )
			{
				s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_xPosition = xWPPosition;
				s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_xNormal   = xWPNormal;

				// Try and find the poly this node is on to get the poly flags.
				static const float fSURFACE_TOLERANCE = 0.25f;
				int iPolyFlags = 0;
				int iPolyIndex, iModuleIndex;
				float fDistanceToHit;
				if ( Tools_LOS::FindPolyInLOS( xWPPosition + xWPNormal * fSURFACE_TOLERANCE, -xWPNormal, iPolyIndex, iModuleIndex, fDistanceToHit ) )
				{
					if ( fDistanceToHit < fSURFACE_TOLERANCE * 2.0f )
					{
						const Asura_Mesh* pxMesh = Asura_Module_System::GetMeshForModule( iModuleIndex );
						if ( pxMesh )
						{
							iPolyFlags = pxMesh->GetPolygonFlags( iPolyIndex );
						}
					}
				}
				s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_iFlags = GetFlagsForNewWaypoint( iPolyFlags, xWPNormal );
				s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_iFlags |= AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_GS2 | AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN;
				s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_fRadius = 0.25f;

				pxWP = &s_apxWaypointStorage[ iBin ][ iNumWPsInBin ];
				++iNumWPsInBin;
				break;
			}
		}
	}

	if ( pxWP )
	{
		// Make sure it's got the gs2 flag.
		pxWP->m_iFlags |= AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_GS2;

		// Also add the species flag based on the GS2 structure type.
		switch ( iStructureType )
		{
			case AVP_GS_ST_ALIEN_VENT:
			{
				pxWP->m_iFlags |= AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN;
				if (xGS2Node.GetNumIncomingLinks() == 0 ) // this is the first link in the GS sequence
				{
					Gs2WayPointID xGS2ID;
					xGS2ID.m_xPosition = pxWP->m_xPosition;
					xGS2ID.m_xNormal = pxWP->m_xNormal;
					s_xAlienInVentGS2.Push_Back(xGS2ID);
				}
				if (xGS2Node.GetNumberLinks() == 0 ) // this is the last link in the GS sequence
				{
					Gs2WayPointID xGS2ID;
					xGS2ID.m_xPosition = pxWP->m_xPosition;
					xGS2ID.m_xNormal = pxWP->m_xNormal;
					s_xAlienOutVentGS2.Push_Back(xGS2ID);
				}
				break;
			}

			case AVP_GS_ST_PREDATOR_JUMP:
			{
				pxWP->m_iFlags |= AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_PREDATOR;
				break;
			}

			default: break;
		}

		return true;
	}
	else
	{
		return false;
	}
}

bool ToolsAVP_Mode_Waypointing::AddEnvHookWaypoints()
{
	// Loop round all attractors.
	for ( Asura_Hashed_List_It< Tools_Attractor > xAttIter( Tools_Attractor_System::GetInstance()->GetAttractorList() ); !xAttIter.Done(); xAttIter.Next() )
	{
		Tools_Attractor* pxAttractor = xAttIter.GetCurrent();
		if ( pxAttractor->GetType() != uAVP_ATTRACTOR_TYPE_BRANCH && pxAttractor->GetType() != uAVP_ATTRACTOR_TYPE_DUCT )
		{
			continue;
		}

		Tools_Attractor_Node* pxPrevNode = pxAttractor->GetFirstNode();
		Tools_Attractor_Node* pxNode	 = pxPrevNode ? pxPrevNode->GetNext() : NULL;
		while ( pxPrevNode && pxNode )
		{
			Asura_Vector_3 xDirection = pxNode->GetPosition() - pxPrevNode->GetPosition();
			xDirection.Normalise();
			Asura_Vector_3 xNormal;
			pxAttractor->GetNormalFromDirection( xNormal, xDirection );

			AddEnvHookWaypoint( pxPrevNode->GetPosition(), xNormal );
			AddEnvHookWaypoint( pxNode->GetPosition(), xNormal );

			pxPrevNode = pxNode;
			pxNode	   = pxNode->GetNext();
		}
	}

	return true;
}

bool ToolsAVP_Mode_Waypointing::AddEnvHookWaypoint( const Asura_Vector_3& xPosition, const Asura_Vector_3& xNormal )
{
	Tools_Nav_Vertex* pxWP = NULL;

	// See if this waypoint already exists.
	for ( int iBin = 0; iBin < NUM_WAYPOINT_BINS; ++iBin )
	{
		for ( int iWP = 0; iWP < s_aiNumWaypoints[ iBin ]; ++iWP )
		{
			if ( s_apxWaypointStorage[ iBin ][ iWP ].m_xPosition.ApproximatelyEquals( xPosition ) &&
				s_apxWaypointStorage[ iBin ][ iWP ].m_xNormal.ApproximatelyEquals( xNormal ) )
			{
				pxWP = &s_apxWaypointStorage[ iBin ][ iWP ];
				break;
			}
		}
	}

	if ( !pxWP )
	{
		// 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 )
			{
				s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_xPosition = xPosition;
				s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_xNormal   = xNormal;

				// Try and find the poly this node is on to get the poly flags.
				static const float fSURFACE_TOLERANCE = 0.25f;
				int iPolyFlags = 0;
				int iPolyIndex, iModuleIndex;
				float fDistanceToHit;
				if ( Tools_LOS::FindPolyInLOS( xPosition + xNormal * fSURFACE_TOLERANCE, -xNormal, iPolyIndex, iModuleIndex, fDistanceToHit ) )
				{
					if ( fDistanceToHit < fSURFACE_TOLERANCE * 2.0f )
					{
						const Asura_Mesh* pxMesh = Asura_Module_System::GetMeshForModule( iModuleIndex );
						if ( pxMesh )
						{
							iPolyFlags = pxMesh->GetPolygonFlags( iPolyIndex );
						}
					}
				}
				s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_iFlags = GetFlagsForNewWaypoint( iPolyFlags, xNormal );
				s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_iFlags |= AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ENV_HOOK;
				s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_fRadius = 0.25f;

				pxWP = &s_apxWaypointStorage[ iBin ][ iNumWPsInBin ];

				++iNumWPsInBin;
				break;
			}
		}
	}

	if ( pxWP )
	{
		// Make sure it's got the env hook flag.
		pxWP->m_iFlags |= AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ENV_HOOK;
		return true;
	}
	else
	{
		return false;
	}
}

bool ToolsAVP_Mode_Waypointing::AddPredatorJumpWaypoints()
{
	// Loop round all attractors.
	for ( Asura_Hashed_List_It< Tools_Attractor > xAttIter( Tools_Attractor_System::GetInstance()->GetAttractorList() ); !xAttIter.Done(); xAttIter.Next() )
	{
		Tools_Attractor* pxAttractor = xAttIter.GetCurrent();
		if ( pxAttractor->GetType() != uAVP_ATTRACTOR_TYPE_PREDATOR_JUMP )
		{
			continue;
		}

		Tools_Attractor_Node* pxNode = pxAttractor->GetFirstNode();

		if (pxNode->GetNext() == NULL)
		{
			Tools_MessageLog::AddMessage(Tools_MessageLog::WAYPOINTING, "Generate Predator Jump Waypoints", 
				"Predator EvnHook at position (%f, %f, %f) is orphan - ignored", pxNode->GetPosition().GetX(), pxNode->GetPosition().GetY(), pxNode->GetPosition().GetZ());
			continue;
		}

		while ( pxNode )
		{
			Asura_Vector_3 xNormal;
			if (!GetSurfaceNormalAtPosition(pxNode->GetPosition(), xNormal, AVP_PolygonFlag_IgnoreForPredatorCollision | AsuraPolygonFlag_NoCollisions ))
			{
				Tools_MessageLog::AddMessage(Tools_MessageLog::WAYPOINTING, "Generate Predator Jump Waypoints", 
					"Predator EvnHook at position (%f, %f, %f) failed to deduce normal", pxNode->GetPosition().GetX(), pxNode->GetPosition().GetY(), pxNode->GetPosition().GetZ());
				xNormal = Asura_Vector_3(0.0f, -1.0f, 0.0f);
			}

			if( !AddPredJumpWaypoint( pxNode->GetPosition(), xNormal )) 
			{
				Tools_MessageLog::AddMessage(Tools_MessageLog::WAYPOINTING, "Generate Predator Jump Waypoints", 
					"Predator EvnHook at position (%f, %f, %f) failed to add waypoint", pxNode->GetPosition().GetX(), pxNode->GetPosition().GetY(), pxNode->GetPosition().GetZ());
			}

			pxNode = pxNode->GetNext();
		}
	}

	return true;
}

DEBUGVAR float dbg_fSmallNormalDifferenceCos = Asura_Maths::Cosine(Asura_Maths::DegToRad(5.0f));

bool ToolsAVP_Mode_Waypointing::AddPredJumpWaypoint( const Asura_Vector_3& xPosition, const Asura_Vector_3& xNormal )
{
	Tools_Nav_Vertex* pxWP = NULL;

	// See if this waypoint already exists.
// 	for ( int iBin = 0; iBin < NUM_WAYPOINT_BINS; ++iBin )
// 	{
// 		for ( int iWP = 0; iWP < s_aiNumWaypoints[ iBin ]; ++iWP )
// 		{
// 			if ( s_apxWaypointStorage[ iBin ][ iWP ].m_xPosition.ApproximatelyEquals( xPosition, 0.2f ) &&
// 				s_apxWaypointStorage[ iBin ][ iWP ].m_xNormal * xNormal > dbg_fSmallNormalDifferenceCos )
// 			{
// 				pxWP = &s_apxWaypointStorage[ iBin ][ iWP ];
// 				break;
// 			}
// 		}
// 	}

	if ( !pxWP )
	{
		// 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 )
			{
				s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_xPosition = xPosition;
				s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_xNormal   = xNormal;

				// Try and find the poly this node is on to get the poly flags.
				static const float fSURFACE_TOLERANCE = 0.25f;
				int iPolyFlags = 0;
				int iPolyIndex, iModuleIndex;
				float fDistanceToHit;
				if ( Tools_LOS::FindPolyInLOS( xPosition + xNormal * fSURFACE_TOLERANCE, -xNormal, iPolyIndex, iModuleIndex, fDistanceToHit ) )
				{
					if ( fDistanceToHit < fSURFACE_TOLERANCE * 2.0f )
					{
						const Asura_Mesh* pxMesh = Asura_Module_System::GetMeshForModule( iModuleIndex );
						if ( pxMesh )
						{
							iPolyFlags = pxMesh->GetPolygonFlags( iPolyIndex );
						}
					}
				}
				s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_iFlags = GetFlagsForNewWaypoint( iPolyFlags, xNormal );
				s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_iFlags |= AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_PREDATOR_JUMP | AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_PREDATOR;
				s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_fRadius = 0.25f;

				pxWP = &s_apxWaypointStorage[ iBin ][ iNumWPsInBin ];

				++iNumWPsInBin;
				break;
			}
		}
	}

	if ( pxWP )
	{
		// Make sure it's got the env hook flag.
		pxWP->m_iFlags |= AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_PREDATOR_JUMP | AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_PREDATOR;
		return true;
	}
	else
	{
		return false;
	}
}

bool ToolsAVP_Mode_Waypointing::AddAlienJumpWaypoints()
{
	// Loop round all attractors.
	for ( Asura_Hashed_List_It< Tools_Attractor > xAttIter( Tools_Attractor_System::GetInstance()->GetAttractorList() ); !xAttIter.Done(); xAttIter.Next() )
	{
		Tools_Attractor* pxAttractor = xAttIter.GetCurrent();
		u_int uType = pxAttractor->GetType();
		if ( uType != uAVP_ATTRACTOR_TYPE_ALIEN_AI_JUMP && uType != uAVP_ATTRACTOR_TYPE_ALIEN_AI_JUMP_OVER )
		{
			continue;
		}
		
		Tools_Attractor_Node* pxNode = pxAttractor->GetFirstNode();

		if (pxNode->GetNext() == NULL)
		{
			Tools_MessageLog::AddMessage(Tools_MessageLog::WAYPOINTING, "Generate Alien Jump Waypoints", 
				"Alien EvnHook at position (%f, %f, %f) is orphan - ignored", pxNode->GetPosition().GetX(), pxNode->GetPosition().GetY(), pxNode->GetPosition().GetZ());
			continue;
		}

		while ( pxNode )
		{
			if (uType != pxNode->GetAttractor()->GetType())
			{
				Tools_MessageLog::AddMessage(Tools_MessageLog::WAYPOINTING, "Generate Alien Jump Waypoints", 
					"Alien EvnHook type at position (%f, %f, %f) does not match other EnvHook", pxNode->GetPosition().GetX(), pxNode->GetPosition().GetY(), pxNode->GetPosition().GetZ());
			}
			bool bJumpOver = uType == uAVP_ATTRACTOR_TYPE_ALIEN_AI_JUMP_OVER;

			Asura_Vector_3 xNormal;
			if (!GetSurfaceNormalAtPositionAnyDirection(pxNode->GetPosition(), xNormal, AVP_PolygonFlag_IgnoreForAlienCollision | AsuraPolygonFlag_NoCollisions ))
			{
				Tools_MessageLog::AddMessage(Tools_MessageLog::WAYPOINTING, "Generate Alien Jump Waypoints", 
					"Alien EvnHook at position (%f, %f, %f) failed to deduce normal", pxNode->GetPosition().GetX(), pxNode->GetPosition().GetY(), pxNode->GetPosition().GetZ());
				xNormal = Asura_Vector_3(0.0f, -1.0f, 0.0f);
			}

			if( !AddAlienJumpWaypoint( pxNode->GetPosition(), xNormal, bJumpOver )) 
			{
				Tools_MessageLog::AddMessage(Tools_MessageLog::WAYPOINTING, "Generate Alien Jump Waypoints", 
					"Alien EvnHook at position (%f, %f, %f) failed to add waypoint", pxNode->GetPosition().GetX(), pxNode->GetPosition().GetY(), pxNode->GetPosition().GetZ());
			}

			pxNode = pxNode->GetNext();
		}
	}

	return true;
}

bool ToolsAVP_Mode_Waypointing::AddAlienJumpWaypoint( const Asura_Vector_3& xPosition, const Asura_Vector_3& xNormal, bool bJumpOver )
{
	Tools_Nav_Vertex* pxWP = NULL;

	// See if this waypoint already exists.
// 	for ( int iBin = 0; iBin < NUM_WAYPOINT_BINS; ++iBin )
// 	{
// 		for ( int iWP = 0; iWP < s_aiNumWaypoints[ iBin ]; ++iWP )
// 		{
// 			if ( s_apxWaypointStorage[ iBin ][ iWP ].m_xPosition.ApproximatelyEquals( xPosition, 0.2f ) &&
// 				s_apxWaypointStorage[ iBin ][ iWP ].m_xNormal * xNormal > dbg_fSmallNormalDifferenceCos )
// 			{
// 				pxWP = &s_apxWaypointStorage[ iBin ][ iWP ];
// 				break;
// 			}
// 		}
// 	}

	if ( !pxWP )
	{
		// 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 )
			{
				s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_xPosition = xPosition;
				s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_xNormal   = xNormal;

				// Try and find the poly this node is on to get the poly flags.
				static const float fSURFACE_TOLERANCE = 0.25f;
				int iPolyFlags = 0;
				int iPolyIndex, iModuleIndex;
				float fDistanceToHit;
				if ( Tools_LOS::FindPolyInLOS( xPosition + xNormal * fSURFACE_TOLERANCE, -xNormal, iPolyIndex, iModuleIndex, fDistanceToHit ) )
				{
					if ( fDistanceToHit < fSURFACE_TOLERANCE * 2.0f )
					{
						const Asura_Mesh* pxMesh = Asura_Module_System::GetMeshForModule( iModuleIndex );
						if ( pxMesh )
						{
							iPolyFlags = pxMesh->GetPolygonFlags( iPolyIndex );
						}
					}
				}
				s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_iFlags = GetFlagsForNewWaypoint( iPolyFlags, xNormal );
				s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_iFlags |= AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN_JUMP | AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN;
				if (bJumpOver)
				{
					s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_iFlags |= AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN_JUMP_OVER;
				}
				s_apxWaypointStorage[ iBin ][ iNumWPsInBin ].m_fRadius = 0.25f;

				pxWP = &s_apxWaypointStorage[ iBin ][ iNumWPsInBin ];

				++iNumWPsInBin;
				break;
			}
		}
	}

	if ( pxWP )
	{
		// Make sure it's got the env hook flag.
		pxWP->m_iFlags |= AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN_JUMP | AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN;
		if (bJumpOver)
		{
			pxWP->m_iFlags |= AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN_JUMP_OVER;
		}
		return true;
	}
	else
	{
		return false;
	}
}




bool ToolsAVP_Mode_Waypointing::AddJumpLinks()
{
    // Need to create a temporary waypoint tree to allow searching quickly.
    s_pxWaypointSearcher = new AVP_Tools_Waypoint_Searcher( s_pxWaypointStorage, s_iNumWaypoints );
    if ( !s_pxWaypointSearcher || !s_pxWaypointSearcher->IsOk() )
    {
        return false;
    }

    const bool bOk = ( AddFloorCeilingJumpLinks() && AddCeilingFloorFallLinks() && AddWallWallJumpLinks() );

    delete s_pxWaypointSearcher;
    s_pxWaypointSearcher = NULL;

    return bOk;
}

bool ToolsAVP_Mode_Waypointing::AddGS2Links()
{
	// Starting at each initial node (a node with the correct structure type set) traverse and add the links for the whole structure.
	// Doing it this way allows us to flag the links correctly whilst we go along.
	// We use the same flood fill algorithm used for zoning.

	// First set all nodes as white.
	int iNumNodes = 0;
	for ( Asura_Hashed_List_It< AVP_ServerNode_GS2Node > xGS2NodeIter( AVP_ServerNode_GS2Node::GetList() );
		!xGS2NodeIter.Done();
		xGS2NodeIter.Next() )
	{
		xGS2NodeIter.GetCurrent()->m_uTraverseHelper = s_usWhiteZoneID;
		++iNumNodes;
	}

	// Now do each structure in turn.
	int iLoopCount = 0;
	for ( Asura_Hashed_List_It< AVP_ServerNode_GS2Node > xGS2NodeIter( AVP_ServerNode_GS2Node::GetList() );
		!xGS2NodeIter.Done();
		xGS2NodeIter.Next(), ++iLoopCount )
	{
		if ( UpdateProgress( iLoopCount, iNumNodes, "Adding GS2 Links..." ) )
		{
			return false;
		}

		AVP_ServerNode_GS2Node* pxGS2InitialNode = xGS2NodeIter.GetCurrent();
		if ( pxGS2InitialNode->GetStructureType() != AVP_GS_ST_ALIEN_VENT && pxGS2InitialNode->GetStructureType() != AVP_GS_ST_PREDATOR_JUMP )
		{
			continue;
		}
		if ( !pxGS2InitialNode->ShouldIntegrateWithNavData() )
		{
			continue;
		}
		if ( pxGS2InitialNode->m_uTraverseHelper != s_usWhiteZoneID )
		{
			// Must have touched this node already during traversal.
			continue;
		}

		const bool bIsAlienStructure = ( pxGS2InitialNode->GetStructureType() == AVP_GS_ST_ALIEN_VENT );

		// Add the whole of this structure.
		Asura_LinkList< Asura_Guid > xPendingQueue;
		xPendingQueue.Push_Back( pxGS2InitialNode->GetGuid() );
		while ( xPendingQueue.Front() != NULL )
		{
			Asura_ListElt< Asura_Guid >* pxElt = xPendingQueue.Front();
			const Asura_Guid uSourceNodeGuid = pxElt->Data();
			xPendingQueue.Remove( pxElt );

			AVP_ServerNode_GS2Node* pxSourceNode = AVP_ServerNode_GS2Node::FindEntity( uSourceNodeGuid );
			if ( !pxSourceNode ) { return false; }

			// Traverse all of this node's links, adding each link.
			for ( u_int uLink = 0; uLink < pxSourceNode->GetNumberLinks(); ++uLink )
			{
				const AVP_ServerNode_GS2Spline* pxLink =
					static_cast< const AVP_ServerNode_GS2Spline* >( Asura_ServerNode_GS2Spline::FindEntity( pxSourceNode->GetLink( uLink ) ) );
				if ( !pxLink || !pxLink->ShouldIntegrateWithNavData() )
				{
					// If the link doesn't exist or shouldn't be added continue.
					continue;
				}
				AVP_ServerNode_GS2Node* pxTargetNode = AVP_ServerNode_GS2Node::FindEntity( pxLink->GetTargetGuid() );
				if ( !pxTargetNode )
				{
					continue;
				}
				// We need to find the waypoints associated with the source and target nodes.
				Tools_Nav_Vertex* pxSourceWaypoint = FindGS2Waypoint( *pxSourceNode );
				Tools_Nav_Vertex* pxTargetWaypoint = FindGS2Waypoint( *pxTargetNode );
				if ( !pxSourceWaypoint || !pxTargetWaypoint )
				{
					continue;
				}

				// Add this link to the nav data.
				Tools_Nav_Link xLink;
				xLink.m_iIndex  = GetWaypointIndex( pxTargetWaypoint );
				xLink.m_uFlags  = bIsAlienStructure ? AVP_Navigation::AVP_AI_CONNECTION_FLAG_ALIEN : AVP_Navigation::AVP_AI_CONNECTION_FLAG_PREDATOR;
				xLink.m_uFlags |= AVP_Navigation::AVP_AI_CONNECTION_FLAG_GS2;
				xLink.m_fLength = ( pxTargetWaypoint->m_xPosition - pxSourceWaypoint->m_xPosition ).Magnitude();
				pxSourceWaypoint->m_xLinks += xLink;

				// Finally add the target node to the pending queue if it has not already been dealt with.
				if ( pxTargetNode->m_uTraverseHelper == s_usWhiteZoneID )
				{
					pxTargetNode->m_uTraverseHelper = s_usGrayZoneID;
					xPendingQueue.Push_Back( pxTargetNode->GetGuid() );
				}
			}

			// Flag the source node as black, to show it has been finished with.
			pxSourceNode->m_uTraverseHelper = s_usBlackZoneID;
		}
	}

	return true;
}

bool ToolsAVP_Mode_Waypointing::AddEnvHookLinks()
{
	// Count the attractors.
	int iNumAttractors = 0;
	for ( Asura_Hashed_List_It< Tools_Attractor > xAttIter( Tools_Attractor_System::GetInstance()->GetAttractorList() ); !xAttIter.Done(); xAttIter.Next() )
	{
		Tools_Attractor* pxAttractor = xAttIter.GetCurrent();
		if ( pxAttractor->GetType() != uAVP_ATTRACTOR_TYPE_BRANCH && pxAttractor->GetType() != uAVP_ATTRACTOR_TYPE_DUCT )
		{
			continue;
		}

		++iNumAttractors;
	}

	// Now do each set of linked attractors in turn.
	int iLoopCount = 0;
	for ( Asura_Hashed_List_It< Tools_Attractor > xAttIter( Tools_Attractor_System::GetInstance()->GetAttractorList() );
		!xAttIter.Done();
		xAttIter.Next(), ++iLoopCount )
	{
		Tools_Attractor* pxAttractor = xAttIter.GetCurrent();
		if ( pxAttractor->GetType() != uAVP_ATTRACTOR_TYPE_BRANCH && pxAttractor->GetType() != uAVP_ATTRACTOR_TYPE_DUCT )
		{
			continue;
		}

		if ( UpdateProgress( iLoopCount, iNumAttractors, "Adding Env Hook Links..." ) )
		{
			return false;
		}

		// Add the links along this attractor.
		Tools_Attractor_Node* pxPrevNode = pxAttractor->GetFirstNode();
		Tools_Attractor_Node* pxNode	 = pxPrevNode ? pxPrevNode->GetNext() : NULL;
		while ( pxPrevNode && pxNode )
		{
			Asura_Vector_3 xDirection = pxNode->GetPosition() - pxPrevNode->GetPosition();
			xDirection.Normalise();
			Asura_Vector_3 xNormal;
			pxAttractor->GetNormalFromDirection( xNormal, xDirection );

			Tools_Nav_Vertex* pxPrevNodeWP = FindEnvHookWaypoint( pxPrevNode->GetPosition(), &xNormal );
			Tools_Nav_Vertex* pxNodeWP     = FindEnvHookWaypoint( pxNode->GetPosition(), &xNormal );
			if ( pxPrevNodeWP && pxNodeWP )
			{
				// Add a link each way between the two waypoints.
				Tools_Nav_Link xLink;
				xLink.m_iIndex  = GetWaypointIndex( pxNodeWP );
				xLink.m_uFlags  = AVP_Navigation::AVP_AI_CONNECTION_FLAG_ALIEN;
				xLink.m_uFlags |= AVP_Navigation::AVP_AI_CONNECTION_FLAG_ENV_HOOK;
				xLink.m_fLength = ( pxNodeWP->m_xPosition - pxPrevNodeWP->m_xPosition ).Magnitude();
				pxPrevNodeWP->m_xLinks += xLink;

				xLink.m_iIndex = GetWaypointIndex( pxPrevNodeWP );
				pxNodeWP->m_xLinks += xLink;
			}

			pxPrevNode = pxNode;
			pxNode	   = pxNode->GetNext();
		}

		// Add any links to other attractors necessary.
		for ( u_int uLink = 0; uLink < pxAttractor->GetNumLinks(); ++uLink )
		{
			Tools_Attractor* pxLinkedAttractor = Tools_Attractor_System::GetInstance()->GetAttractorList()->FindEntity( pxAttractor->GetLink( uLink ) );
			if ( !pxLinkedAttractor ) { continue; }

			// We need to establish which of the base and linked attractors' nodes are the closest and link only those two.
            const Tools_Attractor_Node* pxBaseNode   = NULL;
			const Tools_Attractor_Node* pxLinkedNode = NULL;
			float fDistance = 0.0f;
			FindNearestEnvHookNodePair( pxAttractor, pxLinkedAttractor, pxBaseNode, pxLinkedNode, fDistance );
			if ( !pxBaseNode || !pxLinkedNode ) { continue; }

            Tools_Nav_Vertex* pxBaseNodeWP   = FindEnvHookWaypoint( pxBaseNode->GetPosition() );
			Tools_Nav_Vertex* pxLinkedNodeWP = FindEnvHookWaypoint( pxLinkedNode->GetPosition() );

			// Add a link to the linked attractor (but not back the other way).
			if (pxBaseNodeWP && pxLinkedNodeWP )
			{
				Tools_Nav_Link xLink;
				xLink.m_iIndex  = GetWaypointIndex( pxLinkedNodeWP );
				xLink.m_uFlags  = AVP_Navigation::AVP_AI_CONNECTION_FLAG_ALIEN;
				xLink.m_uFlags |= AVP_Navigation::AVP_AI_CONNECTION_FLAG_ENV_HOOK;
				xLink.m_fLength = fDistance;
				pxBaseNodeWP->m_xLinks += xLink;
			}

		}
	}

	return true;
}

bool ToolsAVP_Mode_Waypointing::AddPredatorJumpLinks()
{
	// Count the attractors.
	int iNumAttractors = 0;
	for ( Asura_Hashed_List_It< Tools_Attractor > xAttIter( Tools_Attractor_System::GetInstance()->GetAttractorList() ); !xAttIter.Done(); xAttIter.Next() )
	{
		if ( xAttIter.GetCurrent()->GetType() == uAVP_ATTRACTOR_TYPE_PREDATOR_JUMP )
		{
			++iNumAttractors;
		}

	}

	AVP_Tools_Waypoint_Searcher xWaypointSearcher( s_pxWaypointStorage, s_iNumWaypoints );

	// Now do each set of linked attractors in turn.
	int iLoopCount = 0;
	for ( Asura_Hashed_List_It< Tools_Attractor > xAttIter( Tools_Attractor_System::GetInstance()->GetAttractorList() );
		!xAttIter.Done();
		xAttIter.Next(), ++iLoopCount )
	{
		Tools_Attractor* pxAttractor = xAttIter.GetCurrent();
		if ( pxAttractor->GetType() != uAVP_ATTRACTOR_TYPE_PREDATOR_JUMP)
		{
			continue;
		}

		if ( UpdateProgress( iLoopCount, iNumAttractors, "Adding Predator Jump Links..." ) )
		{
			return false;
		}

		// Add the links along this attractor.
		Tools_Attractor_Node* pxNode1 = pxAttractor->GetFirstNode();
		Tools_Attractor_Node* pxNode2 = pxNode1 ? pxNode1->GetNext() : NULL;
		while ( pxNode1 && pxNode2 )
		{


			int iWP1Index, iWP2Index;
			Tools_Nav_Vertex* pxVertex1 = FindPredatorJumpWaypoint(pxNode1->GetPosition(), iWP1Index);
			Tools_Nav_Vertex* pxVertex2 = FindPredatorJumpWaypoint(pxNode2->GetPosition(), iWP2Index);

			if (pxVertex1 == NULL)
			{
				Asura_Vector_3 xPos = pxNode1->GetPosition();
				Tools_MessageLog::AddMessage(Tools_MessageLog::WAYPOINTING, "Generate Predator Jump links", 
											 "Predator EvnHook at position (%f, %f, %f) failed to find waypoint", xPos.GetX(), xPos.GetY(), xPos.GetZ());
			}
			else if (pxVertex2 == NULL)
			{
				Asura_Vector_3 xPos = pxNode2->GetPosition();
				Tools_MessageLog::AddMessage(Tools_MessageLog::WAYPOINTING, "Generate Predator Jump links", 
					"Predator EvnHook at position (%f, %f, %f) failed to find waypoint", xPos.GetX(), xPos.GetY(), xPos.GetZ());
			}
			else
			{

				if (!ValidatePredatorJumpTest(pxVertex1->m_xPosition, pxVertex2->m_xPosition) || !ValidatePredatorJumpTest(pxVertex2->m_xPosition, pxVertex1->m_xPosition))
				{
					Asura_Vector_3 xPos1 = pxNode1->GetPosition();
					Asura_Vector_3 xPos2 = pxNode2->GetPosition();
					Tools_MessageLog::AddMessage(Tools_MessageLog::WAYPOINTING, "Generate Predator Jump links", 
						"Predator can not jump between EvnHooks (%f, %f, %f) and (%f, %f, %f)", 
						xPos1.GetX(), xPos1.GetY(), xPos1.GetZ(),
						xPos2.GetX(), xPos2.GetY(), xPos2.GetZ());

				}
				else
				{
					// Add a link each way between the two waypoints.
					Tools_Nav_Link xLink;
					xLink.m_iIndex  = iWP2Index;
					xLink.m_uFlags  = AVP_Navigation::AVP_AI_CONNECTION_FLAG_PREDATOR | AVP_Navigation::AVP_AI_CONNECTION_FLAG_TRANSITION_HIGH_MOBILITY;
					xLink.m_fLength = ( pxVertex1->m_xPosition - pxVertex2->m_xPosition ).Magnitude();
					pxVertex1->m_xLinks += xLink;

					xLink.m_iIndex = iWP1Index;
					pxVertex2->m_xLinks += xLink;

				}
			}

			pxNode1 = pxNode2;
			pxNode2 = pxNode2->GetNext();
		}
	}
	return true;
}

bool ToolsAVP_Mode_Waypointing::AddAlienJumpLinks()
{
	// Count the attractors.
	int iNumAttractors = 0;
	for ( Asura_Hashed_List_It< Tools_Attractor > xAttIter( Tools_Attractor_System::GetInstance()->GetAttractorList() ); !xAttIter.Done(); xAttIter.Next() )
	{
		if ( xAttIter.GetCurrent()->GetType() == uAVP_ATTRACTOR_TYPE_ALIEN_AI_JUMP )
		{
			++iNumAttractors;
		}

	}

	AVP_Tools_Waypoint_Searcher xWaypointSearcher( s_pxWaypointStorage, s_iNumWaypoints );

	// Now do each set of linked attractors in turn.
	int iLoopCount = 0;
	for ( Asura_Hashed_List_It< Tools_Attractor > xAttIter( Tools_Attractor_System::GetInstance()->GetAttractorList() );
		!xAttIter.Done();
		xAttIter.Next(), ++iLoopCount )
	{
		Tools_Attractor* pxAttractor = xAttIter.GetCurrent();
		if ( pxAttractor->GetType() != uAVP_ATTRACTOR_TYPE_ALIEN_AI_JUMP && pxAttractor->GetType() != uAVP_ATTRACTOR_TYPE_ALIEN_AI_JUMP_OVER)
		{
			continue;
		}

		bool bJumpOver = pxAttractor->GetType() == uAVP_ATTRACTOR_TYPE_ALIEN_AI_JUMP_OVER;

		if ( UpdateProgress( iLoopCount, iNumAttractors, "Adding Alien Jump Links..." ) )
		{
			return false;
		}

		// Add the links along this attractor.
		Tools_Attractor_Node* pxNode1 = pxAttractor->GetFirstNode();
		Tools_Attractor_Node* pxNode2 = pxNode1 ? pxNode1->GetNext() : NULL;
		while ( pxNode1 && pxNode2 )
		{


			int iWP1Index, iWP2Index;
			Tools_Nav_Vertex* pxVertex1 = FindAlienJumpWaypoint(pxNode1->GetPosition(), iWP1Index, bJumpOver);
			Tools_Nav_Vertex* pxVertex2 = FindAlienJumpWaypoint(pxNode2->GetPosition(), iWP2Index, bJumpOver);

			if (pxVertex1 == NULL)
			{
				Asura_Vector_3 xPos = pxNode1->GetPosition();
				Tools_MessageLog::AddMessage(Tools_MessageLog::WAYPOINTING, "Generate Alien Jump links", 
											 "Alien EvnHook at position (%f, %f, %f) failed to find waypoint", xPos.GetX(), xPos.GetY(), xPos.GetZ());
			}
			else if (pxVertex2 == NULL)
			{
				Asura_Vector_3 xPos = pxNode2->GetPosition();
				Tools_MessageLog::AddMessage(Tools_MessageLog::WAYPOINTING, "Generate Alien Jump links", 
					"Alien EvnHook at position (%f, %f, %f) failed to find waypoint", xPos.GetX(), xPos.GetY(), xPos.GetZ());
			}
			else
			{

				bool bAddLinks = true;
				if (bJumpOver) // verify that the link is jump-able
				{
					int iVariation;
					const Asura_Vector_3& xPos1 = pxVertex1->m_xPosition;
					const Asura_Vector_3& xPos2 = pxVertex2->m_xPosition;
					if(!AVP_Alien_Movement::GetJumpOverVariation(xPos1, xPos2, ASURA_GUID_UNREGISTERED, iVariation))
					{
						Tools_MessageLog::AddMessage(Tools_MessageLog::WAYPOINTING, "Generate Alien Jump links", 
							"Jump over from (%f, %f, %f) to is not possible (%f, %f, %f)", 
							xPos1.GetX(), xPos1.GetY(), xPos1.GetZ(), 
							xPos2.GetX(), xPos2.GetY(), xPos2.GetZ()
							);
						bAddLinks = false;
					}
					if(!AVP_Alien_Movement::GetJumpOverVariation(xPos2, xPos1, ASURA_GUID_UNREGISTERED, iVariation))
					{
						Tools_MessageLog::AddMessage(Tools_MessageLog::WAYPOINTING, "Generate Alien Jump links", 
							"Jump over from (%f, %f, %f) to is not possible (%f, %f, %f)", 
							xPos2.GetX(), xPos2.GetY(), xPos2.GetZ(),
							xPos1.GetX(), xPos1.GetY(), xPos1.GetZ() 
							);
						bAddLinks = false;
					}
				}

				// Add a link each way between the two waypoints.
				if (bAddLinks)
				{
					Tools_Nav_Link xLink;
					xLink.m_iIndex  = iWP2Index;
					xLink.m_uFlags  = AVP_Navigation::AVP_AI_CONNECTION_FLAG_ALIEN | AVP_Navigation::AVP_AI_CONNECTION_FLAG_TRANSITION_HIGH_MOBILITY | AVP_Navigation::AVP_AI_CONNECTION_FLAG_ENV_HOOK;
					xLink.m_fLength = ( pxVertex1->m_xPosition - pxVertex2->m_xPosition ).Magnitude();
					pxVertex1->m_xLinks += xLink;

					xLink.m_iIndex = iWP1Index;
					pxVertex2->m_xLinks += xLink;

				}
			}

			pxNode1 = pxNode2;
			pxNode2 = pxNode2->GetNext();
		}
	}
	return true;
}



bool ToolsAVP_Mode_Waypointing::AddLiftUDWLinks()
{
	for ( u_int u = 0; u < Asura_AI_Navigation_System::GetNumUserDefinedWaypoints(); ++u )
	{
		if ( s_pxWaypointStorage[ u ].m_iFlags & TOOLS_NAVVERTEX_FLAG_LIFT_UDW )
		{
			const ToolsAVP_Waypoint_Object* pxAVPWPObj =
				static_cast< const ToolsAVP_Waypoint_Object* >( Tools_Waypoint_Object::GetList()->FindObject( s_pxWaypointStorage[ u ].GetUDWPID() ) );
			if ( pxAVPWPObj )
			{
				for ( Asura_LinkList_ConstIt< u_int > xLinkIt( pxAVPWPObj->GetAVPUDW().m_xLinkedUDWs ); !xLinkIt.Done(); xLinkIt.Next() )
				{
					const u_int uDestinationUDWIP = *xLinkIt.GetCurrent();

					// Add a link to the destination waypoint with the correct UDWID.
					for ( int k = 0; k < s_iNumWaypoints; ++k )
					{
						if ( s_pxWaypointStorage[ k ].GetUDWPID() == uDestinationUDWIP )
						{
							Tools_Nav_Link xNavLink;
							xNavLink.m_iIndex  = k;
							xNavLink.m_fLength = ( s_pxWaypointStorage[ k ].m_xPosition - s_pxWaypointStorage[ u ].m_xPosition ).Magnitude();
							xNavLink.m_uFlags  = AVP_Navigation::AVP_AI_CONNECTION_FLAG_HUMANOID;
							s_pxWaypointStorage[ u ].m_xLinks += xNavLink;
							break;
						}
					}
				}
			}
			else
			{
				return false;
			}
		}
	}

	return true;
}

Tools_Nav_Vertex* ToolsAVP_Mode_Waypointing::FindGS2Waypoint( const AVP_ServerNode_GS2Node& xGS2Node ) const
{
	// We could perhaps take advantage of the fact that the GS2 waypoints will be at the end of the array until
	// the final culling stages after linking. However, it's safer to not assume this so that this function will
	// work at any stage of the calculations.

	Asura_Matrix_3x3 xNodeOrient;

	for ( int iWP = 0; iWP < s_iNumWaypoints; ++iWP )
	{
		if ( !( s_pxWaypointStorage[ iWP ].m_iFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_GS2 ) )
		{
			continue;
		}

		if ( !xGS2Node.GetPosition().ApproximatelyEquals( s_pxWaypointStorage[ iWP ].m_xPosition ) )
		{
			continue;
		}

		xGS2Node.GetOrientation( xNodeOrient );
		const Asura_Vector_3 xUp = Asura_Vector_3( 0.0f, -1.0f, 0.0f ) * xNodeOrient;

		if ( !xUp.ApproximatelyEquals( s_pxWaypointStorage[ iWP ].m_xNormal ) )
		{
			continue;
		}

		return &s_pxWaypointStorage[ iWP ];
	}

	return NULL;
}

Tools_Nav_Vertex* ToolsAVP_Mode_Waypointing::FindEnvHookWaypoint( const Asura_Vector_3& xPosition, const Asura_Vector_3* pxNormal /*= NULL*/ ) const
{
	for ( int iWP = 0; iWP < s_iNumWaypoints; ++iWP )
	{
		if ( !( s_pxWaypointStorage[ iWP ].m_iFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ENV_HOOK ) )
		{
			continue;
		}

		if ( !xPosition.ApproximatelyEquals( s_pxWaypointStorage[ iWP ].m_xPosition ) )
		{
			continue;
		}

		if ( pxNormal && !pxNormal->ApproximatelyEquals( s_pxWaypointStorage[ iWP ].m_xNormal ) )
		{
			continue;
		}

		return &s_pxWaypointStorage[ iWP ];
	}

	return NULL;
}

Tools_Nav_Vertex* ToolsAVP_Mode_Waypointing::FindPredatorJumpWaypoint( const Asura_Vector_3& xPosition, int& iWPIndex) const
{
	iWPIndex = -1;
	for ( int iWP = 0; iWP < s_iNumWaypoints; ++iWP )
	{
		if ( !( s_pxWaypointStorage[ iWP ].m_iFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_PREDATOR_JUMP ) )
		{
			continue;
		}

		if ( !xPosition.ApproximatelyEquals( s_pxWaypointStorage[ iWP ].m_xPosition, s_pxWaypointStorage[ iWP ].m_fRadius ) )
		{
			continue;
		}

		iWPIndex = iWP;
		return &s_pxWaypointStorage[ iWP ];
	}

	return NULL;
}

Tools_Nav_Vertex* ToolsAVP_Mode_Waypointing::FindAlienJumpWaypoint( const Asura_Vector_3& xPosition, int& iWPIndex, bool bJumpOver) const
{
	iWPIndex = -1;
	for ( int iWP = 0; iWP < s_iNumWaypoints; ++iWP )
	{
		if ( !( s_pxWaypointStorage[ iWP ].m_iFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN_JUMP ) )
		{
			continue;
		}

		if (bJumpOver)
		{
			if ( !( s_pxWaypointStorage[ iWP ].m_iFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN_JUMP_OVER ) )
			{
				continue;
			}
		}
		else
		{
			if ( ( s_pxWaypointStorage[ iWP ].m_iFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN_JUMP_OVER ) )
			{
				continue;
			}
		}

		if ( !xPosition.ApproximatelyEquals( s_pxWaypointStorage[ iWP ].m_xPosition, s_pxWaypointStorage[ iWP ].m_fRadius ) )
		{
			continue;
		}

		iWPIndex = iWP;
		return &s_pxWaypointStorage[ iWP ];
	}

	return NULL;
}

void ToolsAVP_Mode_Waypointing::FindNearestEnvHookNodePair( const Tools_Attractor* pxBaseAttractor, const Tools_Attractor* pxLinkedAttractor,
														    const Tools_Attractor_Node*& pxBaseNode, const Tools_Attractor_Node*& pxLinkedNode,
															float& fDistance )
{
	// There may well be algorithms for this on the net, but the only applicable one I found requires the two sets of points to be separated
	// by a plane, which we cannot guarantee here. Just using brute force, then, which shouldn't matter because the sets are very small.
	pxBaseNode   = NULL;
	pxLinkedNode = NULL;

	float fLeastDistanceSq = Asura_Maths::LargeFloat;
	const Tools_Attractor_Node* pxBaseSearchNode = pxBaseAttractor->GetFirstNode();
	while ( pxBaseSearchNode )
	{
		const Tools_Attractor_Node* pxLinkedSearchNode = pxLinkedAttractor->GetFirstNode();
		while ( pxLinkedSearchNode )
		{
			const float fDistanceSq = ( pxBaseSearchNode->GetPosition() - pxLinkedSearchNode->GetPosition() ).MagnitudeSquared();
			if ( fDistanceSq < fLeastDistanceSq )
			{
				fLeastDistanceSq = fDistanceSq;

                pxBaseNode   = pxBaseSearchNode;
				pxLinkedNode = pxLinkedSearchNode;
			}

			pxLinkedSearchNode = pxLinkedSearchNode->GetNext();
		}

		pxBaseSearchNode = pxBaseSearchNode->GetNext();
	}

	fDistance = Asura_Maths::Sqrt( fLeastDistanceSq );
}

int ToolsAVP_Mode_Waypointing::GetWaypointIndex( const Tools_Nav_Vertex* pxWaypoint ) const
{
	if ( SomeBinWaypointsExist() )
	{
		return -1;
	}
	else
	{
		return ( pxWaypoint - s_pxWaypointStorage );
	}
}

bool ToolsAVP_Mode_Waypointing::AddFloorCeilingJumpLinks()
{
    Asura_LOS_InputData         xLOSInputData( Asura_Maths::ZeroVector3, Asura_Maths::ZeroVector3, ASURA_GUID_UNREGISTERED );
    const Asura_LOS_OutputData* pxLOSOutputData;

    const float fLOSTestDistance          = s_fMaxFloorCeilingJumpDistance * 1.1f;
    const float fDotSurfaceAngleTolerance = Asura_Maths::Cosine( Asura_Maths::DegToRad( s_fJumpTargetSurfaceAngleTolerance ) );

    // Fire rays up from Alien floor waypoints to see if we hit the ceiling inside any ceiling waypoints.
    for ( int i = 0; i < s_iNumWaypoints; ++i )
    {
        if ( UpdateProgress( i, s_iNumWaypoints, "Generating Floor->Ceiling Jump Links..." ) )
        {
            return false;
        }

        Tools_Nav_Vertex& xBaseWP = s_pxWaypointStorage[ i ];

        if ( !( xBaseWP.m_iFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN ) )
        {
            continue;
        }
        if ( xBaseWP.IsCeiling() || xBaseWP.IsWall() )
        {
            continue;
        }

        const Asura_Vector_3 xSourcePosition = xBaseWP.m_xPosition + xBaseWP.m_xNormal * 0.1f;
        xLOSInputData.Initialise( xSourcePosition, xBaseWP.m_xNormal, ASURA_GUID_UNREGISTERED, fLOSTestDistance );
        pxLOSOutputData = Asura_LOS::TestAgainstEnvironment( &xLOSInputData );
        if ( pxLOSOutputData->m_bHitEnvironment && pxLOSOutputData->m_fDistanceToIntersection < fLOSTestDistance )
        {
            // We've hit the environment.
            // Is the jump length is within our min/max range?
            const float fJumpLength = pxLOSOutputData->m_fDistanceToIntersection + 0.1f;
            if ( s_fMinFloorCeilingJumpDistance <= fJumpLength && fJumpLength <= s_fMaxFloorCeilingJumpDistance )
            {
                // Find the nearest waypoint to our hit position.
                const Asura_Vector_3 xHitPosition = xSourcePosition + ( xBaseWP.m_xNormal * pxLOSOutputData->m_fDistanceToIntersection );
                const int iNearestWP = s_pxWaypointSearcher->FindAlienWaypointNearestTo( xHitPosition );
                if ( iNearestWP != AVP_Tools_Waypoint_Searcher::INVALID_SEARCH_RESULT )
                {
                    // Is the waypoint a ceiling waypoint?
                    if ( !s_pxWaypointStorage[ iNearestWP ].IsCeiling() )
                    {
                        continue;
                    }

                    // Is the waypoint normal within the acceptable angle tolerance?
                    const Asura_Vector_3 xIdealNormal = -xBaseWP.m_xNormal;
                    if ( xIdealNormal * s_pxWaypointStorage[ iNearestWP ].m_xNormal > fDotSurfaceAngleTolerance )
                    {
                        // Add a link between the two waypoints.
						Tools_Nav_Link xNavLink;
						xNavLink.m_iIndex  = iNearestWP;
						xNavLink.m_fLength = fJumpLength;
						xNavLink.m_uFlags  = AVP_Navigation::AVP_AI_CONNECTION_FLAG_ALIEN;
						xNavLink.m_uFlags |= AVP_Navigation::AVP_AI_CONNECTION_FLAG_TRANSITION_HIGH_MOBILITY;
						xBaseWP.m_xLinks += xNavLink;
                    }
                }
            }
        }
    }

    return true;
}

bool ToolsAVP_Mode_Waypointing::AddCeilingFloorFallLinks()
{
    Asura_LOS_InputData         xLOSInputData( Asura_Maths::ZeroVector3, Asura_Maths::ZeroVector3, ASURA_GUID_UNREGISTERED );
    const Asura_LOS_OutputData* pxLOSOutputData;

    const float fLOSTestDistance          = s_fMaxCeilingFloorFallDistance * 1.1f;
    const float fDotSurfaceAngleTolerance = Asura_Maths::Cosine( Asura_Maths::DegToRad( s_fJumpTargetSurfaceAngleTolerance ) );

    // Fire rays down from ceiling waypoints to see if we hit the floor inside any Alien waypoints.
    for ( int i = 0; i < s_iNumWaypoints; ++i )
    {
        if ( UpdateProgress( i, s_iNumWaypoints, "Generating Ceiling->Floor Fall Links..." ) )
        {
            return false;
        }

        Tools_Nav_Vertex& xBaseWP = s_pxWaypointStorage[ i ];

        if ( !( xBaseWP.m_iFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN ) )
        {
            continue;
        }
        if ( !xBaseWP.IsCeiling() )
        {
            continue;
        }

        const Asura_Vector_3 xSourcePosition = xBaseWP.m_xPosition + xBaseWP.m_xNormal * 0.1f;
        xLOSInputData.Initialise( xSourcePosition, xBaseWP.m_xNormal, ASURA_GUID_UNREGISTERED, fLOSTestDistance );
        pxLOSOutputData = Asura_LOS::TestAgainstEnvironment( &xLOSInputData );
        if ( pxLOSOutputData->m_bHitEnvironment && pxLOSOutputData->m_fDistanceToIntersection < fLOSTestDistance )
        {
            // We've hit the environment.
            // Is the jump length is within our min/max range?
            const float fJumpLength = pxLOSOutputData->m_fDistanceToIntersection + 0.1f;
            if ( s_fMinCeilingFloorFallDistance <= fJumpLength && fJumpLength <= s_fMaxCeilingFloorFallDistance )
            {
                // Find the nearest waypoint to our hit position.
                const Asura_Vector_3 xHitPosition = xSourcePosition + ( xBaseWP.m_xNormal * pxLOSOutputData->m_fDistanceToIntersection );
                const int iNearestWP = s_pxWaypointSearcher->FindAlienWaypointNearestTo( xHitPosition );
                if ( iNearestWP != AVP_Tools_Waypoint_Searcher::INVALID_SEARCH_RESULT )
                {
                    // Is the waypoint a floor waypoint?
                    if ( s_pxWaypointStorage[ iNearestWP ].IsCeiling() || s_pxWaypointStorage[ iNearestWP ].IsWall() )
                    {
                        continue;
                    }

                    // Is the waypoint normal within the acceptable angle tolerance?
                    const Asura_Vector_3 xIdealNormal = -xBaseWP.m_xNormal;
                    if ( xIdealNormal * s_pxWaypointStorage[ iNearestWP ].m_xNormal > fDotSurfaceAngleTolerance )
                    {
                        // Add a link between the two waypoints.
						Tools_Nav_Link xNavLink;
						xNavLink.m_iIndex  = iNearestWP;
						xNavLink.m_fLength = fJumpLength;
						xNavLink.m_uFlags  = AVP_Navigation::AVP_AI_CONNECTION_FLAG_ALIEN;
						xNavLink.m_uFlags |= AVP_Navigation::AVP_AI_CONNECTION_FLAG_TRANSITION_LOW_MOBILITY;
						xBaseWP.m_xLinks += xNavLink;
                    }
                }
            }
        }
    }

    return true;
}

bool ToolsAVP_Mode_Waypointing::AddWallWallJumpLinks()
{
    Asura_LOS_InputData         xLOSInputData( Asura_Maths::ZeroVector3, Asura_Maths::ZeroVector3, ASURA_GUID_UNREGISTERED );
    const Asura_LOS_OutputData* pxLOSOutputData;

    const float fLOSTestDistance          = s_fMaxWallWallJumpDistance * 1.1f;
    const float fDotSurfaceAngleTolerance = Asura_Maths::Cosine( Asura_Maths::DegToRad( s_fJumpTargetSurfaceAngleTolerance ) );

    // Fire rays sideways from wall waypoints to see if we hit the opposite wall inside any wall waypoints.
    for ( int i = 0; i < s_iNumWaypoints; ++i )
    {
        if ( UpdateProgress( i, s_iNumWaypoints, "Generating Wall->Wall Jump Links..." ) )
        {
            return false;
        }

        Tools_Nav_Vertex& xBaseWP = s_pxWaypointStorage[ i ];

        if ( !( xBaseWP.m_iFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN ) )
        {
            continue;
        }
        if ( !xBaseWP.IsWall() )
        {
            continue;
        }

        const Asura_Vector_3 xSourcePosition = xBaseWP.m_xPosition + xBaseWP.m_xNormal * 0.1f;
        xLOSInputData.Initialise( xSourcePosition, xBaseWP.m_xNormal, ASURA_GUID_UNREGISTERED, fLOSTestDistance );
        pxLOSOutputData = Asura_LOS::TestAgainstEnvironment( &xLOSInputData );
        if ( pxLOSOutputData->m_bHitEnvironment && pxLOSOutputData->m_fDistanceToIntersection < fLOSTestDistance )
        {
            // We've hit the environment.
            // Is the jump length is within our min/max range?
            const float fJumpLength = pxLOSOutputData->m_fDistanceToIntersection + 0.1f;
            if ( s_fMinWallWallJumpDistance <= fJumpLength && fJumpLength <= s_fMaxWallWallJumpDistance )
            {
                // Find the nearest waypoint to our hit position.
                const Asura_Vector_3 xHitPosition = xSourcePosition + ( xBaseWP.m_xNormal * pxLOSOutputData->m_fDistanceToIntersection );
                const int iNearestWP = s_pxWaypointSearcher->FindAlienWaypointNearestTo( xHitPosition );
                if ( iNearestWP != AVP_Tools_Waypoint_Searcher::INVALID_SEARCH_RESULT )
                {
                    // Is the waypoint a wall waypoint?
                    if ( !s_pxWaypointStorage[ iNearestWP ].IsWall() )
                    {
                        continue;
                    }

                    // Is the waypoint normal within the acceptable angle tolerance?
                    const Asura_Vector_3 xIdealNormal = -xBaseWP.m_xNormal;
                    if ( xIdealNormal * s_pxWaypointStorage[ iNearestWP ].m_xNormal > fDotSurfaceAngleTolerance )
                    {
                        // Add a link between the two waypoints.
						Tools_Nav_Link xNavLink;
						xNavLink.m_iIndex  = iNearestWP;
						xNavLink.m_fLength = fJumpLength;
						xNavLink.m_uFlags  = AVP_Navigation::AVP_AI_CONNECTION_FLAG_ALIEN;
						xNavLink.m_uFlags |= AVP_Navigation::AVP_AI_CONNECTION_FLAG_TRANSITION_MED_MOBILITY;
						xBaseWP.m_xLinks += xNavLink;
                    }
                }
            }
        }
    }

    return true;
}

bool ToolsAVP_Mode_Waypointing::RemoveHumanoidFlagFromInvalidOneWayLinks()
{
	for ( int i = 0; i < Asura_AI_Navigation_System::GetNumberOfWaypoints(); ++i )
    {   
        if ( UpdateProgress( i, Asura_AI_Navigation_System::GetNumberOfWaypoints(), "Flagging Certain One-Way Links as Alien Only..." ) )
        {
            return false;
        }

		Asura_AI_NavVertex* pxWP = Asura_AI_Navigation_System::GetWaypoint( i );

        for ( int c = 0; c < pxWP->m_ucNumConnections; ++c )
        {
			// Don't cull GS2 links.
			if ( pxWP->m_pxConnections[ c ].GetFlags() & AVP_Navigation::AVP_AI_CONNECTION_FLAG_GS2 )
			{
				continue;
			}

			// Don't cull env hook links.
			if ( pxWP->m_pxConnections[ c ].GetFlags() & AVP_Navigation::AVP_AI_CONNECTION_FLAG_ENV_HOOK )
			{
				continue;
			}

            bool bFoundReverseHumanoidLink = false;

			{
				Asura_AI_NavVertex* pxOtherWP = Asura_AI_Navigation_System::GetWaypoint( pxWP->m_pxConnections[ c ].m_usIndex );

				for ( int iOtherConnections = 0; iOtherConnections < pxOtherWP->m_ucNumConnections; ++iOtherConnections )
				{
					if ( ( pxOtherWP->m_pxConnections[ iOtherConnections ].m_usFlags & AVP_Navigation::AVP_AI_CONNECTION_FLAG_HUMANOID ) && ( pxOtherWP->m_pxConnections[ iOtherConnections ].m_usIndex == i ) ) 
					{
						bFoundReverseHumanoidLink = true;
						break;
					}
				}
			}

			// Is this is a humanoid one way link
			if ( bFoundReverseHumanoidLink == true ) 
			{
				bool bHasOtherHumanoidLink = false;

				// Do we have any other humanoid links?
				for ( int iConnection = 0; iConnection < pxWP->m_ucNumConnections; ++iConnection )
				{
					if ( iConnection == c )
					{
						continue;
					}

					if ( pxWP->m_pxConnections[ iConnection ].GetFlags() & AVP_Navigation::AVP_AI_CONNECTION_FLAG_HUMANOID )
					{
						bHasOtherHumanoidLink = true;
					}
				}

				if ( !bHasOtherHumanoidLink )
				{
					Asura_AI_NavVertex* pxWPToAdjust = Asura_AI_Navigation_System::GetWaypoint( pxWP->m_pxConnections[ c ].m_usIndex );

					for ( int iOtherConnections = 0; iOtherConnections < pxWPToAdjust->m_ucNumConnections; ++iOtherConnections )
					{
						if ( ( pxWPToAdjust->m_pxConnections[ iOtherConnections ].GetFlags() & AVP_Navigation::AVP_AI_CONNECTION_FLAG_HUMANOID ) && ( pxWPToAdjust->m_pxConnections[ iOtherConnections ].m_usIndex == i ) ) 
						{
							pxWPToAdjust->m_pxConnections[ iOtherConnections ].m_usFlags &= ~AVP_Navigation::AVP_AI_CONNECTION_FLAG_HUMANOID;
							break;
						}
					}
				}
			}
			else // this isn't a humanoid one way link so it may be an all species one way link.
			{
				bool bFoundReverseLink = false;

				{
					Asura_AI_NavVertex* pxOtherWP = Asura_AI_Navigation_System::GetWaypoint( pxWP->m_pxConnections[ c ].m_usIndex );

					for ( int iOtherConnections = 0; iOtherConnections < pxOtherWP->m_ucNumConnections; ++iOtherConnections )
					{
						if ( pxOtherWP->m_pxConnections[ iOtherConnections ].m_usIndex == i ) 
						{
							bFoundReverseLink = true;
							break;
						}
					}
				}

				// If it is an all species one way link
				if ( bFoundReverseLink == false )
				{
					bool bHasOtherHumanoidLink = false;

					Asura_AI_NavVertex* pxOtherWP = Asura_AI_Navigation_System::GetWaypoint( pxWP->m_pxConnections[ c ].m_usIndex );

					// Do we have any other humanoid links?
					for ( int iConnection = 0; iConnection < pxOtherWP->m_ucNumConnections; ++iConnection )
					{
						if ( pxOtherWP->m_pxConnections[ iConnection ].GetFlags() & AVP_Navigation::AVP_AI_CONNECTION_FLAG_HUMANOID )
						{
							bHasOtherHumanoidLink = true;
						}
					}

					// If we do not have any other humanoid one way links
					if ( !bHasOtherHumanoidLink )
					{
						if ( pxWP->m_pxConnections[ c ].GetFlags() & AVP_Navigation::AVP_AI_CONNECTION_FLAG_HUMANOID )
						{
							pxWP->m_pxConnections[ c ].m_usFlags &= ~AVP_Navigation::AVP_AI_CONNECTION_FLAG_HUMANOID;
							break;
						}
					}
				}
			}
		}
	}

	return true;
}

bool ToolsAVP_Mode_Waypointing::ValidatePredatorJumpTest(const Asura_Vector_3& xFromPosition, const Asura_Vector_3& xToPosition) const
{

	const Asura_Blueprint* pxBlueprint = Asura_Blueprint_System::GetBlueprintByHashID(uAVP_BLUEPRINT_TYPE_MOVESPREDATOR, 0x49c653e6 ); // "TargetedJumpSetWristblades"

	Asura_Assert(pxBlueprint, "Predator Blueprint not found");
	if (!pxBlueprint) 
	{
		return false; 
	}

	// To simplify our approximation, just use the y part of the head marker offset. Should still be good enough.
	// Since the jump may change our orientation and look values, attempting to be exact here is pretty much always going to fail.
	Asura_Vector_3 xHeadOffset( 0.0f, -2.0f, 0.0f);  

// 	Asura_Hierarchy_Skin* pxPredatorSkin = Tools_Animation_Files::GetSkinFromID(0xb31282f7); // "Predator"
// 	Asura_Assert(pxPredatorSkin, "Predator Skin not found");
// 	if ( !pxPredatorSkin || !pxPredatorSkin->GetMarkerPositionInRestPose(AVP_Vertebrate_Description::GetMarkerPointHashID(AVP_Vertebrate_Description::MPID_TARGET_2),&xHeadOffset))
// 	{
// 		Asura_Assert(false, "Head marker position not found");
// 		xHeadOffset.SetX(0.0f); xHeadOffset.SetY(-2.0f); xHeadOffset.SetZ(0.0f);  
// 	}

	Asura_Vector_3 xStartPosition[2];
	xStartPosition[0] = xFromPosition  + Asura_Vector_3(0.0f, -0.1, 0.0f); // add small value to not hit the ground.
	xStartPosition[1] = xFromPosition + Asura_Vector_3(0.0f, xHeadOffset.y, 0.0f);
	Asura_Vector_3 xEndPosition[2];
	xEndPosition[0] = xToPosition  + Asura_Vector_3(0.0f, -0.1, 0.0f);
	xEndPosition[1] = xToPosition + Asura_Vector_3(0.0f, xHeadOffset.y, 0.0f);

	Asura_Value xValue;
	float fBestMaxFlatDistanceSquared = Asura_Maths::LargeFloat;
	float fBestMinHeightChange = 0.0f; // This will always be positive, regardless of the sign of the actual change.
	float fBestHeadroom = 0.0f;
	int iBestVariation = -1;

	Asura_Vector_3 xFlatOffset = xToPosition - xFromPosition;
	const float fHeightOffset = Asura_Maths::Abs(xFlatOffset.y);
	const float fHeightOffsetSign = Asura_Maths::Sign(xFlatOffset.y);
	xFlatOffset.y = 0.0f;
	const float fFlatDistanceSquared = xFlatOffset.MagnitudeSquared();

	// do the nominal test
	for (int i=0; i<pxBlueprint->GetNumParameters(); ++i)
	{
		if (pxBlueprint->GetParameterValueByIndex(i, xValue))
		{

			const Asura_Blueprint* pxVariationBlueprint = Asura_Blueprint_System::GetBlueprintByHashID(uAVP_BLUEPRINT_TYPE_MOVESPREDATOR, xValue.GetHashIDValue());
			if (!pxVariationBlueprint) { continue; }

			float fMaxFlatDistanceSquared;
			float fMinHeightChange;
			float fHeadroom;

			if (!pxVariationBlueprint->GetParameterValueByHashID(uAVP_BLUEPRINT_PARAM_MAXDISTANCE, xValue)) { continue; }
			fMaxFlatDistanceSquared = xValue.GetFloatValue();
			fMaxFlatDistanceSquared *= fMaxFlatDistanceSquared;

			if (!pxVariationBlueprint->GetParameterValueByHashID(uAVP_BLUEPRINT_PARAM_HEIGHTCHANGE, xValue)) { continue; }
			fMinHeightChange = xValue.GetFloatValue();

			if (!pxVariationBlueprint->GetParameterValueByHashID(uAVP_BLUEPRINT_PARAM_HEADROOM, xValue)) { continue; }
			fHeadroom = xValue.GetFloatValue();

			// First, make sure its height change is appropriate. 0 height change is always a valid option.
			if ((fMinHeightChange != 0.0f) && (fHeightOffsetSign != Asura_Maths::Sign(fMinHeightChange))) { continue; }
			fMinHeightChange = Asura_Maths::Abs(fMinHeightChange);
			if (fMinHeightChange < fBestMinHeightChange) { continue; }
			if (fMinHeightChange > fHeightOffset) { continue; }

			// Next, make sure it's within valid distance.
			if (fMaxFlatDistanceSquared < fFlatDistanceSquared) { continue; }
			// Only need to compare with the best if our height changes are equal since that's the priority comparison.
			if ((fMinHeightChange == fBestMinHeightChange) && (fMaxFlatDistanceSquared > fBestMaxFlatDistanceSquared)) { continue; }

			// Finally, the headroom check. We want the greatest clearance we think should be safe. We're rapidly approaching needing LOS tests...
			// Headroom is only a deciding factor if the other two values are equal to the previous best.
			if ((fMinHeightChange == fBestMinHeightChange) && (fMaxFlatDistanceSquared == fBestMaxFlatDistanceSquared) && (fHeadroom < fBestHeadroom)) { continue; }
			// We need two LOS tests to make a reasonable guess - from the current head position to an approximation of head position at the midpoint, and from there to the end head position.
			bool bFailed = false;
			Asura_Vector_3 xDir1 = ((xFromPosition + xToPosition) * 0.5f + Asura_Vector_3(0.0f, -fHeadroom, 0.0f)) - xFromPosition;
			Asura_Vector_3 xDir2 = xToPosition - ((xFromPosition + xToPosition) * 0.5f + Asura_Vector_3(0.0f, -fHeadroom, 0.0f));
			float fDistance1 = xDir1.Normalise();
			float fDistance2 = xDir2.Normalise();

			for (int j =0; j < 2 && !bFailed; ++j)
			{
				const Asura_Vector_3 xMidpoint = (xStartPosition[j] + xEndPosition[j]) * 0.5f + Asura_Vector_3(0.0f, -fHeadroom, 0.0f);

				Asura_LOS_InputData xInput(xStartPosition[j], xDir1, ASURA_GUID_UNREGISTERED, fDistance1, false, AVP_PolygonFlag_IgnoreForPredatorCollision | AsuraPolygonFlag_NoCollisions);
				const Asura_LOS_OutputData* pxOutput = Asura_LOS::TestAgainstEverything(&xInput);
				if (pxOutput->m_fDistanceToIntersection < fDistance1) 
				{ 
					bFailed = true; 
					continue; 
				}

				xInput.m_xOrigin = xMidpoint;
				xInput.m_xDirection = xDir2;
				xInput.m_fInitialMaximumRange = fDistance2;
				pxOutput = Asura_LOS::TestAgainstEverything(&xInput);
				if (pxOutput->m_fDistanceToIntersection < fDistance2) 
				{ 
					bFailed = true;
				}
			}

			if (bFailed) { continue; }

			// Finally, this can be considered successful. Store the values.
			fBestMaxFlatDistanceSquared = fMaxFlatDistanceSquared;
			fBestMinHeightChange = fMinHeightChange;
			fBestHeadroom = fHeadroom;
			iBestVariation = i;
		}
	}

	if (iBestVariation >= 0)
	{
		return true;
	}

	// since the nominal test failed do a test ignoring distance limit and choose the variation with the longest jump possible
	fBestMaxFlatDistanceSquared =0.0f;
	for (int i=0; i<pxBlueprint->GetNumParameters(); ++i)
	{
		if (pxBlueprint->GetParameterValueByIndex(i, xValue))
		{
			const Asura_Blueprint* pxVariationBlueprint = Asura_Blueprint_System::GetBlueprintByHashID(uAVP_BLUEPRINT_TYPE_MOVESPREDATOR, xValue.GetHashIDValue());
			if (!pxVariationBlueprint) { continue; }

			float fMaxFlatDistanceSquared;
			float fMinHeightChange;
			float fHeadroom;

			if (!pxVariationBlueprint->GetParameterValueByHashID(uAVP_BLUEPRINT_PARAM_MAXDISTANCE, xValue)) { continue; }
			fMaxFlatDistanceSquared = xValue.GetFloatValue();
			fMaxFlatDistanceSquared *= fMaxFlatDistanceSquared;

			if (!pxVariationBlueprint->GetParameterValueByHashID(uAVP_BLUEPRINT_PARAM_HEIGHTCHANGE, xValue)) { continue; }
			fMinHeightChange = xValue.GetFloatValue();

			if (!pxVariationBlueprint->GetParameterValueByHashID(uAVP_BLUEPRINT_PARAM_HEADROOM, xValue)) { continue; }
			fHeadroom = xValue.GetFloatValue();

			// First, make sure its height change is appropriate. 0 height change is always a valid option.
			if ((fMinHeightChange != 0.0f) && (fHeightOffsetSign != Asura_Maths::Sign(fMinHeightChange))) { continue; }
			fMinHeightChange = Asura_Maths::Abs(fMinHeightChange);
			if (fMinHeightChange < fBestMinHeightChange) { continue; }
			if (fMinHeightChange > fHeightOffset) { continue; }

			// Ignore distance check
			// Now pick the largest distance distance 
			// Only need to compare with the best if our height changes are equal since that's the priority comparison.
			if ((fMinHeightChange == fBestMinHeightChange) && (fMaxFlatDistanceSquared < fBestMaxFlatDistanceSquared)) { continue; }

			if ((fMinHeightChange == fBestMinHeightChange) && (fMaxFlatDistanceSquared == fBestMaxFlatDistanceSquared) && (fHeadroom < fBestHeadroom)) { continue; }
			// We need two LOS tests to make a reasonable guess - from the current head position to an approximation of head position at the midpoint, and from there to the end head position.
			bool bFailed = false;
			Asura_Vector_3 xMidPoint = (xFromPosition + xToPosition) * 0.5f + Asura_Vector_3(0.0f, -fHeadroom, 0.0f);
			Asura_Vector_3 xDir1 = xMidPoint - xFromPosition;
			Asura_Vector_3 xDir2 = xToPosition - xMidPoint;
			float fDistance1 = xDir1.Normalise();
			float fDistance2 = xDir2.Normalise();

			for (int j =0; j < 2 && !bFailed; ++j)
			{
				const Asura_Vector_3 xMidpoint = (xStartPosition[j] + xEndPosition[j]) * 0.5f + Asura_Vector_3(0.0f, -fHeadroom, 0.0f);

				Asura_LOS_InputData xInput(xStartPosition[j], xDir1, ASURA_GUID_UNREGISTERED, fDistance1, false, AVP_PolygonFlag_IgnoreForPredatorCollision | AsuraPolygonFlag_NoCollisions);
				const Asura_LOS_OutputData* pxOutput = Asura_LOS::TestAgainstEverything(&xInput);
				Tools_MessageLog::AddMessage(Tools_MessageLog::WAYPOINTING, "Generate Predator Jump links",
					"LOS from (%f, %f, %f) to (%f, %f, %f) expected intersection distance: %f - actual intersection distance: %f, hit entity: %d, env index: %d",
					xFromPosition.x, xFromPosition.y, xFromPosition.z, 
					xMidPoint.x, xMidPoint.y, xMidPoint.z,
					fDistance1, pxOutput->m_fDistanceToIntersection,
					pxOutput->m_uHitEntity, pxOutput->m_uEnvironmentFaceIndex);
				if (pxOutput->m_fDistanceToIntersection < fDistance1) 
				{ 
					bFailed = true; 
					continue; 
				}

				xInput.m_xOrigin = xMidpoint;
				xInput.m_xDirection = xDir2;
				xInput.m_fInitialMaximumRange = fDistance2;
				pxOutput = Asura_LOS::TestAgainstEverything(&xInput);
				Tools_MessageLog::AddMessage(Tools_MessageLog::WAYPOINTING, "Generate Predator Jump links",
					"LOS from (%f, %f, %f) to (%f, %f, %f) expected intersection distance: %f - actual intersection distance: %f, hit entity: %d, env index: %d", 
					xMidPoint.x, xMidPoint.y, xMidPoint.z,
					xToPosition.x, xToPosition.y, xToPosition.z, 
					fDistance2, pxOutput->m_fDistanceToIntersection, 
					pxOutput->m_uHitEntity, pxOutput->m_uEnvironmentFaceIndex);

				if (pxOutput->m_fDistanceToIntersection < fDistance2) 
				{ 
					bFailed = true;
				}
			}

			if (bFailed) { continue; }

			// Finally, this can be considered successful. Store the values.
			fBestMaxFlatDistanceSquared = fMaxFlatDistanceSquared;
			fBestMinHeightChange = fMinHeightChange;
			fBestHeadroom = fHeadroom;
			iBestVariation = i;
		}
	}

	if (iBestVariation >= 0)
	{
		return true;
	}
	else
	{
		return false;
	}
}

bool ToolsAVP_Mode_Waypointing::FlagTransitionLinks()
{
    // If these are Alien waypoints and (they change surface type or (they are not floor and the normals are far enough apart)).
    const float fCosTransitionLinkAngleMin = Asura_Maths::Cosine( Asura_Maths::DegToRad( s_fTransitionLinkMinAngle ) );

    for ( int i = 0; i < s_iNumWaypoints; ++i )
    {
		Tools_Nav_Vertex& xSourceWP = s_pxWaypointStorage[ i ];
        if ( !( xSourceWP.m_iFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN ) )
        {
            continue;
        }

        for ( int iLinkIndex = 0; iLinkIndex < s_pxWaypointStorage[ i ].GetNumLinks(); ++iLinkIndex )
        {
            const int iTargetWaypointIndex = xSourceWP.m_xLinks[ iLinkIndex ].m_iIndex;
			const Tools_Nav_Vertex& xTargetWP = s_pxWaypointStorage[ iTargetWaypointIndex ];

            if ( !( xTargetWP.m_iFlags & AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN ) )
            {
                continue;
            }

            if ( ( xSourceWP.IsCeiling() && !xTargetWP.IsCeiling() ) ||
                ( xSourceWP.IsWall() && !xTargetWP.IsWall() ) ||
                ( xSourceWP.IsFloor() && !xTargetWP.IsFloor() ) )
            {
                xSourceWP.m_xLinks[ iLinkIndex ].m_uFlags |= TOOLSAVP_CONNECTION_FLAG_TRANSITION;
                continue;
            }

			if ( xSourceWP.IsFloor() && xTargetWP.IsFloor() )
			{
				continue;
			}

            const float fDot = xSourceWP.m_xNormal * xTargetWP.m_xNormal;
            if ( fDot <= fCosTransitionLinkAngleMin )
            {
                xSourceWP.m_xLinks[ iLinkIndex ].m_uFlags |= TOOLSAVP_CONNECTION_FLAG_TRANSITION;
            }
        }
    }

    return true;
}

bool ToolsAVP_Mode_Waypointing::CullOverlappingNonTransitionLinks()
{
    for ( int i = 0; i < s_iNumWaypoints; ++i )
    {
        // 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 )
        {
            // Skip transition links.
            if ( ( s_pxWaypointStorage[ i ].m_xLinks[ iLinkIndex ].m_uFlags & TOOLSAVP_CONNECTION_FLAG_TRANSITION ) )
            {
                continue;
            }

// 			if ( (s_pxWaypointStorage[ i ].m_iFlags & 
// 				(AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ENV_HOOK | 
// 				AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_GS2 | 
// 				AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_PREDATOR_JUMP | 
// 				AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN_JUMP)) != 0 )
// 			{
// 				continue;
// 			}
//			int iToWPIndex = s_pxWaypointStorage[ i ].m_xLinks[ iLinkIndex ].m_iIndex;
// 			if ( (s_pxWaypointStorage[ iToWPIndex ].m_iFlags & 
// 				(AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ENV_HOOK | 
// 				AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_GS2 | 
// 				AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_PREDATOR_JUMP | 
// 				AVP_Navigation::AVP_AI_NAVVERTEX_FLAG_ALIEN_JUMP)) != 0 )
// 			{
// 				continue;
// 			}
// 

			if (s_pxWaypointStorage[ i ].m_xLinks[ iLinkIndex ].m_uFlags & ( AVP_Navigation::AVP_AI_CONNECTION_FLAG_GS2 | AVP_Navigation::AVP_AI_CONNECTION_FLAG_ENV_HOOK) )
			{
				continue;
			}

            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 ToolsAVP_Mode_Waypointing::CullTransitionLinksUsingDistanceFromPlanes()
{
    return true;

    //static float s_fMinDistance = 1.0f;
    //static float s_fMaxDistance = 7.5f;

    //for ( int i = 0; i < s_iNumWaypoints; ++i )
    //{
    //    Tools_Nav_Vertex& xBaseWaypoint = s_pxWaypointStorage[ i ];
    //    for ( int iLinkIndex = s_pxWaypointStorage[ i ].m_iNumLinks - 1; iLinkIndex >= 0; --iLinkIndex )
    //    {
    //        const Tools_Nav_Link& xBaseLink = xBaseWaypoint.m_pxLinks[ iLinkIndex ];
    //        if ( !( xBaseLink.m_uFlags & TOOLSAVP_CONNECTION_FLAG_TRANSITION ) )
    //        {
    //            // Not a potential transition link.
    //            continue;
    //        }

    //        const Tools_Nav_Vertex& xTargetWaypoint = s_pxWaypointStorage[ xBaseLink.m_iIndex ];

    //        // Make a plane up from the base waypoint position and normal and see how far the target waypoint is away.
    //        // This target-plane distance must be between certain limits.
    //        AVP_Plane xBasePlane( Asura_AVector3( xBaseWaypoint.m_xPosition ), Asura_AVector3( xBaseWaypoint.m_xNormal ) );
    //        const float fTargetDistance = xBasePlane.CalculateClosestDistance( xTargetWaypoint.m_xPosition );

    //        // Make a plane up from the target waypoint position and normal and see how far the base waypoint is away.
    //        // This base-plane distance must be between certain limits.
    //        AVP_Plane xTargetPlane( Asura_AVector3( xTargetWaypoint.m_xPosition ), Asura_AVector3( xTargetWaypoint.m_xNormal ) );
    //        const float fBaseDistance = xTargetPlane.CalculateClosestDistance( xBaseWaypoint.m_xPosition );

    //        if ( s_fMinDistance <= fBaseDistance && fBaseDistance <= s_fMaxDistance &&
    //            s_fMinDistance <= fTargetDistance && fTargetDistance <= s_fMaxDistance )
    //        {
    //            continue;
    //        }

    //        // Cull this link.
    //        s_pxWaypointStorage[ i ].m_pxLinks[ iLinkIndex ] = s_pxWaypointStorage[ i ].m_pxLinks[ --s_pxWaypointStorage[ i ].m_iNumLinks ];
    //    }
    //}

    //return true;
}

bool ToolsAVP_Mode_Waypointing::CullLessDirectTransitionLinks()
{
    int*   piLinkIndices        = new int[ s_iNumWaypoints ];
    int    iLinkCount           = 0;
    float* pfLinkAngleScores    = new float[ s_iNumWaypoints ];
    float* pfLinkDistanceScores = new float[ s_iNumWaypoints ];

    if ( !piLinkIndices || !pfLinkAngleScores || !pfLinkDistanceScores )
    {
        delete [] piLinkIndices;
        delete [] pfLinkAngleScores;
        delete [] pfLinkDistanceScores;
        return false;
    }

    for ( int i = 0; i < s_iNumWaypoints; ++i )
    {
        Tools_Nav_Vertex& xBaseWaypoint = s_pxWaypointStorage[ i ];
        for ( int iBaseLinkIndex = 0; iBaseLinkIndex < xBaseWaypoint.GetNumLinks(); ++iBaseLinkIndex )
        {
            const Tools_Nav_Link& xBaseLink = xBaseWaypoint.m_xLinks[ iBaseLinkIndex ];
            if ( !( xBaseLink.m_uFlags & TOOLSAVP_CONNECTION_FLAG_TRANSITION ) )
            {
                // Not a potential transition link.
                continue;
            }

            const Tools_Nav_Vertex& xSurfaceWaypoint  = s_pxWaypointStorage[ xBaseLink.m_iIndex ];
            // Determine if we are looking at concave or convex transitions onto this surface.
            const bool              bSurfaceIsConcave = ( ( xSurfaceWaypoint.m_xPosition - xBaseWaypoint.m_xPosition ) * xSurfaceWaypoint.m_xNormal ) < 0.0f;

            EnumerateLinksOntoThisSurface( xBaseWaypoint, xSurfaceWaypoint, bSurfaceIsConcave, iLinkCount, piLinkIndices );

            // Score all links and work out which scores are acceptable.
            CalculateLinkAngleScores( xBaseWaypoint, xSurfaceWaypoint, bSurfaceIsConcave, iLinkCount, piLinkIndices, pfLinkAngleScores );
            CalculateLinkDistanceScores( xBaseWaypoint, iLinkCount, piLinkIndices, pfLinkDistanceScores );
            CalculateAcceptableScores( iLinkCount, pfLinkAngleScores, pfLinkDistanceScores );

            // Delete all links in the list we generated that are not acceptable, working backwards.
            // Also delete any link coming the other way.
            for ( int iSurfaceLinkIndex = iLinkCount - 1; iSurfaceLinkIndex >= 0; --iSurfaceLinkIndex )
            {
                if ( pfLinkDistanceScores[ iSurfaceLinkIndex ] > 0.0f && pfLinkAngleScores[ iSurfaceLinkIndex ] > 0.0f )
                {
                    continue;
                }

                const int iLinkIndexToDelete = piLinkIndices[ iSurfaceLinkIndex ];

                // Delete the link coming the other way if it exists.
                const int iLinkTargetIndex = xBaseWaypoint.m_xLinks[ iLinkIndexToDelete ].m_iIndex;
                Tools_Nav_Vertex& xTargetWaypoint = s_pxWaypointStorage[ iLinkTargetIndex ];
                for ( int j = 0; j < xTargetWaypoint.GetNumLinks(); ++j )
                {
                    if ( xTargetWaypoint.m_xLinks[ j ].m_iIndex == i )
                    {
						xTargetWaypoint.m_xLinks.Remove( j, 1 );
                        break;
                    }
                }

				xBaseWaypoint.m_xLinks.Remove( iLinkIndexToDelete, 1 );
            }
        }
    }

    delete [] piLinkIndices;
    delete [] pfLinkAngleScores;
    delete [] pfLinkDistanceScores;
    return true;
}

void ToolsAVP_Mode_Waypointing::EnumerateLinksOntoThisSurface( const Tools_Nav_Vertex& xBaseWaypoint, const Tools_Nav_Vertex& xSurfaceWaypoint,
                                                               const bool bSurfaceIsConcave,
                                                               int& iLinkCount, int* piLinkIndices )
{
    const float fCosSurfaceAngleTolerance = Asura_Maths::Cosine( Asura_Maths::DegToRad( s_fSurfaceAngleTolerance ) );

    // Count and record the number of links that we consider go to the same surface
    // (whose target waypoint normal is close enough to our reference waypoint normal).
    iLinkCount = 0;
    for ( int iTestLinkIndex = 0; iTestLinkIndex < xBaseWaypoint.GetNumLinks(); ++iTestLinkIndex )
    {
        const Tools_Nav_Link& xTestLink = xBaseWaypoint.m_xLinks[ iTestLinkIndex ];
        if ( !( xBaseWaypoint.m_xLinks[ iTestLinkIndex ].m_uFlags & TOOLSAVP_CONNECTION_FLAG_TRANSITION ) )
        {
            // Not a potential transition link.
            continue;
        }

		// ignore env and gs2 links
		if ( xBaseWaypoint.m_xLinks[ iTestLinkIndex ].m_uFlags & AVP_Navigation::AVP_AI_CONNECTION_FLAG_ENV_HOOK )
		{
			continue;
		}

		if ( xBaseWaypoint.m_xLinks[ iTestLinkIndex ].m_uFlags & AVP_Navigation::AVP_AI_CONNECTION_FLAG_GS2 )
		{
			continue;
		}

        const Tools_Nav_Vertex& xTestWaypoint = s_pxWaypointStorage[ xTestLink.m_iIndex ];
        if ( xSurfaceWaypoint.m_xNormal * xTestWaypoint.m_xNormal < fCosSurfaceAngleTolerance )
        {
            // Normal is too far away => not on the same surface.
            continue;
        }

        const bool bTestIsConcave = ( ( xTestWaypoint.m_xPosition - xBaseWaypoint.m_xPosition ) * xTestWaypoint.m_xNormal ) < 0.0f;
        if ( bTestIsConcave != bSurfaceIsConcave )
        {
            // This must be a transition to a different surface.
            continue;
        }

        piLinkIndices[ iLinkCount++ ] = iTestLinkIndex;
    }
}

void ToolsAVP_Mode_Waypointing::CalculateLinkAngleScores( const Tools_Nav_Vertex& xBaseWaypoint, const Tools_Nav_Vertex& xSurfaceWaypoint,
                                                          const bool bSurfaceIsConcave,
                                                          const int iLinkCount, const int* const piLinkIndices,
                                                          float* pfLinkAngleScores )
{
    const float fCosTransitionLinkMaxAngleToPerfect = Asura_Maths::Cosine( Asura_Maths::DegToRad( s_fTransitionLinkMaxAngleToPerfect ) );

    // Work out the link angle scores by projecting the base waypoint normal onto the surface plane, constructing an orientation
    // matrix from this and our base normal and what the X offset is in the new space.
    AVP_Plane xSurfacePlane( Asura_AVector3( xSurfaceWaypoint.m_xPosition ), Asura_AVector3( xSurfaceWaypoint.m_xNormal ) );

    Asura_Vector_3 xBaseDash;
    xSurfacePlane.CalculateClosestPoint( xBaseWaypoint.m_xPosition, xBaseDash );

    Asura_Vector_3 xForward = xBaseDash - xBaseWaypoint.m_xPosition;
    Asura_Vector_3 xUp = bSurfaceIsConcave ? xBaseWaypoint.m_xNormal : -xBaseWaypoint.m_xNormal;
    Asura_Vector_3 xSide = xUp ^ xForward;
    xForward = xSide ^ xUp;
    xSide.Normalise();
    xForward.Normalise();
    const Asura_Matrix_3x3 xSpaceMatrix( xSide[0], xUp[0], xForward[0],
        xSide[1], xUp[1], xForward[1],
        xSide[2], xUp[2], xForward[2] );
    const Asura_Vector_3 xLocalSpaceUp = xSpaceMatrix.ApplyTranspose( xUp );

    for ( int iSurfaceLinkIndex = 0; iSurfaceLinkIndex < iLinkCount; ++iSurfaceLinkIndex )
    {
        const int               iSurfaceLinkWPIndex  = xBaseWaypoint.m_xLinks[ piLinkIndices[ iSurfaceLinkIndex ] ].m_iIndex;
        const Tools_Nav_Vertex& xSurfaceLinkWaypoint = s_pxWaypointStorage[ iSurfaceLinkWPIndex ];
        Asura_Vector_3 xToSurfaceLinkWP = xSurfaceLinkWaypoint.m_xPosition - xBaseWaypoint.m_xPosition;
        xToSurfaceLinkWP = xSpaceMatrix.ApplyTranspose( xToSurfaceLinkWP );

        // If this is within s_fTransitionLinkMaxAngleToPerfect then score it.
        xToSurfaceLinkWP.z = 0.0f;
        xToSurfaceLinkWP.Normalise();
        float fDot = xToSurfaceLinkWP * xLocalSpaceUp;
        if ( fDot > fCosTransitionLinkMaxAngleToPerfect )
        {
            pfLinkAngleScores[ iSurfaceLinkIndex ] = ( fDot - fCosTransitionLinkMaxAngleToPerfect ) * ( 1.0f / ( 1.0f - fCosTransitionLinkMaxAngleToPerfect ) );
        }
        else
        {
            pfLinkAngleScores[ iSurfaceLinkIndex ] = 0.0f;
        }
    }
}

void ToolsAVP_Mode_Waypointing::CalculateLinkDistanceScores( const Tools_Nav_Vertex& xBaseWaypoint,
                                                             const int iLinkCount, const int* const piLinkIndices,
                                                             float* pfLinkDistanceScores )
{
#ifdef SCORE_DISTANCE_LINEARLY
    static float s_fDistanceScalar = 1.0f / ( s_fMaxTransitionLinkDistance - s_fMinTransitionLinkDistance );

    // The less the distance the higher the score.
    AVP_Plane xBasePlane( Asura_AVector3( xBaseWaypoint.m_xPosition ), Asura_AVector3( xBaseWaypoint.m_xNormal ) );

    for ( int iSurfaceLinkIndex = 0; iSurfaceLinkIndex < iLinkCount; ++iSurfaceLinkIndex )
    {
        const int               iSurfaceLinkWPIndex  = xBaseWaypoint.m_xLinks[ piLinkIndices[ iSurfaceLinkIndex ] ].m_iIndex;
        const Tools_Nav_Vertex& xSurfaceLinkWaypoint = s_pxWaypointStorage[ iSurfaceLinkWPIndex ];

        // Convex corners will cause these distances to come out negative.
        const float fTargetDistance = Asura_Maths::Abs( xBasePlane.CalculateClosestDistance( xSurfaceLinkWaypoint.m_xPosition ) );

        AVP_Plane xTargetPlane( Asura_AVector3( xSurfaceLinkWaypoint.m_xPosition ), Asura_AVector3( xSurfaceLinkWaypoint.m_xNormal ) );
        const float fBaseDistance = Asura_Maths::Abs( xTargetPlane.CalculateClosestDistance( xBaseWaypoint.m_xPosition ) );

        if ( s_fMinTransitionLinkDistance <= fTargetDistance && fTargetDistance <= s_fMaxTransitionLinkDistance &&
            s_fMinTransitionLinkDistance <= fBaseDistance && fBaseDistance <= s_fMaxTransitionLinkDistance )
        {
            pfLinkDistanceScores[ iSurfaceLinkIndex ] = 1.0f - ( ( fBaseDistance - s_fMinTransitionLinkDistance ) * s_fDistanceScalar );
            pfLinkDistanceScores[ iSurfaceLinkIndex ] += 1.0f - ( ( fTargetDistance - s_fMinTransitionLinkDistance ) * s_fDistanceScalar );
            pfLinkDistanceScores[ iSurfaceLinkIndex ] *= 0.5f;
        }
        else
        {
            // If any of the distances are out of range then this link doesn't score.
            pfLinkDistanceScores[ iSurfaceLinkIndex ] = 0.0f;
        }
    }
#else
    // Code to calculate score based on ideal distance.

    static float s_fMinDistanceScalar = 1.0f / ( s_fIdealTransitionLinkDistance - s_fMinTransitionLinkDistance );
    static float s_fMaxDistanceScalar = 1.0f / ( s_fMaxTransitionLinkDistance - s_fIdealTransitionLinkDistance );

    AVP_Plane xBasePlane( Asura_AVector3( xBaseWaypoint.m_xPosition ), Asura_AVector3( xBaseWaypoint.m_xNormal ) );

    for ( int iSurfaceLinkIndex = 0; iSurfaceLinkIndex < iLinkCount; ++iSurfaceLinkIndex )
    {
        const int               iSurfaceLinkWPIndex  = xBaseWaypoint.m_xLinks[ piLinkIndices[ iSurfaceLinkIndex ] ].m_iIndex;
        const Tools_Nav_Vertex& xSurfaceLinkWaypoint = s_pxWaypointStorage[ iSurfaceLinkWPIndex ];

        // Convex corners will cause these distances to come out negative.
        const float fTargetDistance = Asura_Maths::Abs( xBasePlane.CalculateClosestDistance( xSurfaceLinkWaypoint.m_xPosition ) );

        AVP_Plane xTargetPlane( Asura_AVector3( xSurfaceLinkWaypoint.m_xPosition ), Asura_AVector3( xSurfaceLinkWaypoint.m_xNormal ) );
        const float fBaseDistance = Asura_Maths::Abs( xTargetPlane.CalculateClosestDistance( xBaseWaypoint.m_xPosition ) );

        if ( s_fMinTransitionLinkDistance <= fTargetDistance && fTargetDistance <= s_fMaxTransitionLinkDistance &&
            s_fMinTransitionLinkDistance <= fBaseDistance && fBaseDistance <= s_fMaxTransitionLinkDistance )
        {
            pfLinkDistanceScores[ iSurfaceLinkIndex ] = 0.0f;
            if ( fBaseDistance < s_fIdealTransitionLinkDistance )
            {
                pfLinkDistanceScores[ iSurfaceLinkIndex ] += 1.0f - ( ( s_fIdealTransitionLinkDistance - fBaseDistance ) * s_fMinDistanceScalar );
            }
            else
            {
                pfLinkDistanceScores[ iSurfaceLinkIndex ] += ( fBaseDistance - s_fIdealTransitionLinkDistance ) * s_fMaxDistanceScalar;
            }
            if ( fTargetDistance < s_fIdealTransitionLinkDistance )
            {
                pfLinkDistanceScores[ iSurfaceLinkIndex ] += 1.0f - ( ( s_fIdealTransitionLinkDistance - fTargetDistance ) * s_fMinDistanceScalar );
            }
            else
            {
                pfLinkDistanceScores[ iSurfaceLinkIndex ] += ( fTargetDistance - s_fIdealTransitionLinkDistance ) * s_fMaxDistanceScalar;
            }
            pfLinkDistanceScores[ iSurfaceLinkIndex ] *= 0.5f;
        }
        else
        {
            // If any of the distances are out of range then this link doesn't score.
            pfLinkDistanceScores[ iSurfaceLinkIndex ] = 0.0f;
        }
    }
#endif
}

void ToolsAVP_Mode_Waypointing::CalculateAcceptableScores( const int iLinkCount, float* pfLinkAngleScores, float* pfLinkDistanceScores )
{
    for ( int iSurfaceLinkIndex = 0; iSurfaceLinkIndex < iLinkCount; ++iSurfaceLinkIndex )
    {
        //const float fTotalScore = ( 1.0f - s_fDistanceAngleScoreWeighting ) * pfLinkDistanceScores[ iSurfaceLinkIndex ] +
        //    s_fDistanceAngleScoreWeighting * pfLinkAngleScores[ iSurfaceLinkIndex ];
        //if ( fTotalScore >= s_fTransitionLinkMinAcceptableScore )
        if ( pfLinkAngleScores[ iSurfaceLinkIndex ] > 0.0f && pfLinkDistanceScores[ iSurfaceLinkIndex ] > 0.0f )
        {
            pfLinkDistanceScores[ iSurfaceLinkIndex ] = pfLinkAngleScores[ iSurfaceLinkIndex ] = 1.0f;
        }
        else
        {
            pfLinkDistanceScores[ iSurfaceLinkIndex ] = pfLinkAngleScores[ iSurfaceLinkIndex ] = 0.0f;
        }
    }
}

bool ToolsAVP_Mode_Waypointing::FinalFlagTransitionLinks()
{
	// Any surviving links with the 'transition' flag are transition links.
	for ( int i = 0; i < s_iNumWaypoints; ++i )
	{   
		for ( int iLinkIndex = 0; iLinkIndex < s_pxWaypointStorage[ i ].GetNumLinks(); ++iLinkIndex )
		{
			if ( s_pxWaypointStorage[ i ].m_xLinks[ iLinkIndex ].m_uFlags & TOOLSAVP_CONNECTION_FLAG_TRANSITION )
			{
				const int iTargetWaypointIndex = s_pxWaypointStorage[ i ].m_xLinks[ iLinkIndex ].m_iIndex;

				if ( s_pxWaypointStorage[ iTargetWaypointIndex ].IsCeiling() )
				{
					s_pxWaypointStorage[ i ].m_xLinks[ iLinkIndex ].m_uFlags |= AVP_Navigation::AVP_AI_CONNECTION_FLAG_TRANSITION_HIGH_MOBILITY;
				}
				else if ( s_pxWaypointStorage[ iTargetWaypointIndex ].IsWall() )
				{
					s_pxWaypointStorage[ i ].m_xLinks[ iLinkIndex ].m_uFlags |= AVP_Navigation::AVP_AI_CONNECTION_FLAG_TRANSITION_MED_MOBILITY;
				}
				else
				{
					s_pxWaypointStorage[ i ].m_xLinks[ iLinkIndex ].m_uFlags |= AVP_Navigation::AVP_AI_CONNECTION_FLAG_TRANSITION_LOW_MOBILITY;
				}
			}
		}
	}

	return true;
}

bool ToolsAVP_Mode_Waypointing::UnflagTransitionLinks()
{
    for ( int i = 0; i < s_iNumWaypoints; ++i )
    {   
        for ( int iLinkIndex = 0; iLinkIndex < s_pxWaypointStorage[ i ].GetNumLinks(); ++iLinkIndex )
        {
            s_pxWaypointStorage[ i ].m_xLinks[ iLinkIndex ].m_uFlags &= ~TOOLSAVP_CONNECTION_FLAG_TRANSITION;
        }
    }

    return true;
}

void ToolsAVP_Mode_Waypointing::AVPAABBTreePerBinSizeTestCallback( 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, "ToolsAVP_Mode_Waypointing::AVPAABBTreePerBinSizeTestCallback - 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;
    }
	if ( ( xTestWaypoint.IsFloor() && !xCallbackWaypoint.IsFloor() ) ||
		( xTestWaypoint.IsWall() && !xCallbackWaypoint.IsWall() ) ||
		( xTestWaypoint.IsCeiling() && !xCallbackWaypoint.IsCeiling() ) )
	{
		// If the waypoints are on different surfaces they shouldn't cull each other.
		return;
	}
    const float fDot = xTestWaypoint.m_xNormal * xCallbackWaypoint.m_xNormal;
    if ( fDot < s_fMaxDotProductBetweenWaypointPolygonNormals )
    {
        // If the dot between the culling waypoint and this waypoint's normals is outside the s_fMaxDotProductBetweenWaypointPolygonNormals limit then don't cull.
        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;
    }
}

////////////////////////////////////////////////////////////////////////////////
// Function     : Tools_Mode_Waypointing::MoveToDestination_AVP
// Author       : Andy Tate
// Date         : 4-Jun-2007
////////////////////////////////////////////////////////////////////////////////

bool ToolsAVP_Mode_Waypointing::AVPMoveToDestination( const Asura_Vector_3& xStart, const Asura_Vector_3& xFinish,
                                                      const Asura_Vector_3& xDestinationNormal,
                                                      const Asura_Vector_3& xAllowedOffsetVector,
                                                      const bool bIsAlienRoute, const bool bIsTransitionLink, const bool bIsConcave,
                                                      Asura_Physics_Object* pxPhysicsObject, float& fMaxFallDistance )
{
	const float					fTimeStep		= Asura_Physics_System::GetTimeStep();
    const bool					bFeelsGravity	= pxPhysicsObject->FeelsGravity();
    const Asura_Matrix_3x3&		xOrientation	= pxPhysicsObject->GetOrientation();
    Asura_Quat			        xCurrentOrientation( xOrientation );
    Asura_Vector_3		        xNormal( -xOrientation.e12, -xOrientation.e22, -xOrientation.e32 );
    Asura_Bounding_Box          xDestinationBB;

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

    // Generate a bounding-box around the destination position...
    {
        Asura_Matrix_3x3 xOrientationMatrix;
        Asura_Maths::OrientationFromNormal( xOrientationMatrix, xDestinationNormal );
        const Asura_Vector_3 xReverseOffset( -xAllowedOffsetVector.x, 0.1f, -xAllowedOffsetVector.z );

        Asura_Bounding_Box xTempBB;
        xTempBB.Zero();
        xTempBB.Set( xReverseOffset );
        xTempBB.Set( xAllowedOffsetVector );
        xDestinationBB.FromOBB( xTempBB, xOrientationMatrix );
        xDestinationBB.Displace( xFinish );
    }

    fMaxFallDistance = 0.0f;
    pxPhysicsObject->TeleportTo( xStart );
    pxPhysicsObject->SetLinVelocity( Asura_Maths::ZeroVector3 );
	pxPhysicsObject->SetActorJumpVelocity( Asura_Maths::ZeroVector3 );

    s_uNumPhysicsPositions = 0;
    s_pxPhysicsVelocities[ s_uNumPhysicsPositions ]   = Asura_Maths::ZeroVector3;
    s_pxPhysicsPositions[ s_uNumPhysicsPositions ]    = pxPhysicsObject->GetPosition();
    s_pxPhysicsOrientations[ s_uNumPhysicsPositions ] = xCurrentOrientation;
    ++s_uNumPhysicsPositions;

    AVP_PhysicsObjectHandler xPhysicsObjectHandler( pxPhysicsObject );
    if ( bIsAlienRoute )
    {
        xPhysicsObjectHandler.EnableSurfaceReorientation( Asura_Maths::ArcCosine( s_fMaxDotProductForLinkBetweenWaypointNormals ) );
    }
    else
    {
        xPhysicsObjectHandler.DisableSurfaceReorientation();
        xPhysicsObjectHandler.ResetUsingWorldSpaceOrientation( xOrientation );
    }
	xPhysicsObjectHandler.GetPhysicsObject()->SetReportObjectCollisions( true );

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

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

        // Calculate the angular velocity required to get us aligned with our velocity.
        Asura_Angle fAngle;
        CalculateObjectSpaceAngVelocity( xPhysicsObjectHandler.GetPhysicsObject()->GetOrientation(), xVelocity, fAngle );
        static float s_fAngleFactor = 5.0f;
        fAngle = Asura_Maths::ClampToRange( fAngle * s_fAngleFactor, -Asura_Maths::Deg360, Asura_Maths::Deg360 );

        float fSpeed = bIsTransitionLink ? ( bIsConcave ? 4.0f : 1.0f ) : 10.0f;
        if ( s_bFallDirectlyDownwards && !bIsAlienRoute && !pxPhysicsObject->IsInContactWithFloor() )
        {
            // 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.
            fSpeed = 0.5f;
        }

        // Now update our movement and (re)orientation.
        const Asura_AVector3 xObjectSpaceVelocity( 0.0f, 0.0f, fSpeed );
        static const Asura_AVector3 xObjectSpaceAxis( 0.0f, 1.0f, 0.0f );
        xPhysicsObjectHandler.CalculateMovementUsingAngAndLinVelocity( fTimeStep,
            xObjectSpaceAxis, fAngle, xObjectSpaceVelocity, bFeelsGravity, AVP_PhysicsObjectHandler::OBJECT_SPACE );
        Asura_Physics_System::UpdateForWaypointing();

        xPosition = xPhysicsObjectHandler.GetPhysicsObject()->GetPosition();
        xPhysicsObjectHandler.GetCurrentSurfaceNormal(xNormal);
        xCurrentOrientation.FromMatrix( xPhysicsObjectHandler.GetPhysicsObject()->GetOrientation() );

        // 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 ( s_uNumPhysicsPositions < MAX_STORED_PHYSICS_POSITIONS )
        {
            s_pxPhysicsVelocities[ s_uNumPhysicsPositions ]   = xVelocity;
            s_pxPhysicsPositions[ s_uNumPhysicsPositions ]    = xPosition;
            s_pxPhysicsOrientations[ s_uNumPhysicsPositions ] = xCurrentOrientation;
            ++s_uNumPhysicsPositions;
        }

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

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

                // This is an Alien route. If we hit a poly that is at a greater angle than we can orientate to then cull the link,
                // unless it is flagged as 'ignore for reorientation'. (This is to allow us to ignore little steps.)
                const bool bIsIgnoreForReorientation = ( ( uPolyFlags & AVP_PolygonFlag_IgnoreForReorientation ) != 0 );
                if ( !bIsIgnoreForReorientation )
                {
                    // We need the poly normal not the face normal.
                    Asura_AVector3 xPolyNormal;
                    GetPolyNormal( pxEnvCollisionReport->GetFaceID(), xPolyNormal );

                    const float fDot = xPolyNormal * -pxPhysicsObject->GetGravityDirection();
                    if ( fDot < s_fMaxDotProductForLinkBetweenWaypointNormals )
                    {
                        return false;
                    }
                }

                pxEnvCollisionReport = pxEnvCollisionReport->GetNextReport();
            }
        }
        else
        {
            const Asura_Physics_EnvironmentCollisionReport* pxEnvCollisionReport = pxPhysicsObject->GetEnvironmentCollisionReportList();
            while ( pxEnvCollisionReport )
            {
                // If we hit a poly that is not marked as floor/steps or dense then cull the link.
                const u_int uPolyFlags = pxEnvCollisionReport->Tools_GetPolyFlags();
                if ( !( uPolyFlags & AsuraPolygonFlag_Floor || uPolyFlags & AsuraPolygonFlag_Step || uPolyFlags & AsuraPolygonFlag_DenseWaypoints ) )
                {
					//u_int uModuleIndex;
					//u_int uPolyIndex;
					//pxEnvCollisionReport->Tools_CalcModuleAndPolyIndex( uModuleIndex, uPolyIndex );
					//Asura_OutputToDebugger( ( "nok module: %u, poly: %u\n", uModuleIndex, uPolyIndex ) );
                    return false;
                }

                // This is a humanoid route. If we hit a wall or ceiling poly then cull the link,
                // unless it is flagged as 'physics walkable'. (This is to allow us to ignore little steps.)
                const bool bIsPhysicsWalkable = ( ( uPolyFlags & AsuraPolygonFlag_Physics_WalkableSurface ) != 0 );
                if ( !bIsPhysicsWalkable )
                {
                    // We need the poly normal not the face normal.
                    Asura_AVector3 xPolyNormalA;
                    GetPolyNormal( pxEnvCollisionReport->GetFaceID(), xPolyNormalA );
                    const Asura_Vector_3 xPolyNormal( xPolyNormalA );

                    ToolsAVP_Mode_Waypointing* pxToolsAVPWaypointing = static_cast< ToolsAVP_Mode_Waypointing* >( GetInstance() );
                    if ( pxToolsAVPWaypointing->IsWallPoly( xPolyNormal ) || pxToolsAVPWaypointing->IsCeilingPoly( xPolyNormal ) )
                    {
                        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;
}

bool ToolsAVP_Mode_Waypointing::SourceWaypointNormalIsValidForAttractor( const Asura_Attractor_Compiled& xAttractor,
                                                                         const float fAttractorAngle,
                                                                         const Tools_Nav_Vertex& xSourceWaypoint )
{
    // Tests on the source waypoint normal to ensure that it is valid for this attractor.
    Asura_Vector_3 xAttractorNormal;
    xAttractor.GetNormal( xAttractorNormal );
    Asura_Vector_3 xAttractorAxis;
    xAttractor.GetAxisDirection( xAttractorAxis );

    // Transform the waypoint position into attractor space to see what 'side' it's on.
    Asura_Matrix_3x3 xAttractorMatrix;
    CreateAttractorWorldMatrix( xAttractorNormal, xAttractorAxis, xAttractorMatrix );

    Asura_Vector_3 xClosestPointOnAttractorAxis;
    xAttractor.GetRelativePosition( xSourceWaypoint.m_xPosition, 0.0f, NULL, &xClosestPointOnAttractorAxis );
    Asura_Vector_3 xToWaypointPosition = xSourceWaypoint.m_xPosition - xClosestPointOnAttractorAxis;
    xToWaypointPosition = xAttractorMatrix.ApplyTranspose( xToWaypointPosition );
    const bool bRHS = ( xToWaypointPosition.x > 0.0f );

    // Rotate the attractor normal round the attractor axis by half the attractor angle. This is our ideal poly vector.
    // Cross this with the attractor axis again to find our ideal waypoint normal.
    Asura_Vector_3 xIdealWaypointNormal;
    Asura_Quat xRotationQuat;
    xRotationQuat.FromRotation( bRHS ? ( fAttractorAngle * 0.5f ) : ( -fAttractorAngle * 0.5f ), xAttractorAxis );
    xIdealWaypointNormal = xRotationQuat.RotateVector( xAttractorNormal);
    xIdealWaypointNormal = bRHS ? ( xIdealWaypointNormal ^ xAttractorAxis ) : ( xAttractorAxis ^ xIdealWaypointNormal );

    // The waypoint normal must be within a certain angle of the ideal.
    const float fDotAngleTolerance = Asura_Maths::Cosine( Asura_Maths::DegToRad( s_fAttractorAngleTolerance ) );
    const float fDot = xSourceWaypoint.m_xNormal * xIdealWaypointNormal;
    return ( fDot > fDotAngleTolerance );
}

bool ToolsAVP_Mode_Waypointing::TargetWaypointNormalIsValidForAttractor( const Asura_Attractor_Compiled& xAttractor,
                                                                         const float fAttractorAngle,
                                                                         const Tools_Nav_Vertex& xSourceWaypoint,
                                                                         const Tools_Nav_Vertex& xTargetWaypoint )
{
    // Run the same tests as for the source waypoint.
    if ( !SourceWaypointNormalIsValidForAttractor( xAttractor, fAttractorAngle, xTargetWaypoint ) )
    {
        return false;
    }

    // Now make sure that one waypoint is on one side of the attractor and one is on the other.
    Asura_Vector_3 xAttractorNormal;
    xAttractor.GetNormal( xAttractorNormal );
    Asura_Vector_3 xAttractorAxis;
    xAttractor.GetAxisDirection( xAttractorAxis );

    // Transform the waypoint positions into attractor space to see what 'side' they're on.
    Asura_Matrix_3x3 xAttractorMatrix;
    CreateAttractorWorldMatrix( xAttractorNormal, xAttractorAxis, xAttractorMatrix );

    Asura_Vector_3 xClosestPointOnAttractorAxis;
    Asura_Vector_3 xToWaypointPosition;

    xAttractor.GetRelativePosition( xSourceWaypoint.m_xPosition, 0.0f, NULL, &xClosestPointOnAttractorAxis );
    xToWaypointPosition = xSourceWaypoint.m_xPosition - xClosestPointOnAttractorAxis;
    xToWaypointPosition = xAttractorMatrix.ApplyTranspose( xToWaypointPosition );
    const bool bSourceIsRHS = ( xToWaypointPosition.x > 0.0f );

    xAttractor.GetRelativePosition( xTargetWaypoint.m_xPosition, 0.0f, NULL, &xClosestPointOnAttractorAxis );
    xToWaypointPosition = xTargetWaypoint.m_xPosition - xClosestPointOnAttractorAxis;
    xToWaypointPosition = xAttractorMatrix.ApplyTranspose( xToWaypointPosition );
    const bool bTargetIsRHS = ( xToWaypointPosition.x > 0.0f );

    return ( bSourceIsRHS != bTargetIsRHS );
}

bool ToolsAVP_Mode_Waypointing::FindAttractorAngle( const Asura_Attractor_Compiled& xAttractor, float& fAngle )
{
    // We have decided not to constrain the collision geometry to meet at pre-defined angles, therefore we cannot realistically
    // have the attractor at each joint requiring transition links be of a specific type (and therefore specify the angle involved).
    // We must calculate the angle of the attractor by finding a poly adjacent to it and using this poly's normal and the attractor normal.
    Asura_Vector_3 xAdjacentPolyVector;
    if ( !FindAdjacentPolyVector( xAttractor, xAdjacentPolyVector ) ) { return false; }

    Asura_Vector_3 xAttractorNormal;
    xAttractor.GetNormal( xAttractorNormal );

    fAngle = Asura_Maths::ArcCosine( xAdjacentPolyVector * xAttractorNormal );
    fAngle *= 2.0f;

    return true;
}

bool ToolsAVP_Mode_Waypointing::FindAdjacentPolyVector( const Asura_Attractor_Compiled& xAttractor, Asura_Vector_3& xPolyVector )
{
    // First find an adjacent poly. The poly will have two of its verts on the attractor axis. Then get this poly's normal and return it.
    for ( int iModuleIndex = 0; iModuleIndex < Asura_Module_System::GetNumberOfModules(); ++iModuleIndex )
    {
        Asura_Vector_3 xModulePosition;
        Asura_Module_System::GetPositionOfModule( iModuleIndex, xModulePosition );

        // Reject irrelevant modules.
        Asura_Bounding_Box xModuleBB = Asura_Module_System::GetBoundingBoxOfModule( iModuleIndex );
        xModuleBB.Displace( xModulePosition );
        if ( !xAttractor.CouldIntersect( xModuleBB ) ) { continue; }

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

        for ( int iPolyIndex = 0; iPolyIndex < pxMesh->NumberOfPolygons; ++iPolyIndex )
        {
            const u_int uPolyFlags = static_cast< u_int >( pxMesh->PolygonFlagDataPtr ? pxMesh->PolygonFlagDataPtr[ iPolyIndex ].Flags : 0 );
            if ( uPolyFlags & AsuraPolygonFlag_NoCollisions ) { continue; }

            int iNumAdjacentVerts = 0;
            int aiAdjacentVertIndex[ 2 ];

            // Get the distance from each vert to the attractor axis.
            const int iNumVerts = pxMesh->PolygonDataPtr[ iPolyIndex ].IsQuad() ? 4 : 3;
            for ( int iVertIndex = 0; iVertIndex < iNumVerts && iNumAdjacentVerts < 2; ++iVertIndex )
            {
                const Asura_Vector_3 xVertexWorldPosition = pxMesh->VertexDataPtr[ pxMesh->PolygonDataPtr[ iPolyIndex ].VertexID[ iVertIndex ] ].Position + xModulePosition;
                float fDistanceFromAxis;
                xAttractor.GetRelativePosition( xVertexWorldPosition, 0.0f, NULL, NULL, &fDistanceFromAxis );
                // Allow anything less than 20cm away.
                if ( fDistanceFromAxis < 0.2f )
                {
                    aiAdjacentVertIndex[ iNumAdjacentVerts++ ] = iVertIndex;
                }
            }

            // If we've found 2 verts that are adjacent then this is an adjacent poly.
            if ( iNumAdjacentVerts > 1 )
            {
                // We now need a vector parallel to the poly plane and at 90° from the adjacent edge to the attractor.
                // Find a vert that isn't adjacent to the attractor.
                int iNonAdjacentIndex = -1;
                for ( int iPotentialVertIndex = 0; iPotentialVertIndex < iNumVerts && iNonAdjacentIndex == -1; ++iPotentialVertIndex )
                {
                    if ( aiAdjacentVertIndex[ 0 ] != iPotentialVertIndex && aiAdjacentVertIndex[ 1 ] != iPotentialVertIndex )
                    {
                        iNonAdjacentIndex = iPotentialVertIndex;
                    }
                }
                Asura_Assert( iNonAdjacentIndex != -1, "ToolsAVP_Mode_Waypointing::FindAdjacentPolyVector - algorithm error" );

                // Calculate the vector from an adjacent position to a non-adjacent position.
                const Asura_Vector_3& xAdjacentPosition    = pxMesh->VertexDataPtr[ pxMesh->PolygonDataPtr[ iPolyIndex ].VertexID[ aiAdjacentVertIndex[ 0 ] ] ].Position;
                const Asura_Vector_3& xNonAdjacentPosition = pxMesh->VertexDataPtr[ pxMesh->PolygonDataPtr[ iPolyIndex ].VertexID[ iNonAdjacentIndex ] ].Position;
                xPolyVector = xNonAdjacentPosition - xAdjacentPosition;

                // We now need to ensure this vector meets the attractor axis (== the adjacent edge of the poly) at right angles.
                Asura_Vector_3 xAttractorAxisDirection;
                xAttractor.GetAxisDirection( xAttractorAxisDirection );
                xPolyVector = xPolyVector ^ xAttractorAxisDirection;
                xPolyVector = xAttractorAxisDirection ^ xPolyVector;

                // Finally, normalise the vector.
                xPolyVector.Normalise();

                return true;
            }
        }
    }

    return false;
}

void ToolsAVP_Mode_Waypointing::CreateAttractorWorldMatrix( const Asura_Vector_3& xAttractorNormal, const Asura_Vector_3& xAttractorAxis, Asura_Matrix_3x3& xAttractorMatrix )
{
    Asura_Vector_3 xAttractorRight = xAttractorAxis ^ xAttractorNormal;
    xAttractorRight.Normalise();

    xAttractorMatrix.e11 = xAttractorRight.x;
    xAttractorMatrix.e21 = xAttractorRight.y;
    xAttractorMatrix.e31 = xAttractorRight.z;
    xAttractorMatrix.e12 = xAttractorNormal.x;
    xAttractorMatrix.e22 = xAttractorNormal.y;
    xAttractorMatrix.e32 = xAttractorNormal.z;
    xAttractorMatrix.e13 = xAttractorAxis.x;
    xAttractorMatrix.e23 = xAttractorAxis.y;
    xAttractorMatrix.e33 = xAttractorAxis.z;
}

void ToolsAVP_Mode_Waypointing::GetPolyNormal( const u_int uFaceID, Asura_AVector3& xPolyNormal )
{
    Asura_Physics_Tri xTriangle( uFaceID );
    xTriangle.GetNormal( xPolyNormal );
}

void ToolsAVP_Mode_Waypointing::CalculateObjectSpaceAngVelocity( const Asura_Matrix_3x3& xCurrentOrientation, const Asura_Vector_3& xDesiredDirection, Asura_Angle& fAngVelocity )
{
    const Asura_AVector3 xUp( xCurrentOrientation.e12, xCurrentOrientation.e22, xCurrentOrientation.e32 );
    const Asura_AVector3 xCurrentDirection( xCurrentOrientation.e13, xCurrentOrientation.e23, xCurrentOrientation.e33 );
    // The desired direction has already been resolved to our XZ plane and normalised.
    Asura_AVector3		 xDirection( xDesiredDirection );

    Asura_Quat xRotation;
    xRotation.RotationFromAToB( xCurrentDirection, xDirection );

    Asura_AVector3 xAxis;
    fAngVelocity = xRotation.ToRotation( xAxis );
    const float	fDotProduct	= xUp * xAxis;
    if ( fDotProduct < -0.900f )
    {
        fAngVelocity = -fAngVelocity;
    }
}

void ToolsAVP_Mode_Waypointing::AddLastRouteToFailedRoutes( const Asura_Vector_3& xStartPosition, const Asura_Vector_3& xFinishPosition )
{
    if ( s_uNumFailedLinkRoutes < MAX_FAILED_LINK_ROUTES )
    {
        LinkRouteData& xLRD = s_axFailedLinkRoutes[ s_uNumFailedLinkRoutes ];
        xLRD.m_xStartPosition  = xStartPosition;
		xLRD.m_xFinishPosition = xFinishPosition;
        xLRD.m_xBB             = s_xPhysicsBoundingBox;
        if ( xLRD.InitialiseArrays( s_uNumPhysicsPositions ) )
        {
            Asura_Mem::Copy( xLRD.m_pxVelocityArray,    s_pxPhysicsVelocities,   s_uNumPhysicsPositions * sizeof( Asura_Vector_3 ) );
            Asura_Mem::Copy( xLRD.m_pxPositionArray,    s_pxPhysicsPositions,    s_uNumPhysicsPositions * sizeof( Asura_Vector_3 ) );
            Asura_Mem::Copy( xLRD.m_pxOrientationArray, s_pxPhysicsOrientations, s_uNumPhysicsPositions * sizeof( Asura_Quat ) );
            ++s_uNumFailedLinkRoutes;
        }

        if ( s_uNumFailedLinkRoutes == 1 )
        {
            s_xNumFailedLinkRoutesString = "There is 1 failed link route.";
        }
        else
        {
            Asura_String xNumFLRString;
            Asura_StringUtil::ToString( s_uNumFailedLinkRoutes, xNumFLRString );

            s_xNumFailedLinkRoutesString = "There are ";
            s_xNumFailedLinkRoutesString += xNumFLRString;
            s_xNumFailedLinkRoutesString += " failed link routes.";
        }
    }
}

void ToolsAVP_Mode_Waypointing::AVPCalculateWaypointVolumeThreadFunction( const void* pParams )
{
    const float fMinDotProductBetweenWaypointPolygonNormals = Asura_Maths::Cosine( Asura_Maths::DegToRad( s_fWaypointVolumeMaxAngle ) );

    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 )
        {
			Tools_Nav_Vertex& xTestWaypoint = s_apxWaypointStorage[ uBin ][ iWaypointIndex ];

			if ( xTestWaypoint.m_iFlags & TOOLS_NAVVERTEX_FLAG_LIFT_UDW )
			{
				// Lift UDWs specify their own radius.
				continue;
			}

            // Initialise the waypoint size to the maximum.
			xTestWaypoint.m_fRadius = xTestWaypoint.IsDense() ? s_fMaxDenseWaypointRadius : s_fMaxWaypointRadius;

            const Asura_Vector_3& xWaypointNormal	= xTestWaypoint.m_xNormal;
            const Asura_Vector_3& xWaypointPosition = xTestWaypoint.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 == xTestWaypoint.m_iModuleIndex ) { continue; }
                if ( iModule == -1 ) { iModule = xTestWaypoint.m_iModuleIndex; }

                // If we've already shrunk this waypoint below the min radius then stop.
                if ( RadiusIsLessThanMinRadius( xTestWaypoint.m_iFlags, xTestWaypoint.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 = ( xTestWaypoint.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;
					const Asura_Vector_3& xPolyNormal = pxMesh->NormalsDataPtr[ iPoly ].FaceNormal;

				    // 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 if ( xTestWaypoint.IsWall() )
					{
						// Floor and ceiling polys block.
						if ( StaticIsFloorPoly( uPolyFlags, xPolyNormal ) || StaticIsCeilingPoly( xPolyNormal ) )
						{
							bMayBlock = true;
						}
						else
						{
							// Also, any polys outside our normal requirements.
							const float	fDotProduct = xPolyNormal * xWaypointNormal;
							if ( fDotProduct < s_fMaxDotProductBetweenWaypointPolygonNormals && fDotProduct > fMinDotProductBetweenWaypointPolygonNormals )
							{
								bMayBlock = true;
							}
						}
					}
					else if ( xTestWaypoint.IsFloor() )
					{
						// Wall polys block.
						if ( StaticIsWallPoly( uPolyFlags, xPolyNormal ) )
						{
							bMayBlock = true;
						}
					}
					else // must be a ceiling waypoint
				    {
						// Wall polys block.
						if ( StaticIsWallPoly( uPolyFlags, xPolyNormal ) )
						{
							bMayBlock = true;
						}
						else
						{
							// Also, any polys outside our normal requirements.
							const float	fDotProduct = xPolyNormal * xWaypointNormal;
							if ( fDotProduct < s_fMaxDotProductBetweenWaypointPolygonNormals && fDotProduct > fMinDotProductBetweenWaypointPolygonNormals )
							{
								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;
                }

				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 ToolsAVP_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, "ToolsAVP_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 )
        {
            const Asura_AI_NavConnection& xConnection = pxBaseWaypoint->m_pxConnections[ c ];

            if ( ( xConnection.m_usFlags & AVP_Navigation::AVP_AI_CONNECTION_FLAG_HUMANOID ) == 0 )
            {
                continue;
            }

            Asura_AI_NavVertex* pxNextWaypoint = Asura_AI_Navigation_System::GetWaypoint( xConnection.m_usIndex );
            if ( pxNextWaypoint->m_usZone == ASURA_NAVVERTEX_MAX_ZONE_ID )
            {
                pxNextWaypoint->m_usZone = s_usGrayZoneID;
                xPendingQueue.Push_Back( xConnection.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;
}


bool ToolsAVP_Mode_Waypointing::GetSurfaceNormalAtPositionAnyDirection( const Asura_Vector_3& xPosition, Asura_Vector_3& xSurfaceNormal, u_int uLOSFlags /*= AsuraPolygonFlag_NoCollisions*/ )
{
	Asura_Vector_3 axTestDirections[14];
	// primary
	axTestDirections[0] = Asura_Vector_3(1.0f, 0.0f, 0.0f);
	axTestDirections[1] = Asura_Vector_3(-1.0f, 0.0f, 0.0f);
	axTestDirections[2] = Asura_Vector_3(0.0f, 1.0f, 0.0f);
	axTestDirections[3] = Asura_Vector_3(0.0f, -1.0f, 0.0f);
	axTestDirections[4] = Asura_Vector_3(0.0f, 0.0f, 1.0f);
	axTestDirections[5] = Asura_Vector_3(0.0f, 0.0f, -1.0f);
	// edges
	axTestDirections[6] = Asura_Vector_3(1.0f, 1.0f, 1.0f);
	axTestDirections[7] = Asura_Vector_3(-1.0f, 1.0f, 1.0f);
	axTestDirections[8] = Asura_Vector_3(1.0f, -1.0f, 1.0f);
	axTestDirections[9] = Asura_Vector_3(-1.0f, -1.0f, 1.0f);
	axTestDirections[10] = Asura_Vector_3(1.0f, 1.0f, -1.0f);
	axTestDirections[11] = Asura_Vector_3(-1.0f, 1.0f, -1.0f);
	axTestDirections[12] = Asura_Vector_3(1.0f, -1.0f, -1.0f);
	axTestDirections[13] = Asura_Vector_3(-1.0f, -1.0f, -1.0f);

	bool bFound = false;
	float fBestDistance = 10.0f;

	for (int i =0; i < 14; ++i)
	{
		axTestDirections[i].Normalise();
		const Asura_LOS_InputData xInputData( xPosition - axTestDirections[i] * 0.01f ,
			axTestDirections[i],
			ASURA_GUID_UNREGISTERED,
			1.0f,
			false,
			uLOSFlags );
		const Asura_LOS_OutputData* pxOutputData = AVP_LOS::TestAgainstEnvironmentAndMoveableObjects( &xInputData );
		
		if (pxOutputData->m_bHitEnvironment)
		{
			if (pxOutputData->m_fDistanceToIntersection < fBestDistance)
			{
				bFound = true;
				xSurfaceNormal = pxOutputData->m_xNormal;
				fBestDistance = pxOutputData->m_fDistanceToIntersection;
			}
		}

	}

	return ( bFound );
}


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

#undef SCORE_DISTANCE_LINEARLY
