From ac082709cdd4863e5869e1fb8eb39d4c00faeed6 Mon Sep 17 00:00:00 2001
From: MarkT <mark@wildfiregames.com>
Date: Sat, 29 May 2004 03:32:33 +0000
Subject: [PATCH] Added some pathfinding code. (SparsePath)

This was SVN commit r284.
---
 source/main.cpp                             |  26 ++-
 source/ps/Vector2D.h                        |  25 +++
 source/simulation/BaseEntity.cpp            |  12 +-
 source/simulation/BaseEntity.h              |   3 +
 source/simulation/BoundingObjects.cpp       |  41 +++-
 source/simulation/BoundingObjects.h         |  10 +-
 source/simulation/Collision.cpp             |  58 ++++++
 source/simulation/Collision.h               |  11 +
 source/simulation/Entity.cpp                |  58 +++++-
 source/simulation/Entity.h                  |  11 +-
 source/simulation/EntityHandles.cpp         |   9 +-
 source/simulation/EntityHandles.h           |   2 +-
 source/simulation/EntityManager.cpp         |  16 ++
 source/simulation/EntityManager.h           |   3 +
 source/simulation/EntityProperties.cpp      |   8 +-
 source/simulation/EntityProperties.h        |  12 --
 source/simulation/EntityStateProcessing.cpp |  23 ++-
 source/simulation/PathfindEngine.cpp        |   7 +
 source/simulation/PathfindEngine.h          |  26 +++
 source/simulation/PathfindSparse.cpp        | 210 ++++++++++++++++++++
 source/simulation/PathfindSparse.h          |  64 ++++++
 source/terrain/Vector3D.cpp                 |   5 +
 source/terrain/Vector3D.h                   |   2 +
 23 files changed, 584 insertions(+), 58 deletions(-)
 create mode 100755 source/simulation/PathfindEngine.cpp
 create mode 100755 source/simulation/PathfindEngine.h
 create mode 100755 source/simulation/PathfindSparse.cpp
 create mode 100755 source/simulation/PathfindSparse.h

diff --git a/source/main.cpp b/source/main.cpp
index 2a6a71818d..3de6ebe094 100755
--- a/source/main.cpp
+++ b/source/main.cpp
@@ -28,6 +28,7 @@
 #include "Entity.h"
 #include "EntityHandles.h"
 #include "EntityManager.h"
+#include "PathfindEngine.h"
 
 
 #ifndef NO_GUI
@@ -49,6 +50,8 @@ static bool g_NoGLVBO=false;
 
 static bool g_VSync = false;
 
+static bool g_EntGraph = false;
+
 static float g_Gamma = 1.0f;
 
 // mapfile to load or null for no map (and to use default terrain)
@@ -274,16 +277,19 @@ static void Render()
 	RenderTerrain();
 	RenderModels();
 	g_Renderer.FlushFrame();
-/*
-	glPushAttrib( GL_ENABLE_BIT );
-	glDisable( GL_LIGHTING );
-	glDisable( GL_TEXTURE_2D );
-	glColor3f( 1.0f, 0.0f, 1.0f );
 
-	// g_EntityManager.renderAll(); // <-- collision outlines...
+	if( g_EntGraph )
+	{
+		glPushAttrib( GL_ENABLE_BIT );
+		glDisable( GL_LIGHTING );
+		glDisable( GL_TEXTURE_2D );
+		glDisable( GL_DEPTH_TEST );
+		glColor3f( 1.0f, 0.0f, 1.0f );
 
-	glPopAttrib();
-*/
+		g_EntityManager.renderAll(); // <-- collision outlines, pathing routes
+
+		glPopAttrib();
+	}
 
 	// overlay mode
 	glPushAttrib(GL_ENABLE_BIT);
@@ -372,6 +378,8 @@ void ParseArgs(int argc, char* argv[])
 						if( g_Gamma == 0.0f ) g_Gamma = 1.0f;
 					}
 					break;
+				case 'e':
+					g_EntGraph = true;
 				case 'v':
 					g_VSync = true;
 					break;
@@ -507,6 +515,7 @@ int main(int argc, char* argv[])
 	// This needs to be done after the renderer has loaded all its actors...
 	new CBaseEntityCollection;
 	new CEntityManager;
+	new CPathfindEngine;
 
 	g_EntityTemplateCollection.loadTemplates();
 
@@ -606,6 +615,7 @@ if(!g_MapFile)
 
 	delete &g_ScriptingHost;
 	delete &g_Config;
+	delete &g_Pathfinder;
 	delete &g_EntityManager;
 	delete &g_EntityTemplateCollection;
 
diff --git a/source/ps/Vector2D.h b/source/ps/Vector2D.h
index cadc167b24..e85f819b74 100755
--- a/source/ps/Vector2D.h
+++ b/source/ps/Vector2D.h
@@ -23,10 +23,26 @@ public:
 	{
 		return( u.x * v.x + u.y * v.y );
 	}
+	static inline float betadot( const CVector2D& u, const CVector2D& v )
+	{
+		// Beta-dot product. I have no idea if that's its correct name
+		// but use of it tends to simplify collision formulae.
+		// At the moment I think all of my code uses seperate vectors
+		// and dots them together, though.
+		return( u.x * v.y - u.y * v.x );
+	}
+	inline CVector2D beta() const
+	{
+		return( CVector2D( y, -x ) );
+	}
 	inline float dot( const CVector2D& u ) const
 	{
 		return( dot( *this, u ) );
 	}
