added basic flow field collision detection for terrain and Prims in scene. Static at the moment

Flocking/Boid.cs

@@ -26,28 +26,28 @@
  */
 using System;
 using System.Collections.Generic;
+using log4net;
 using OpenMetaverse;
 
 namespace Flocking
 {
 	public class Boid
 	{
+		private static readonly ILog m_log = LogManager.GetLogger (System.Reflection.MethodBase.GetCurrentMethod ().DeclaringType);
 		private string m_id;
 		
 		private Vector3 m_loc;
-		private Quaternion m_rotation;
 		private Vector3 m_vel;
 		private Vector3 m_acc;
 		private Random m_rndnums = new Random (Environment.TickCount);
 		private float m_tolerance;		// how close can we get to things witout being edgy	 
 		private float m_maxForce;    	// Maximum steering force
 		private float m_maxSpeed;    	// Maximum speed
-		
+				
-		private float m_width = 255f;
-		private float m_height = 255f;
-		
 		private float m_neighborDist = 25.0f;
 		private float m_desiredSeparation = 20.0f;
+		
+		private FlowMap m_flowMap;
 
 		
 		/// <summary>
@@ -62,15 +62,15 @@ namespace Flocking
 		/// <param name='mf'>
 		/// Mf. max force / acceleration this boid can extert
 		/// </param>
-		public Boid (string id, float ms, float mf)
+		public Boid (string id, float ms, float mf, FlowMap flowMap)
 		{
 			m_id = id;
 			m_acc = Vector3.Zero;
 			m_vel = new Vector3 (m_rndnums.Next (-1, 1), m_rndnums.Next (-1, 1), m_rndnums.Next (-1, 1));
-    
+			m_tolerance = 5.0f;
-			m_tolerance = 2.0f;
 			m_maxSpeed = ms;
 			m_maxForce = mf;
+			m_flowMap = flowMap;
 		}
 		
 		public Vector3 Location {
@@ -82,10 +82,6 @@ namespace Flocking
 			get { return m_vel;}
 		}
 		
-		public Quaternion Rotation {
-			get { return m_rotation; }
-		}
-		
 		public String Id {
 			get {return m_id;}
 		}
@@ -98,14 +94,19 @@ namespace Flocking
 		/// </param>
 		public void MoveInSceneRelativeToFlock (List<Boid> boids)
 		{
-			Vector3 previousLoc = new Vector3( m_loc );
+			// we would like to stay with our mates
 			Flock (boids);
+
+			// our first priority is to not hurt ourselves
+			AvoidObstacles ();
+			
+			// then we want to avoid any threats
+			//		this not implemented yet
+			
+			
+			// ok so we worked our where we want to go, so ...
 			UpdatePositionInScene ();
-			AvoidBorders ();
 			
-			m_rotation = Vector3.RotationBetween( previousLoc, m_loc );
-			
-			//render();
 		}
 
 		/// <summary>
@@ -221,8 +222,40 @@ namespace Flocking
 		/// if we get too close wrap us around 
 		/// CHANGE THIS to navigate away from whatever it is we are too close to
 		/// </summary>
-		void AvoidBorders ()
+		void AvoidObstacles ()
 		{
+			//look tolerance metres ahead
+			Vector3 normVel = Vector3.Normalize(m_vel);
+			Vector3 inFront = m_loc + Vector3.Multiply(normVel, m_tolerance);
+			if( m_flowMap.WouldHitObstacle( m_loc, inFront ) ) {
+				AdjustVelocityToAvoidObstacles ();
+	
+			}
+		}
+
+		void AdjustVelocityToAvoidObstacles ()
+		{
+			for( int i = 1; i < 5; i++ ) {
+				Vector3 normVel = Vector3.Normalize(m_vel);
+				int xDelta = m_rndnums.Next (-i, i);
+				int yDelta = m_rndnums.Next (-i, i);
+				int zDelta = m_rndnums.Next (-i, i);
+				normVel.X += xDelta;
+				normVel.Y += yDelta;
+				normVel.Z += zDelta;
+				Vector3 inFront = m_loc + Vector3.Multiply(normVel, m_tolerance);
+				if( !m_flowMap.WouldHitObstacle( m_loc, inFront ) ) {
+					m_vel.X += xDelta;
+					m_vel.Y += yDelta;
+					m_vel.Z += zDelta;
+					//m_log.Info("avoided");
+					return;
+				}
+			}
+			//m_log.Info("didn't avoid");
+			// try increaing our acceleration
+			// or try decreasing our acceleration
+			// or turn around - coz where we came from was OK
 			if (m_loc.X < 5 || m_loc.X > 250)
 				m_vel.X = -m_vel.X;
 			if (m_loc.Y < 5 || m_loc.Y > 250)

Flocking/FlockingModel.cs

@@ -32,39 +32,47 @@ namespace Flocking
 {
 	public class FlockingModel
 	{
-        private  List<Boid> flock = new List<Boid>();
+        private List<Boid> m_flock = new List<Boid>();
+		private FlowMap m_flowMap;
 		
-		private int m_xRange = 200;
+		private Random m_rnd = new Random(Environment.TickCount);
-		private int m_yRange = 200;
-		private int m_zRange = 200;
 		
 		public int Size {
-			get {return flock.Count;}
+			get {return m_flock.Count;}
 			set {
-				if( value < flock.Count ) {
+				if( value < m_flock.Count ) {
-					flock.RemoveRange( 0, flock.Count - value );
+					m_flock.RemoveRange( 0, m_flock.Count - value );
-				} else while( value > flock.Count ) {
+				} else while( value > m_flock.Count ) {
-					AddBoid( "boid"+flock.Count);	
+					AddBoid( "boid"+m_flock.Count);	
 				}
 			}
 		}
 
 		void AddBoid (string name)
 		{
-			Boid boid = new Boid (name, 3.0f, 0.05f);
+			Boid boid = new Boid (name, 3.0f, 0.05f, m_flowMap);
-			boid.Location = new Vector3 (m_xRange / 2f, m_yRange / 2f, m_zRange / 2f);
+			
-			flock.Add (boid);
+			// find an initial random location for this Boid
+			// somewhere not within an obstacle
+			int xInit = m_rnd.Next(m_flowMap.LengthX);
+			int yInit = m_rnd.Next(m_flowMap.LengthY);
+			int zInit = m_rnd.Next(m_flowMap.LengthZ);
+			
+			while( m_flowMap.IsWithinObstacle( xInit, yInit, zInit ) ){
+				xInit = m_rnd.Next(m_flowMap.LengthX);
+				yInit = m_rnd.Next(m_flowMap.LengthY);
+				zInit = m_rnd.Next(m_flowMap.LengthZ);
+			}
+				
+			boid.Location = new Vector3 (Convert.ToSingle(xInit), Convert.ToSingle(yInit), Convert.ToSingle(zInit));
+			m_flock.Add (boid);
 		}
 						
 						
 				
-		public void Initialise (int num, int xRange, int yRange, int zRange)
+		public void Initialise (int num, FlowMap flowMap)
 		{
-			m_xRange = xRange;
+			m_flowMap = flowMap;			
-			m_yRange = yRange;
-			m_zRange = zRange;
-			
-			//TODO: fill in the initial Flock array properly
   			for (int i = 0; i < num; i++) {
 				AddBoid ("boid"+i );
   			}
@@ -72,11 +80,11 @@ namespace Flocking
 
 		public List<Boid> UpdateFlockPos ()
 		{
-    		foreach (Boid b in flock) {
+    		foreach (Boid b in m_flock) {
-      			b.MoveInSceneRelativeToFlock(flock);  // Passing the entire list of boids to each boid individually
+      			b.MoveInSceneRelativeToFlock(m_flock);  // Passing the entire list of boids to each boid individually
     		}
 			
-			return flock;
+			return m_flock;
 		}
 	}
 }

Flocking/FlockingModule.cs

@@ -54,6 +54,7 @@ namespace Flocking
 		private int m_frameUpdateRate = 1;
 		private int m_chatChannel = 118;
 		private string m_boidPrim;
+		private int m_flockSize = 100;
 		private UUID m_owner;
 
 		#region IRegionModule Members
@@ -68,6 +69,7 @@ namespace Flocking
 			if (config != null) {
 				m_chatChannel = config.GetInt ("chat-channel", 118);
 				m_boidPrim = config.GetString ("boid-prim", "boidPrim");
+				m_flockSize = config.GetInt ("flock-size", 100);
 				
 				// we're in the config - so turn on this module
 				m_enabled = true;
@@ -96,8 +98,13 @@ namespace Flocking
 		public void RegionLoaded (Scene scene)
 		{
 			if (m_enabled) {
+				
+				//make a flow map for this scene
+				FlowMap flowMap = new FlowMap(scene );
+				flowMap.Initialise();
+				
 				// Generate initial flock values
-				m_model.Initialise (200, 255, 255, 255);
+				m_model.Initialise (m_flockSize, flowMap);
 				
 				// who is the owner for the flock in this region
 				m_owner = m_scene.RegionInfo.EstateSettings.EstateOwner;
@@ -153,7 +160,7 @@ namespace Flocking
 			string cmd = msg.Message.ToLower ();
 			
 			//stick ui in the args so we know to respond in world
-			//bit of a hack - but lets us us CommandDelegate inWorld
+			//bit of a hack - but lets us use CommandDelegate inWorld
 			string[] args = (cmd + " <ui>").Split (" ".ToCharArray ());
 			
 			if (cmd.StartsWith ("stop")) {

Flocking/FlowMap.cs

@@ -0,0 +1,158 @@
+using System;
+using OpenMetaverse;
+using OpenSim.Region.Framework.Scenes;
+
+namespace Flocking
+{
+	public class FlowMap
+	{
+		private Scene m_scene;
+		private float[,,] m_flowMap = new float[256,256,256];
+		
+		public FlowMap (Scene scene)
+		{
+			m_scene = scene;
+		}
+		
+		public int LengthX {
+			get {return 256;}
+		}
+		public int LengthY {
+			get {return 256;}
+		}
+		public int LengthZ {
+			get {return 256;}
+		}
+		
+		public void Initialise() {
+			//fill in the boundaries
+			for( int x = 0; x < 256; x++ ) {
+				for( int y = 0; y < 256; y++ ) {
+					m_flowMap[x,y,0] = 100f;
+					m_flowMap[x,y,255] = 100f;
+				}
+			}
+			for( int x = 0; x < 256; x++ ) {
+				for( int z = 0; z < 256; z++ ) {
+					m_flowMap[x,0,z] = 100f;
+					m_flowMap[x,255,z] = 100f;
+				}
+			}
+			for( int y = 0; y < 256; y++ ) {
+				for( int z = 0; z < 256; z++ ) {
+					m_flowMap[0,y,z] = 100f;
+					m_flowMap[255,y,z] = 100f;
+				}
+			}
+			
+			//fill in the terrain
+			for( int x = 0; x < 256; x++ ) {
+				for( int y = 0; y < 256; y++ ) {
+					int zMax = Convert.ToInt32(m_scene.GetGroundHeight( x, y ));
+					for( int z = 1; z < zMax; z++ ) {
+						m_flowMap[x,y,z] = 100f;
+					}
+				}
+			}
+			
+			// fill in the things
+			foreach( EntityBase entity in m_scene.GetEntities() ) {
+				if( entity is SceneObjectGroup ) {
+					SceneObjectGroup sog = (SceneObjectGroup)entity;
+					
+					//todo: ignore phantom
+					float fmaxX, fminX, fmaxY, fminY, fmaxZ, fminZ;
+					int maxX, minX, maxY, minY, maxZ, minZ;
+					sog.GetAxisAlignedBoundingBoxRaw( out fminX, out fmaxX, out fminY, out fmaxY, out fminZ, out fmaxZ );
+					
+					minX = Convert.ToInt32(fminX);
+					maxX = Convert.ToInt32(fmaxX);
+					minY = Convert.ToInt32(fminY);
+					maxY = Convert.ToInt32(fmaxX);
+					minZ = Convert.ToInt32(fminZ);
+					maxZ = Convert.ToInt32(fmaxZ);
+					
+					for( int x = minX; x < maxX; x++ ) {
+						for( int y = minY; y < maxY; y++ ) {
+							for( int z = minZ; z < maxZ; z++ ) {
+								m_flowMap[x,y,z] = 100f;
+							}
+						}
+					}
+				}
+			}
+		}
+
+		public bool WouldHitObstacle (Vector3 currPos, Vector3 targetPos)
+		{
+			bool retVal = false;
+			//fail fast
+			if( IsOutOfBounds(targetPos) ) {
+				retVal = true;
+			} else if( IsWithinObstacle(targetPos) ) {
+				retVal = true;
+			} else if( IntersectsObstacle (currPos, targetPos) ) {
+				retVal = true;
+			}
+			
+			return retVal;
+		}
+		
+		public bool IsOutOfBounds(Vector3 targetPos) {
+			bool retVal = false;
+			if( targetPos.X < 5f ||
+				targetPos.X > 250f ||
+				targetPos.Y < 5f ||
+				targetPos.Y > 250f ||
+				targetPos.Z < 5f ||
+				targetPos.Z > 250f ) {
+				
+				retVal = true;
+			}
+			
+			return retVal;
+		}
+
+		public bool IntersectsObstacle (Vector3 currPos, Vector3 targetPos)
+		{
+			bool retVal = false;
+			// Ray trace the Vector and fail as soon as we hit something
+			Vector3 direction = targetPos - currPos;
+			float length = direction.Length();
+			// check every metre
+			for( float i = 1f; i < length; i += 1f ) {
+				Vector3 rayPos = currPos + ( direction * i );
+				//give up if we go OOB on this ray
+				if( IsOutOfBounds( rayPos ) ){ 
+					retVal = true;
+					break;
+				}
+				else if( IsWithinObstacle( rayPos ) ) {
+					retVal = true;
+					break;
+				}
+			}
+			
+			return retVal;
+		}
+		
+		public bool IsWithinObstacle( Vector3 targetPos ) {
+			return IsWithinObstacle(Convert.ToInt32(targetPos.X), Convert.ToInt32(targetPos.Y),Convert.ToInt32(targetPos.Z));
+		}
+		
+		public bool IsWithinObstacle( int x, int y, int z ) {
+			bool retVal = false;
+			if( x > LengthX || y > LengthY || z > LengthZ ) {
+				retVal = true;
+			} else if( x < 0 || y < 0 || z < 0 ) {
+				retVal = true;
+			} else if (m_flowMap[x,y,z] > 50f) {
+				retVal = true;
+			}
+			return retVal;	
+		}
+	}
+	
+	
+}
+