// get a value based on a position in 3d world space
	    private bool GetMapValue(Vector3 point)
		{
			Vector3 local = point - Center;
            local.Y = 0;
			Vector3 localXZ = local;

			float hxs = XSize / 2;
			float hzs = ZSize / 2;

			float x = localXZ.X;
			float z = localXZ.Z;

			bool isOut = (x > +hxs) || (x < -hxs) || (z > +hzs) || (z < -hzs);

			if (isOut)
			{
				return _outsideValue;
			}
			else
			{
                int i = (int)Utilities.RemapInterval(x, -hxs, hxs, 0.0f, Resolution);
                int j = (int)Utilities.RemapInterval(z, -hzs, hzs, 0.0f, Resolution);
				return GetMapBit(i, j);
			}
		}

		// ------------------------------------------------------------------------
		// drawing of lines, circles and (filled) disks to annotate steering
		// behaviors.  When called during OpenSteerDemo's simulation update phase,
		// these functions call a "deferred draw" routine which buffer the
		// arguments for use during the redraw phase.
		//
		// note: "circle" means unfilled
		//       "disk" means filled
		//       "XZ" means on a plane parallel to the X and Z axes (perp to Y)
		//       "3d" means the circle is perpendicular to the given "axis"
		//       "segments" is the number of line segments used to draw the circle

		// draw an opaque colored line segment between two locations in space
		public void Line(Vector3 startPoint, Vector3 endPoint, Vector3 color, float opacity = 1)
		{
			if (_isEnabled && Drawer != null)
			{
				Drawer.Line(startPoint, endPoint, new Color(new Microsoft.Xna.Framework.Vector3(color.X, color.Y, color.Z)), opacity);
			}
		}

	    public override void Redraw(float currentTime, float elapsedTime)
		{
			// selected vehicle (user can mouse click to select another)
			IVehicle selected = Demo.SelectedVehicle;

			// vehicle nearest mouse (to be highlighted)
			IVehicle nearMouse = Demo.VehicleNearestToMouse();

			// update camera
			Demo.UpdateCamera(elapsedTime, selected);

			// draw "ground plane"
			Demo.GridUtility(selected.Position);

			// update, draw and annotate each agent
			foreach (LowSpeedTurn agent in _all)
			{
			    agent.Draw();

			    // display speed near agent's screen position
			    Color textColor = new Color(new Vector3(0.8f, 0.8f, 1.0f).ToXna());
			    Vector3 textOffset = new Vector3(0, 0.25f, 0);
			    Vector3 textPosition = agent.Position + textOffset;
			    String annote = String.Format("{0:0.00}", agent.Speed);
			    Drawing.Draw2dTextAt3dLocation(annote, textPosition, textColor);
			}

			// highlight vehicle nearest mouse
			Demo.HighlightVehicleUtility(nearMouse);
		}

 /// <summary>
 /// Cleans up the pair tester.
 /// </summary>
 public override void CleanUp()
 {
     convex = null;
     state = CollisionState.Plane;
     escapeAttempts = 0;
     localSeparatingAxis = new System.Numerics.Vector3();
     Updated = false;
 }

 ///<summary>
 /// Constructs a triangle shape from cached data.
 ///</summary>
 ///<param name="vA">First vertex in the triangle.</param>
 ///<param name="vB">Second vertex in the triangle.</param>
 ///<param name="vC">Third vertex in the triangle.</param>
 /// <param name="description">Cached information about the shape. Assumed to be correct; no extra processing or validation is performed.</param>
 public TriangleShape(System.Numerics.Vector3 vA, System.Numerics.Vector3 vB, System.Numerics.Vector3 vC, ConvexShapeDescription description)
 {
     //Recenter.  Convexes should contain the origin.
     var center = (vA + vB + vC) / 3;
     this.vA = vA - center;
     this.vB = vB - center;
     this.vC = vC - center;
     UpdateConvexShapeInfo(description);
 }

 ///<summary>
 /// Cleans up the pair tester.
 ///</summary>
 public void CleanUp()
 {
     state = CollisionState.Separated;
     previousState = CollisionState.Separated;
     cachedSimplex = new CachedSimplex();
     localSeparatingAxis = new System.Numerics.Vector3();
     collidableA = null;
     collidableB = null;
 }

		// reset state
		public override void Reset()
		{
			base.Reset(); // reset the vehicle 
			Speed = 0.0f;         // speed along Forward direction.

			Position = new Vector3(0, 0, 0);
			if (_trail == null) _trail = new Trail(100, 6000);
			_trail.Clear();    // prevent long streaks due to teleportation 
		}

 private void CreateDatabase()
 {
     Vector3 center = Vector3.Zero;
     const float DIV = 10.0f;
     Vector3 divisions = new Vector3(DIV, DIV, DIV);
     const float DIAMETER = Fighter.WORLD_RADIUS * 2;
     Vector3 dimensions = new Vector3(DIAMETER, DIAMETER, DIAMETER);
     _pd = new LocalityQueryProximityDatabase<IVehicle>(center, dimensions, divisions);
 }

		public void AnnotateAvoidNeighbor(IVehicle threat, Vector3 ourFuture, Vector3 threatFuture)
		{
			Color green = new Color((byte)(255.0f * 0.15f), (byte)(255.0f * 0.6f), 0);

            annotation.Line(Position, ourFuture, green.ToVector3().FromXna());
            annotation.Line(threat.Position, threatFuture, green.ToVector3().FromXna());
            annotation.Line(ourFuture, threatFuture, Color.Red.ToVector3().FromXna());
            annotation.CircleXZ(Radius, ourFuture, green.ToVector3().FromXna(), 12);
            annotation.CircleXZ(Radius, threatFuture, green.ToVector3().FromXna(), 12);
		}

		public void Draw()
		{
			Vector3 b = new Vector3(_min.X, 0, _max.Z);
			Vector3 c = new Vector3(_max.X, 0, _min.Z);
			Color color = new Color(255, 255, 0);
			Drawing.DrawLineAlpha(_min, b, color, 1.0f);
			Drawing.DrawLineAlpha(b, _max, color, 1.0f);
			Drawing.DrawLineAlpha(_max, c, color, 1.0f);
			Drawing.DrawLineAlpha(c, _min, color, 1.0f);
		}

		public void DrawHomeBase()
		{
			Vector3 up = new Vector3(0, 0.01f, 0);
			Color atColor = new Color((byte)(255.0f * 0.3f), (byte)(255.0f * 0.3f), (byte)(255.0f * 0.5f));
			Color noColor = Color.Gray;
			bool reached = Plugin.CtfSeeker.State == SeekerState.AtGoal;
			Color baseColor = (reached ? atColor : noColor);
            Drawing.DrawXZDisk(_baseRadius, Globals.HomeBaseCenter, baseColor, 40);
            Drawing.DrawXZDisk(_baseRadius / 15, Globals.HomeBaseCenter + up, Color.Black, 20);
		}

		public static void AddToBuffer(Vector3 s, Vector3 e, Color c)
		{
		    if (_index >= _deferredLines.Count)
		        _deferredLines.Add(new DeferredLine());

            _deferredLines[_index]._startPoint = s;
            _deferredLines[_index]._endPoint = e;
            _deferredLines[_index]._color = c;

            _index++;
		}

		/// <summary>
		/// Records a position for the current time, called once per update.
		/// </summary>
		/// <param name="currentTime"></param>
		/// <param name="position"></param>
		public void Record(float currentTime, Vector3 position)
		{
			float timeSinceLastTrailSample = currentTime - _lastSampleTime;
			if (timeSinceLastTrailSample > _sampleInterval)
			{
				_currentIndex = (_currentIndex + 1) % _vertices.Length;
				_vertices[_currentIndex] = position;
				_dottedPhase = (_dottedPhase + 1) % 2;
				bool tick = (Math.Floor(currentTime) > Math.Floor(_lastSampleTime));
				_flags[_currentIndex] = (byte)(_dottedPhase | (tick ? 2 : 0));
				_lastSampleTime = currentTime;
			}
			_currentPosition = position;
		}

		public TerrainMap(Vector3 c, float x, float z, int r)
		{
			Center = c;
			XSize = x;
			ZSize = z;
			Resolution = r;
			_outsideValue = false;

			_map = new bool[Resolution * Resolution];
			for (int i = 0; i < Resolution * Resolution; i++)
			{
				_map[i] = false;
			}
		}

        ///<summary>
        /// Computes the expansion of the minkowski sum due to margins in a given direction.
        ///</summary>
        ///<param name="marginA">First margin.</param>
        ///<param name="marginB">Second margin.</param>
        ///<param name="direction">Extreme point direction.</param>
        ///<param name="contribution">Margin contribution to the extreme point.</param>
        public static void ExpandMinkowskiSum(float marginA, float marginB, ref System.Numerics.Vector3 direction, out System.Numerics.Vector3 contribution)
        {
            float lengthSquared = direction.LengthSquared();
            if (lengthSquared > Toolbox.Epsilon)
            {
                //The contribution to the minkowski sum by the margin is:
                //direction * marginA - (-direction) * marginB.
                Vector3Ex.Multiply(ref direction, (marginA + marginB) / (float)Math.Sqrt(lengthSquared), out contribution);

            }
            else
            {
                contribution = new System.Numerics.Vector3();
            }
        }

		// reset state
		public override void Reset()
		{
			base.Reset(); // reset the vehicle 
			Speed = 0.0f;         // speed along Forward direction.

			// Place me on my part of the field, looking at oponnents goal
			Position = new Vector3(_imTeamA ? RandomHelpers.Random() * 20 : -RandomHelpers.Random() * 20, 0, (RandomHelpers.Random() - 0.5f) * 20);
			if (_myID < 9)
			{
				Position = _imTeamA ? (Globals.PlayerPosition[_myID]) : (new Vector3(-Globals.PlayerPosition[_myID].X, Globals.PlayerPosition[_myID].Y, Globals.PlayerPosition[_myID].Z));
			}
			_home = Position;

			if (_trail == null) _trail = new Trail(10, 60);
			_trail.Clear();    // prevent long streaks due to teleportation 
		}

        /// <summary>
        /// Gets the bounding box of the shape given a transform.
        /// </summary>
        /// <param name="shapeTransform">Transform to use.</param>
        /// <param name="boundingBox">Bounding box of the transformed shape.</param>
        public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox)
        {
            #if !WINDOWS
            boundingBox = new BoundingBox();
            #endif
            Matrix3x3 o;
            Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
            //Sample the local directions from the orientation matrix, implicitly transposed.

            System.Numerics.Vector3 right;
            var direction = new System.Numerics.Vector3(o.M11, o.M21, o.M31);
            GetLocalExtremePointWithoutMargin(ref direction, out right);

            System.Numerics.Vector3 left;
            direction = new System.Numerics.Vector3(-o.M11, -o.M21, -o.M31);
            GetLocalExtremePointWithoutMargin(ref direction, out left);

            System.Numerics.Vector3 up;
            direction = new System.Numerics.Vector3(o.M12, o.M22, o.M32);
            GetLocalExtremePointWithoutMargin(ref direction, out up);

            System.Numerics.Vector3 down;
            direction = new System.Numerics.Vector3(-o.M12, -o.M22, -o.M32);
            GetLocalExtremePointWithoutMargin(ref direction, out down);

            System.Numerics.Vector3 backward;
            direction = new System.Numerics.Vector3(o.M13, o.M23, o.M33);
            GetLocalExtremePointWithoutMargin(ref direction, out backward);

            System.Numerics.Vector3 forward;
            direction = new System.Numerics.Vector3(-o.M13, -o.M23, -o.M33);
            GetLocalExtremePointWithoutMargin(ref direction, out forward);

            //Rather than transforming each axis independently (and doing three times as many operations as required), just get the 6 required values directly.
            System.Numerics.Vector3 positive, negative;
            TransformLocalExtremePoints(ref right, ref up, ref backward, ref o, out positive);
            TransformLocalExtremePoints(ref left, ref down, ref forward, ref o, out negative);

            //The positive and negative vectors represent the X, Y and Z coordinates of the extreme points in world space along the world space axes.
            boundingBox.Max.X = shapeTransform.Position.X + positive.X + collisionMargin;
            boundingBox.Max.Y = shapeTransform.Position.Y + positive.Y + collisionMargin;
            boundingBox.Max.Z = shapeTransform.Position.Z + positive.Z + collisionMargin;

            boundingBox.Min.X = shapeTransform.Position.X + negative.X - collisionMargin;
            boundingBox.Min.Y = shapeTransform.Position.Y + negative.Y - collisionMargin;
            boundingBox.Min.Z = shapeTransform.Position.Z + negative.Z - collisionMargin;
        }

		public void xxxDrawMap()
		{
			float xs = XSize / Resolution;
			float zs = ZSize / Resolution;
			Vector3 alongRow = new Vector3(xs, 0, 0);
			Vector3 nextRow = new Vector3(-XSize, 0, zs);
			Vector3 g = new Vector3((XSize - xs) / -2, 0, (ZSize - zs) / -2);
			g += Center;
			for (int j = 0; j < Resolution; j++)
			{
				for (int i = 0; i < Resolution; i++)
				{
					if (GetMapBit(i, j))
					{
						// spikes
						// Vector3 spikeTop (0, 5.0f, 0);
						// drawLine (g, g+spikeTop, gWhite);

						// squares
						const float ROCK_HEIGHT = 0;
						Vector3 v1 = new Vector3(+xs / 2, ROCK_HEIGHT, +zs / 2);
						Vector3 v2 = new Vector3(+xs / 2, ROCK_HEIGHT, -zs / 2);
						Vector3 v3 = new Vector3(-xs / 2, ROCK_HEIGHT, -zs / 2);
						Vector3 v4 = new Vector3(-xs / 2, ROCK_HEIGHT, +zs / 2);
						// Vector3 redRockColor (0.6f, 0.1f, 0.0f);
						Color orangeRockColor = new Color((byte)(255.0f * 0.5f), (byte)(255.0f * 0.2f), (byte)(255.0f * 0.0f));
						Drawing.DrawQuadrangle(g + v1, g + v2, g + v3, g + v4, orangeRockColor);

						// pyramids
						// Vector3 top (0, xs/2, 0);
						// Vector3 redRockColor (0.6f, 0.1f, 0.0f);
						// Vector3 orangeRockColor (0.5f, 0.2f, 0.0f);
						// drawTriangle (g+v1, g+v2, g+top, redRockColor);
						// drawTriangle (g+v2, g+v3, g+top, orangeRockColor);
						// drawTriangle (g+v3, g+v4, g+top, redRockColor);
						// drawTriangle (g+v4, g+v1, g+top, orangeRockColor);
					}
					g += alongRow;
				}
				g += nextRow;
			}
		}

		// reset state
		public override void Reset()
		{
			// reset vehicle state
			base.Reset();

			// speed along Forward direction.
			Speed = _startSpeed;

			// initial position along X axis
			Position = new Vector3(_startX, 0, 0);

			// for next instance: step starting location
			_startX += 2;

			// for next instance: step speed
			_startSpeed += 0.15f;

			// 15 seconds and 150 points along the trail
			_trail = new Trail(15, 150);
		}

		// called when steerToFollowPath decides steering is required
		public void AnnotatePathFollowing(Vector3 future, Vector3 onPath, Vector3 target, float outside)
		{
			Color yellow = Color.Yellow;
			Color lightOrange = new Color((byte)(255.0f * 1.0f), (byte)(255.0f * 0.5f), 0);
			Color darkOrange = new Color((byte)(255.0f * 0.6f), (byte)(255.0f * 0.3f), 0);

			// draw line from our position to our predicted future position
            annotation.Line(Position, future, yellow.ToVector3().FromXna());

			// draw line from our position to our steering target on the path
            annotation.Line(Position, target, Color.Orange.ToVector3().FromXna());

			// draw a two-toned line between the future test point and its
			// projection onto the path, the change from dark to light color
			// indicates the boundary of the tube.
            Vector3 boundaryOffset = Vector3.Normalize(onPath - future);
            boundaryOffset *= outside;
			Vector3 onPathBoundary = future + boundaryOffset;
            annotation.Line(onPath, onPathBoundary, darkOrange.ToVector3().FromXna());
            annotation.Line(onPathBoundary, future, lightOrange.ToVector3().FromXna());
		}