+	inline float betadot( const CVector2D& u ) const
+	{	
+		return( betadot( *this, u ) );
+	}
 	inline CVector2D operator+( const CVector2D& u ) const
 	{
 		return( CVector2D( x + u.x, y + u.y ) );
@@ -49,11 +65,20 @@ public:
 	{
 		return( CVector2D( x * scale, y * scale ) );
 	}
+	inline CVector2D operator/( const float scale ) const
+	{
+		return( CVector2D( x / scale, y / scale ) );
+	}
 	inline CVector2D& operator*=( const float scale )
 	{
 		x *= scale; y *= scale;
 		return( *this );
 	} 
+	inline CVector2D& operator/=( const float scale )
+	{
+		x /= scale; y /= scale;
+		return( *this );
+	}
 	inline float length() const
 	{
 		return( sqrt( x * x + y * y ) );
diff --git a/source/simulation/BaseEntity.cpp b/source/simulation/BaseEntity.cpp
index af10508e84..cf31a5d443 100755
--- a/source/simulation/BaseEntity.cpp
+++ b/source/simulation/BaseEntity.cpp
@@ -69,6 +69,8 @@ bool CBaseEntity::loadXML( CStr filename )
 					DOMNode *value_node= child_element->getChildNodes()->item(0);
 					CStr element_value=value_node ? XMLString::transcode(value_node->getNodeValue()) : "";
 
+					//m_properties[element_name] = element_value;
+
 					if( element_name == CStr( "Name" ) )
 					{
 						m_name = element_value;
@@ -80,7 +82,11 @@ bool CBaseEntity::loadXML( CStr filename )
 					else if( element_name == CStr( "Speed" ) )
 					{
 						m_speed = element_value.ToFloat();
-					} 
+					}
+					else if( element_name == CStr( "TurningRadius" ) )
+					{
+						m_turningRadius = element_value.ToFloat();
+					}
 					else if( element_name == CStr( "Size" ) )
 					{
 						if( !m_bound_circle )
@@ -113,9 +119,9 @@ bool CBaseEntity::loadXML( CStr filename )
 						m_bound_circle->m_offset.y = y.ToFloat();
 						m_bound_box->m_offset.x = x.ToFloat();
 						m_bound_box->m_offset.y = y.ToFloat();
-						
-						
+
 					}
+					
 				}
 			}
 
diff --git a/source/simulation/BaseEntity.h b/source/simulation/BaseEntity.h
index 82335eaebc..dc10c0c691 100755
--- a/source/simulation/BaseEntity.h
+++ b/source/simulation/BaseEntity.h
@@ -33,6 +33,7 @@ public:
 
 	// Base stats
 
+	
 	CObjectEntry* m_actorObject;
 
 	CStr m_name;
@@ -41,6 +42,8 @@ public:
 	CBoundingObject::EBoundingType m_bound_type;
 
 	float m_speed;
+	float m_turningRadius;
+	
 
 	// Extended properties table
 
diff --git a/source/simulation/BoundingObjects.cpp b/source/simulation/BoundingObjects.cpp
index ce97486511..6201340251 100755
--- a/source/simulation/BoundingObjects.cpp
+++ b/source/simulation/BoundingObjects.cpp
@@ -13,12 +13,21 @@ bool CBoundingObject::intersects( CBoundingObject* obj )
 
 	if( obj->m_type > m_type )	// More complex types get the burden of processing.
 	{
-		return( obj->_intersects( this, &delta ) );
+		return( obj->_intersects( this, delta ) );
 	}
 	else
-		return( _intersects( obj, &delta ) );
+		return( _intersects( obj, delta ) );
 }
 
+bool CBoundingObject::contains( const CVector2D& point )
+{
+	CVector2D delta = m_pos - point;
+
+	if( !delta.within( m_radius ) )
+		return( false );
+
+	return( _contains( point, delta ) );
+}
 CBoundingCircle::CBoundingCircle( float x, float y, float radius )
 {
 	m_type = BOUND_CIRCLE;
@@ -45,7 +54,7 @@ void CBoundingCircle::setRadius( float radius )
 	m_radius = radius;
 }
 
-bool CBoundingCircle::_intersects( CBoundingObject* obj, CVector2D* delta )
+bool CBoundingCircle::_intersects( CBoundingObject* obj, const CVector2D& delta )
 {
 	assert( obj->m_type == BOUND_CIRCLE );
 	// Easy enough. The only time this gets called is a circle-circle collision,
@@ -53,6 +62,11 @@ bool CBoundingCircle::_intersects( CBoundingObject* obj, CVector2D* delta )
 	return( true );
 }
 
+bool CBoundingCircle::_contains( const CVector2D& point, const CVector2D& delta )
+{
+	return( true );
+}
+
 void CBoundingCircle::render( float height )
 {
 	glBegin( GL_LINE_LOOP );
@@ -107,16 +121,16 @@ void CBoundingBox::setOrientation( const CVector2D& u )
 	m_v.y = -m_u.x;
 }
 
-bool CBoundingBox::_intersects( CBoundingObject* obj, CVector2D* delta )
+bool CBoundingBox::_intersects( CBoundingObject* obj, const CVector2D& delta )
 {
 	if( obj->m_type == BOUND_CIRCLE )
 	{
 		// Imperfect but quick...
 
 		CBoundingCircle* c = (CBoundingCircle*)obj;
-		float deltah = fabs( delta->dot( m_u ) );
+		float deltah = fabs( delta.dot( m_u ) );
 		if( deltah > ( m_h + c->m_radius ) ) return( false );
-		float deltaw = fabs( delta->dot( m_v ) );
+		float deltaw = fabs( delta.dot( m_v ) );
 		if( deltaw > ( m_w + c->m_radius ) ) return( false );
 		return( true );
 	}
@@ -153,7 +167,7 @@ bool CBoundingBox::_intersects( CBoundingObject* obj, CVector2D* delta )
 
 		prj1 = fabs( vu * b->m_h + vv * b->m_w );
 		prj2 = fabs( vu * b->m_h - vv * b->m_h );
-		dm = delta->dot( m_v );
+		dm = delta.dot( m_v );
 
 		if( prj1 > prj2 )
 		{
@@ -166,7 +180,7 @@ bool CBoundingBox::_intersects( CBoundingObject* obj, CVector2D* delta )
 
 		prj1 = fabs( uu * b->m_h + uv * b->m_w );
 		prj2 = fabs( uu * b->m_h - uv * b->m_w );
-		dm = delta->dot( m_u );
+		dm = delta.dot( m_u );
 
 		if( prj1 > prj2 ) 
 		{
@@ -179,6 +193,7 @@ bool CBoundingBox::_intersects( CBoundingObject* obj, CVector2D* delta )
 
 		prj1 = fabs( uv * m_h + vv * m_w );
 		prj2 = fabs( uv * m_h - vv * m_w );
+		dm = delta.dot( b->m_v );
 
 		if( prj1 > prj2 )
 		{
@@ -191,6 +206,7 @@ bool CBoundingBox::_intersects( CBoundingObject* obj, CVector2D* delta )
 
 		prj1 = fabs( uu * m_h + vu * m_w );
 		prj2 = fabs( uu * m_h - vu * m_w );
+		dm = delta.dot( b->m_u );
 
 		if( prj1 > prj2 )
 		{
@@ -204,6 +220,15 @@ bool CBoundingBox::_intersects( CBoundingObject* obj, CVector2D* delta )
 	}
 }
 
+bool CBoundingBox::_contains( const CVector2D& point, const CVector2D& delta )
+{
+	float deltah = fabs( delta.dot( m_u ) );
+	if( deltah > m_h ) return( false );
+	float deltaw = fabs( delta.dot( m_v ) );
+	if( deltaw > m_w ) return( false );
+	return( true );
+}
+
 void CBoundingBox::render( float height )
 {
 	glBegin( GL_LINE_LOOP );
diff --git a/source/simulation/BoundingObjects.h b/source/simulation/BoundingObjects.h
index 0073fd7b7d..999e03d258 100755
--- a/source/simulation/BoundingObjects.h
+++ b/source/simulation/BoundingObjects.h
@@ -30,7 +30,9 @@ public:
 	float m_radius;
 	void setPosition( float x, float y );
 	bool intersects( CBoundingObject* obj );
-	virtual bool _intersects( CBoundingObject* obj, CVector2D* delta ) = 0;
+	bool contains( const CVector2D& point );
+	virtual bool _intersects( CBoundingObject* obj, const CVector2D& delta ) = 0;
+	virtual bool _contains( const CVector2D& point, const CVector2D& delta ) = 0;
 	virtual void render( float height ) = 0;	// Temporary
 };
 
@@ -41,7 +43,8 @@ public:
 	CBoundingCircle( float x, float y, float radius );
 	CBoundingCircle( float x, float y, CBoundingCircle* copy );
 	void setRadius( float radius );
-	bool _intersects( CBoundingObject* obj, CVector2D* delta );
+	bool _intersects( CBoundingObject* obj, const CVector2D& delta );
+	bool _contains( const CVector2D& point, const CVector2D& delta );
 	void render( float height );	// Temporary
 };
 
@@ -68,7 +71,8 @@ public:
 	void setOrientation( const CVector2D& orientation );
 	float getWidth() const { return( 2.0f * m_w ); };
 	float getHeight() const { return( 2.0f * m_h ); };
-	bool _intersects( CBoundingObject* obj, CVector2D* delta );
+	bool _intersects( CBoundingObject* obj, const CVector2D& delta );
+	bool _contains( const CVector2D& point, const CVector2D& delta );
 	void render( float height );	// Temporary
 };
 
diff --git a/source/simulation/Collision.cpp b/source/simulation/Collision.cpp
index 160cb64571..8c876c1446 100755
--- a/source/simulation/Collision.cpp
+++ b/source/simulation/Collision.cpp
@@ -1,6 +1,26 @@
 #include "Collision.h"
 #include "EntityManager.h"
 
+CBoundingObject* getContainingObject( const CVector2D& point )
+{
+	std::vector<HEntity>* entities = g_EntityManager.getActive();
+	std::vector<HEntity>::iterator it;
+
+	for( it = entities->begin(); it != entities->end(); it++ )
+	{
+		assert( (*it)->m_bounds );
+		if( (*it)->m_bounds->contains( point ) )
+		{
+			CBoundingObject* bounds = (*it)->m_bounds;
+			delete( entities );
+			return( bounds );
+		}
+	}
+
+	delete( entities );
+	return( NULL );
+}
+
 HEntity getCollisionObject( CEntity* entity )
 {
 	assert( entity->m_bounds ); 
@@ -32,4 +52,42 @@ HEntity getCollisionObject( CEntity* entity, float x, float y )
 	HEntity _e = getCollisionObject( entity );
 	entity->m_bounds->setPosition( _x, _y );
 	return( _e );
+}
+
+bool getRayIntersection( const CVector2D& source, const CVector2D& forward, const CVector2D& right, float length, float maxDistance, rayIntersectionResults* results )
+{
+	std::vector<HEntity>* entities = g_EntityManager.getActive();
+	std::vector<HEntity>::iterator it;
+
+	float closestApproach, dist;
+
+	CVector2D delta;
+
+	results->distance = length + maxDistance;
+	results->boundingObject = NULL;
+
+	for( it = entities->begin(); it != entities->end(); it++ )
+	{
+		assert( (*it)->m_bounds );
+		if( (*it)->m_speed ) continue;
+		CBoundingObject* obj = (*it)->m_bounds;
+		delta = obj->m_pos - source;
+		closestApproach = delta.dot( right );
+		dist = delta.dot( forward );
+
+		if( ( fabs( closestApproach ) < maxDistance + obj->m_radius ) && ( dist > -maxDistance ) && ( dist < length + maxDistance ) ) 
+		{
+			if( dist < results->distance )
+			{
+				results->boundingObject = obj;
+				results->closestApproach = closestApproach;
+				results->distance = dist;
+				results->hEntity = (*it);
+				results->position = obj->m_pos;
+			}
+		}
+	}
+	delete( entities );
+	if( results->boundingObject ) return( true );
+	return( false );
 }
\ No newline at end of file
diff --git a/source/simulation/Collision.h b/source/simulation/Collision.h
index 93e3267a08..8b1d08eec8 100755
--- a/source/simulation/Collision.h
+++ b/source/simulation/Collision.h
@@ -18,7 +18,18 @@
 #include "BoundingObjects.h"
 #include "Entity.h"
 
+struct rayIntersectionResults
+{
+	HEntity hEntity;
+	CBoundingObject* boundingObject;
+	CVector2D position;
+	float closestApproach;
+	float distance;
+};
+
 HEntity getCollisionObject( CEntity* entity );
 HEntity getCollisionObject( CEntity* entity, float x, float y );
+CBoundingObject* getContainingObject( const CVector2D& point );
+bool getRayIntersection( const CVector2D& source, const CVector2D& forward, const CVector2D& right, float length, float maxDistance, rayIntersectionResults* results );
 
 #endif
\ No newline at end of file
diff --git a/source/simulation/Entity.cpp b/source/simulation/Entity.cpp
index 39ae73a427..f7b77a0ea5 100755
--- a/source/simulation/Entity.cpp
+++ b/source/simulation/Entity.cpp
@@ -27,6 +27,7 @@ CEntity::CEntity( CBaseEntity* base, CVector3D position, float orientation )
 	// Set up our instance data
 
 	m_speed = m_base->m_speed;
+	m_turningRadius = m_base->m_turningRadius;
 	m_position = position;
 	m_orientation = orientation;
 
@@ -46,13 +47,24 @@ CEntity::CEntity( CBaseEntity* base, CVector3D position, float orientation )
 	updateActorTransforms();
 
 	// Register the addresses of our native properties with the properties table
-	/*
+	
 	m_properties["speed"].associate( &m_speed );
 	m_properties["orientation"].associate( &m_orientation );
 	m_properties["position"].associate( &m_position );
-	*/
+	
 }
 
+CEntity::~CEntity()
+{
+	if( m_actor )
+	{
+		if( m_actor->m_Model ) delete( m_actor->m_Model );
+		g_UnitMan.RemoveUnit( m_actor );
+		delete( m_actor );
+	}
+	if( m_bounds ) delete( m_bounds );
+}
+	
 bool isWaypoint( CEntity* e )
 {
 	return( e->m_base->m_name == CStr( "Waypoint" ) );
@@ -162,6 +174,12 @@ void CEntity::dispatch( CMessage* msg )
 	case CMessage::EMSG_INIT:
 		if( m_base->m_name == CStr( "Prometheus Dude" ) )
 		{
+			if( getCollisionObject( this ) )
+			{
+				// Prometheus telefragging. (Appeared inside another object)
+				g_EntityManager.kill( me );
+				return;
+			}
 			std::vector<HEntity>* waypoints = g_EntityManager.matches( isWaypoint );
 			while( !waypoints->empty() )
 			{
@@ -191,10 +209,44 @@ void CEntity::render()
 {
 	// Rich! Help! ;)
 	// We can loose this later on, I just need a way to see collision boxes temporarily
+	
+	if( !m_orderQueue.empty() )
+	{
+		glShadeModel( GL_FLAT );
+		glBegin( GL_LINE_STRIP );
+		
+		std::deque<CEntityOrder>::iterator it;
 
+		glVertex3f( m_position.X, m_position.Y + 0.25f /* 20.0f */, m_position.Z );
+
+		for( it = m_orderQueue.begin(); it < m_orderQueue.end(); it++ )
+		{
+			float x = it->m_data[0].location.x;
+			float y = it->m_data[0].location.y;
+			switch( it->m_type )
+			{
+			case CEntityOrder::ORDER_GOTO:
+				glColor3f( 1.0f, 0.0f, 0.0f ); break;
+			case CEntityOrder::ORDER_GOTO_COLLISION:
+				glColor3f( 1.0f, 0.5f, 0.5f ); break;
+			case CEntityOrder::ORDER_GOTO_NOPATHING:
+				glColor3f( 0.5f, 0.5f, 0.5f ); break;
+			default:
+				glColor3f( 1.0f, 1.0f, 1.0f ); break;
+			}
+			glVertex3f( x, getExactGroundLevel( x, y ) + 0.25f /* 20.0f */, y );
+		}
+
+		glEnd();
+		glShadeModel( GL_SMOOTH );
+	}
+	
 	glColor3f( 1.0f, 1.0f, 1.0f );
+	
+
 	if( getCollisionObject( this ) ) glColor3f( 0.5f, 0.5f, 1.0f );
-	m_bounds->render( m_position.Y + 0.25f );
+	m_bounds->render( m_position.Y + 0.25f /* 20.0f */ );
+
 }
 
 void PASAPScenario()
diff --git a/source/simulation/Entity.h b/source/simulation/Entity.h
index 7f1329d5d9..e2440736ec 100755
--- a/source/simulation/Entity.h
+++ b/source/simulation/Entity.h
@@ -9,7 +9,14 @@
 //
 //			HEntity me: is a reference to this entity. Use instead of the address-of operator for
 //						non-temporary references. See EntityHandles.h
-//						When an entity dies, this should be set to refer to the bad-handle handle.
+//
+//			Destroying entities: An entity is destroyed when all references to it expire.
+//								 It is somewhat unfunny if this happens while a method from this
+//                               class is still executing. If you need to kill an entity,
+//								 use g_EntityManager.kill( entity ). If kill() releases the final handle,
+//								 the entity is placed in the reaper queue and is deleted immediately
+//								 prior to its next update cycle.
+//
 //			CUnit* m_actor: is the visible representation of this entity.
 //			std::hash_map m_properties: isn't yet used, is capable of storing properties defined by script.
 //			
@@ -48,6 +55,7 @@ private:
 public:
 	CStr m_name;
 	float m_speed;
+	float m_turningRadius;
 	CVector3D m_position;
 	CBoundingObject* m_bounds;
 	float m_targetorientation;
@@ -70,6 +78,7 @@ private:
 	bool processPatrol( CEntityOrder* current, float timestep );
 
 public:
+	~CEntity();
 
 	// Handle-to-self.
 	HEntity me;
diff --git a/source/simulation/EntityHandles.cpp b/source/simulation/EntityHandles.cpp
index 8095e7c320..5e7e1d2a44 100755
--- a/source/simulation/EntityHandles.cpp
+++ b/source/simulation/EntityHandles.cpp
@@ -54,25 +54,22 @@ void HEntity::decRef()
 {
 	if( m_handle != INVALID_HANDLE )
 	{
-		assert( g_EntityManager.m_entities[m_handle].m_refcount != 0 );
 		if( --g_EntityManager.m_entities[m_handle].m_refcount == 0 )
-		{
-			delete( g_EntityManager.m_entities[m_handle].m_entity );
-		}
+			g_EntityManager.destroy( m_handle );
 	}
 }
 
 CEntity* HEntity::operator->() const
 {
 	assert( m_handle != INVALID_HANDLE );
-	assert( g_EntityManager.m_entities[m_handle].m_refcount != 0 );
+	assert( g_EntityManager.m_entities[m_handle].m_refcount );
 	return( g_EntityManager.m_entities[m_handle].m_entity );
 }
 
 CEntity& HEntity::operator*() const
 {
 	assert( m_handle != INVALID_HANDLE );
-	assert( g_EntityManager.m_entities[m_handle].m_refcount != 0 );
+	assert( g_EntityManager.m_entities[m_handle].m_refcount );
 	return( *g_EntityManager.m_entities[m_handle].m_entity );
 }
 
diff --git a/source/simulation/EntityHandles.h b/source/simulation/EntityHandles.h
index 02efa45759..9391cdf7ea 100755
--- a/source/simulation/EntityHandles.h
+++ b/source/simulation/EntityHandles.h
@@ -31,7 +31,7 @@ class CHandle
 {
 public:
 	CEntity* m_entity;
-	u16 m_refcount;
+	i16 m_refcount;
 	CHandle();
 };
 
diff --git a/source/simulation/EntityManager.cpp b/source/simulation/EntityManager.cpp
index 084bf80083..e87307c373 100755
--- a/source/simulation/EntityManager.cpp
+++ b/source/simulation/EntityManager.cpp
@@ -55,8 +55,18 @@ void CEntityManager::dispatchAll( CMessage* msg )
 			m_entities[i].m_entity->dispatch( msg );
 }
 
+void CEntityManager::kill( HEntity entity )
+{
+	m_reaper.push_back( entity );
+}
+
 void CEntityManager::updateAll( float timestep )
 {
+	std::vector<HEntity>::iterator it;
+	for( it = m_reaper.begin(); it < m_reaper.end(); it++ )
+		(*it)->me = HEntity();
+	m_reaper.clear();
+
 	for( int i = 0; i < MAX_HANDLES; i++ )
 		if( m_entities[i].m_refcount )
 			m_entities[i].m_entity->update( timestep );
@@ -69,4 +79,10 @@ void CEntityManager::renderAll()
 			m_entities[i].m_entity->render();
 }
 
+void CEntityManager::destroy( u16 handle )
+{
+	m_entities[handle].m_entity->me.m_handle = INVALID_HANDLE;
+	delete( m_entities[handle].m_entity );
+}
+
 bool CEntityManager::m_extant = false;
\ No newline at end of file
diff --git a/source/simulation/EntityManager.h b/source/simulation/EntityManager.h
index 1a9ba117c5..38b971f988 100755
--- a/source/simulation/EntityManager.h
+++ b/source/simulation/EntityManager.h
@@ -32,14 +32,17 @@ class CEntityManager : public Singleton<CEntityManager>
 friend HEntity;
 friend CHandle;
 	CHandle m_entities[MAX_HANDLES];
+	std::vector<HEntity> m_reaper;
 	int m_nextalloc;
 	static bool m_extant;
+	void destroy( u16 handle );
 public:
 	typedef bool (*EntityPredicate)( CEntity* target );
 	CEntityManager();
 	~CEntityManager();
 	HEntity create( CBaseEntity* base, CVector3D position, float orientation );
 	HEntity create( CStr templatename, CVector3D position, float orientation );
+	void kill( HEntity ent );
 	void updateAll( float timestep );
 	void dispatchAll( CMessage* msg );
 	void renderAll(); // TODO MT: What's the correct way to hook this up to the renderer?
diff --git a/source/simulation/EntityProperties.cpp b/source/simulation/EntityProperties.cpp
index 1148fa100e..6357ae0beb 100755
--- a/source/simulation/EntityProperties.cpp
+++ b/source/simulation/EntityProperties.cpp
@@ -130,7 +130,7 @@ void CGenericProperty::associate( i32* value )
 	releaseData();
 	m_type = (EPropTypes)( PROP_INTEGER | PROP_INTRINSIC | PROP_TYPELOCKED );
 	m_integerptr = value;
-	*m_integerptr = current;
+	//*m_integerptr = current;
 }
 
 void CGenericProperty::associate( float* value )
@@ -139,7 +139,7 @@ void CGenericProperty::associate( float* value )
 	releaseData();
 	m_type = (EPropTypes)( PROP_FLOAT | PROP_INTRINSIC | PROP_TYPELOCKED );
 	m_floatptr = value;
-	*m_floatptr = current;
+	//*m_floatptr = current;
 }
 
 void CGenericProperty::associate( CStr* value )
@@ -148,7 +148,7 @@ void CGenericProperty::associate( CStr* value )
 	releaseData();
 	m_type = (EPropTypes)( PROP_STRING | PROP_VECTOR | PROP_TYPELOCKED );
 	m_string = value;
-	*m_string = current;
+	//*m_string = current;
 }
 
 void CGenericProperty::associate( CVector3D* value )
@@ -157,7 +157,7 @@ void CGenericProperty::associate( CVector3D* value )
 	releaseData();
 	m_type = (EPropTypes)( PROP_VECTOR | PROP_INTRINSIC | PROP_TYPELOCKED );
 	m_vector = value;
-	*value = current;
+	//*value = current;
 }
 
 void CGenericProperty::typelock( EPropTypes type )
diff --git a/source/simulation/EntityProperties.h b/source/simulation/EntityProperties.h
index 9f308108bd..c4cc2a719e 100755
--- a/source/simulation/EntityProperties.h
+++ b/source/simulation/EntityProperties.h
@@ -65,18 +65,6 @@ public:
 	~CGenericProperty();
 	void releaseData();
 
-	/*
-	CGenericProperty( i32 value ); // Create an integer property with a given value.
-	CGenericProperty( i32* value );  // Create an integer property that points to the given variable.
-	CGenericProperty( float value ); // Create a floating-point property with a given value.
-	CGenericProperty( float* value ); // Create a floating-point property that points to the given variable.
-	CGenericProperty( CStr& value ); // Create a string object property that's initialized to a copy of the given string.
-	CGenericProperty( CStr* value ); // Create a string object property that points to the given variable.
-	CGenericProperty( CVector3D& value ); // Create a vector object property that's initialized to a copy of the given vector.
-	CGenericProperty( CVector3D* value ); // Create a vector object property that points to the given variable.
-	CGenericProperty( void* value ); // Create a general property that points to the given value.
-	*/
-
 	// Associator functions: Links the property with the specified engine variable.
 	void associate( i32* value ); 
 	void associate( float* value ); 
diff --git a/source/simulation/EntityStateProcessing.cpp b/source/simulation/EntityStateProcessing.cpp
index b5f7ffd814..c25518c02a 100755
--- a/source/simulation/EntityStateProcessing.cpp
+++ b/source/simulation/EntityStateProcessing.cpp
@@ -4,6 +4,7 @@
 #include "Model.h"
 
 #include "Collision.h"
+#include "PathfindEngine.h"
 
 bool CEntity::processGotoNoPathing( CEntityOrder* current, float timestep )
 {
@@ -11,13 +12,20 @@ bool CEntity::processGotoNoPathing( CEntityOrder* current, float timestep )
 	delta.x = (float)current->m_data[0].location.x - m_position.X;
 	delta.y = (float)current->m_data[0].location.y - m_position.Z;
 
-	m_ahead = delta.normalize();
+	float len = delta.length();
+
+	if( len < 0.1f )
+	{
+		m_orderQueue.pop_front();
+		return( false );
+	}
+
+	m_ahead = delta / len;
 
 	if( m_bounds->m_type == CBoundingObject::BOUND_OABB )
 		((CBoundingBox*)m_bounds)->setOrientation( m_ahead );
 
-	float len = delta.length();
-
+	
 	float scale = timestep * m_speed;
 
 	if( scale > len )
@@ -99,23 +107,20 @@ bool CEntity::processGotoNoPathing( CEntityOrder* current, float timestep )
 
 	snapToGround();
 	updateActorTransforms();
-	if( len < 0.1f )
-		m_orderQueue.pop_front();
+
 	return( false );
 }
 
 bool CEntity::processGoto( CEntityOrder* current, float timestep )
 {
-	CEntityOrder pathfind_solution;
-	pathfind_solution.m_type = CEntityOrder::ORDER_GOTO_NOPATHING;
-	pathfind_solution.m_data[0] = current->m_data[0];
+	CVector2D path_to = current->m_data[0].location;
 	m_orderQueue.pop_front();
-	m_orderQueue.push_front( pathfind_solution );
 	if( m_actor->m_Model->GetAnimation() != m_actor->m_Object->m_WalkAnim )
 	{
 		m_actor->m_Model->SetAnimation( m_actor->m_Object->m_WalkAnim );
 		m_actor->m_Model->Update( ( rand() * 1000.0f ) / 1000.0f );
 	}
+	g_Pathfinder.requestPath( me, path_to );
 	return( true );
 }
 
diff --git a/source/simulation/PathfindEngine.cpp b/source/simulation/PathfindEngine.cpp
new file mode 100755
index 0000000000..58b8576597
--- /dev/null
+++ b/source/simulation/PathfindEngine.cpp
@@ -0,0 +1,7 @@
+#include "PathfindEngine.h"
+#include "PathfindSparse.h"
+
+void CPathfindEngine::requestPath( HEntity entity, const CVector2D& destination )
+{
+	pathSparse( entity, destination );
+}
\ No newline at end of file
diff --git a/source/simulation/PathfindEngine.h b/source/simulation/PathfindEngine.h
new file mode 100755
index 0000000000..a80bb152db
--- /dev/null
+++ b/source/simulation/PathfindEngine.h
@@ -0,0 +1,26 @@
+// PathfindEngine.h
+//
+// Last modified: 28 May 04, Mark Thompson mot20@cam.ac.uk / mark@wildfiregames.com
+// 
+// The pathfinding engine singleton.
+//
+// Usage: g_Pathfinder.requestPath( HEntity me, float x, float y );
+//
+// Mark Thompson mot20@cam.ac.uk / mark@wildfiregames.com
+
+#ifndef PATHFIND_ENGINE_INCLUDED
+#define PATHFIND_ENGINE_INCLUDED
+
+#include "Singleton.h"
+#include "EntityHandles.h"
+#include "Vector2D.h"
+
+#define g_Pathfinder CPathfindEngine::GetSingleton()
+
+class CPathfindEngine : public Singleton<CPathfindEngine>
+{
+public:
+	void requestPath( HEntity entity, const CVector2D& destination );
+};
+
+#endif
diff --git a/source/simulation/PathfindSparse.cpp b/source/simulation/PathfindSparse.cpp
new file mode 100755
index 0000000000..88fb6ee71a
--- /dev/null
+++ b/source/simulation/PathfindSparse.cpp
@@ -0,0 +1,210 @@
+#include "PathfindSparse.h"
+
+#define NODESMOOTH_STEPS 4
+
+sparsePathTree::sparsePathTree( const CVector2D& _from, const CVector2D& _to, HEntity _entity, CBoundingObject* _destinationCollisionObject )
+{
+	from = _from; to = _to;
+	assert( from.length() > 0.01f );
+	assert( to.length() > 0.01f );
+
+	entity = _entity; destinationCollisionObject = _destinationCollisionObject;
+	leftPre = NULL; leftPost = NULL;
+	rightPre = NULL; rightPost = NULL;
+	type = SPF_OPEN_UNVISITED;
+	leftImpossible = false; rightImpossible = false;
+}
+
+sparsePathTree::~sparsePathTree()
+{
+	if( leftPre ) delete( leftPre );
+	if( leftPost ) delete( leftPost );
+	if( rightPre ) delete( rightPre );
+	if( rightPost ) delete( rightPost );
+}
+
+bool sparsePathTree::slice()
+{
+	if( type == SPF_OPEN_UNVISITED )
+	{
+		rayIntersectionResults r;
+
+		CVector2D forward = to - from;
+		float len = forward.length();
+		forward /= len;
+		CVector2D right = CVector2D( forward.y, -forward.x );
+
+		// Hit nothing or hit destination; that's OK.
+		if( !getRayIntersection( from, forward, right, len, entity->m_bounds->m_radius * 1.1f, &r ) || ( r.boundingObject == destinationCollisionObject ) )
+		{
+			type = SPF_CLOSED_DIRECT;
+			return( true );
+		}
+
+		float turningRadius = ( entity->m_bounds->m_radius + r.boundingObject->m_radius ) * 1.1f; 
+		
+		if( turningRadius < entity->m_turningRadius ) turningRadius = entity->m_turningRadius;
+
+		// Too close, an impossible turn
+		if( r.distance < turningRadius ||
+			r.distance > ( len - turningRadius ) )
+		{
+			type = SPF_IMPOSSIBLE;
+			return( true );
+		}
+
+		CVector2D delta = r.position - from;
+		float length = delta.length();
+
+		float offsetDistance = ( turningRadius * length / sqrt( length * length - turningRadius * turningRadius ) );
+		
+		favourLeft = false;
+		if( r.closestApproach < 0 )
+			favourLeft = true;
+
+		// First we path to the left...
+
+		left = r.position - right * offsetDistance;
+		leftPre = new sparsePathTree( from, left, entity, destinationCollisionObject );
+		leftPost = new sparsePathTree( left, to, entity, destinationCollisionObject );
+
+		// Then we path to the right...
+
+		right = r.position + right * offsetDistance;
+		rightPre = new sparsePathTree( from, right, entity, destinationCollisionObject );
+		rightPost = new sparsePathTree( right, to, entity, destinationCollisionObject );
+
+		// If anybody reaches this point and is thinking:
+		//
+		// "Let's Do The Time-Warp Agaaaain!"
+		//
+		// Let me know.
+
+		type = SPF_OPEN_PROCESSING;
+
+		return( true );
+	}
+	else /* type == SPF_OPEN_PROCESSING */
+	{
+		bool done = false;
+		if( !leftImpossible )
+		{
+			if( !done && ( leftPre->type & SPF_OPEN ) )
+				done |= leftPre->slice();
+			if( !done && ( leftPost->type & SPF_OPEN ) )
+				done |= leftPost->slice();
+			if( ( leftPre->type == SPF_IMPOSSIBLE ) || ( leftPost->type == SPF_IMPOSSIBLE ) )
+				leftImpossible = true;
+		}
+		if( !rightImpossible && !done )
+		{
+			if( !done && ( rightPre->type & SPF_OPEN ) )
+				done |= rightPre->slice();
+			if( !done && ( rightPost->type & SPF_OPEN ) )
+				done |= rightPost->slice();
+			if( ( rightPre->type == SPF_IMPOSSIBLE ) || ( rightPost->type == SPF_IMPOSSIBLE ) )
+				rightImpossible = true;
+		}
+		if( leftImpossible && rightImpossible )
+		{
+			type = SPF_IMPOSSIBLE;
+			return( done );
+		}
+		if( ( ( leftPre->type & SPF_SOLVED ) && ( leftPost->type & SPF_SOLVED ) ) ||
+			( ( rightPre->type & SPF_SOLVED ) && ( rightPost->type & SPF_SOLVED ) ) )
+		{
+			type = SPF_CLOSED_WAYPOINTED;
+			return( done );
+		}
+		return( done );
+	}
+}
+
+void sparsePathTree::pushResults( std::vector<CVector2D>& nodelist )
+{
+	assert( type & SPF_SOLVED );
+
+	if( type == SPF_CLOSED_DIRECT )
+	{
+		nodelist.push_back( to );
+	}
+	else /* type == SPF_CLOSED_WAYPOINTED */
+	{
+		leftImpossible = !( ( leftPre->type & SPF_SOLVED ) && ( leftPost->type & SPF_SOLVED ) );
+		rightImpossible = !( ( rightPre->type & SPF_SOLVED ) && ( rightPost->type & SPF_SOLVED ) );
+
+		if( !leftImpossible && ( favourLeft || rightImpossible ) )
+		{
+			leftPost->pushResults( nodelist );
+			leftPre->pushResults( nodelist );
+		}
+		else
+		{
+			assert( !rightImpossible );
+			rightPost->pushResults( nodelist );
+			rightPre->pushResults( nodelist );
+		}
+	}
+}		
+
+void nodeSmooth( HEntity entity, std::vector<CVector2D>& nodelist )
+{
+	// All your CPU are belong to us.
+	// But Jan wanted it ;)
+
+	std::vector<CVector2D>::iterator it;
+	CVector2D next = nodelist.front();
+
+	CEntityOrder node;
+	node.m_type = CEntityOrder::ORDER_GOTO_NOPATHING;
+	node.m_data[0].location = next;
+
+	entity->m_orderQueue.push_front( node );
+
+	for( it = nodelist.begin() + 1; it != nodelist.end(); it++ )
+	{
+		if( ( it + 1 ) == nodelist.end() ) break;
+		CVector2D current = *it;
+		CVector2D previous = *( it + 1 );
+		CVector2D u = current - previous;
+		CVector2D v = next - current;
+		u = u.normalize();
+		v = v.normalize();
+		CVector2D ubar = u.beta();
+		CVector2D vbar = v.beta();
+		float alpha = entity->m_turningRadius * ( ubar - vbar ).length() / ( u + v ).length();
+		u *= alpha;
+		v *= alpha;
+
+		for( int t = NODESMOOTH_STEPS; t >= 0; t-- )
+		{
+			float lambda = t / (float)NODESMOOTH_STEPS;
+			CVector2D arcpoint = current + v * lambda * lambda - u * ( 1 - lambda ) * ( 1 - lambda );
+			node.m_data[0].location = arcpoint;
+			entity->m_orderQueue.push_front( node );
+		}
+		
+		next = current;
+	}
+}
+
+void pathSparse( HEntity entity, CVector2D destination )
+{
+	std::vector<CVector2D> pathnodes;
+	sparsePathTree sparseEngine( CVector2D( entity->m_position.X, entity->m_position.Z ), destination, entity, getContainingObject( destination ) );
+	while( sparseEngine.type & sparsePathTree::SPF_OPEN ) sparseEngine.slice();
+
+	if( sparseEngine.type & sparsePathTree::SPF_SOLVED )
+	{
+		sparseEngine.pushResults( pathnodes );
+		nodeSmooth( entity, pathnodes );
+	}
+	else
+	{
+		// Try a straight line. All we can do, really.
+		CEntityOrder direct;
+		direct.m_type = CEntityOrder::ORDER_GOTO_NOPATHING;
+		direct.m_data[0].location = destination;
+		entity->m_orderQueue.push_front( direct );
+	}
+}
\ No newline at end of file
diff --git a/source/simulation/PathfindSparse.h b/source/simulation/PathfindSparse.h
new file mode 100755
index 0000000000..b078228164
--- /dev/null
+++ b/source/simulation/PathfindSparse.h
@@ -0,0 +1,64 @@
+// PathfindSparse.h
+//
+// Last modified: 28 May 04, Mark Thompson mot20@cam.ac.uk / mark@wildfiregames.com
+// 
+// Sparse pathfinder.
+//
+// Usage: You won't. See PathfindEngine.h
+//
+// Notes: A geometric pathfinder primarily for path postprocessing. Takes straight line
+//          paths and warps them to avoid obstacles.
+//        Sparse, because it runs in /exponential/ time with number of detours. Hence, only use
+//          where obstructions are sparse. You'll have fun if you try and path through, say, 
+//          a forest with this.
+//
+//        It also won't work at all through impassable terrain.
+//
+// TODO: Fix the algorithm for OABB obstacles.
+// TODO: Replace with timesliced version and proper manager (either a singleton or tasks linked into g_Pathfinder)
+//
+// Mark Thompson mot20@cam.ac.uk / mark@wildfiregames.com
+
+#ifndef PATHFIND_SPARSE_INCLUDED
+#define PATHFIND_SPARSE_INCLUDED
+
+#include "EntityHandles.h"
+#include "Vector2D.h"
+#include "Collision.h"
+
+struct sparsePathTree
+{
+	enum
+	{
+		SPF_IMPOSSIBLE = 0,
+		SPF_OPEN_UNVISITED = 4,
+		SPF_OPEN_PROCESSING = 5,
+		SPF_CLOSED_DIRECT = 2,
+		SPF_CLOSED_WAYPOINTED = 3,
+		SPF_OPEN = 4,
+		SPF_SOLVED = 2
+	} type;
+	HEntity entity;
+	CBoundingObject* destinationCollisionObject;
+	CVector2D from;
+	CVector2D to;
+	bool leftImpossible;
+	CVector2D left;
+	bool rightImpossible;
+	CVector2D right;
+	bool favourLeft;
+	sparsePathTree* leftPre;
+	sparsePathTree* leftPost;
+	sparsePathTree* rightPre;
+	sparsePathTree* rightPost;
+	sparsePathTree( const CVector2D& from, const CVector2D& to, HEntity entity, CBoundingObject* destinationCollisionObject );
+	~sparsePathTree();
+	bool slice();
+	void pushResults( std::vector<CVector2D>& nodelist );
+};
+
+void nodeSmooth( HEntity entity, std::vector<CVector2D>& nodelist );
+void pathSparse( HEntity entity, CVector2D destination );
+bool pathSparseRecursive( HEntity entity, CVector2D from, CVector2D to, CBoundingObject* destinationCollisionObject );
+
+#endif
\ No newline at end of file
diff --git a/source/terrain/Vector3D.cpp b/source/terrain/Vector3D.cpp
index 2f3a3b2ca3..a20ce5c39d 100755
--- a/source/terrain/Vector3D.cpp
+++ b/source/terrain/Vector3D.cpp
@@ -118,6 +118,11 @@ void CVector3D::Clear ()
 	X = Y = Z = 0.0f;
 }
 
+bool CVector3D::operator==( const CVector3D& vector ) const
+{
+	return( ( X == vector.X ) && ( Y == vector.Y ) && ( Z == vector.Z ) );
+}
+
 //Dot product
 float CVector3D::Dot (const CVector3D &vector) const 
 {
diff --git a/source/terrain/Vector3D.h b/source/terrain/Vector3D.h
index 8a68a1f6bd..7ea9a3800d 100755
--- a/source/terrain/Vector3D.h
+++ b/source/terrain/Vector3D.h
@@ -48,6 +48,8 @@ class CVector3D
 		// negation
 		CVector3D operator-() const;
 
+		bool operator==( const CVector3D& vector ) const;
+
 	public:
 		void Set (float x, float y, float z);
 		void Clear ();