public virtual void Update(float worldTime, Sim.Environment.Terrain t)
        {
            float floorPos = t.CalculateHeight(Position.X, Position.Z) + 20;

            if (currentWaypoint != null)
            {
                // the agent has a waypoint in the queue
                if (distanceToTravel <= 0)
                {
                    // the agent has reached or just passed its destination, so it uses the next waypoint
                    currentWaypoint = currentWaypoint.Next;
                    SetMovement();
                }
                else
                {
                    distanceToTravel -= speed;
                }
            }

            position = position + velocity;
            position.Y = (float)Math.Sin(worldTime * 0.006f + randomOffset)*4 + floorPos + 5;
            light.MoveTo(Position);

            if (Updated != null)
            {
                Updated(this, EventArgs.Empty);
                info = String.Format("- Color: {0}", light.Color);
            }
        }
        /// <summary>
        /// Creates and connects all nodes in the grid
        /// </summary>
        private void BuildGrid(Sim.Environment.Terrain terrain)
        {
            // create the nodes at their proper locations
            nodes = new Node[width * length];
            for (int z = 0; z < length; z++) 
            {
                for (int x = 0; x < width; x++) 
                {
                    int i = x + z * width;
                    float y = terrain.CalculateHeight(x * spacing, z * spacing);
                    nodes[i] = new Node(new Vector3(x * spacing, y, z * spacing));

                    if (y < 5) // underwater
                        nodes[i].State = NodeState.Invalid;
                    else if (GMath.AngleOfIncline(terrain.CalculateSurfaceNormal(x * spacing, z * spacing)) < 50)
                        nodes[i].State = NodeState.Invalid;
                }
            }

            // connect adjacent nodes
            for (int z = 0; z < length; z++)
            {
                for (int x = 0; x < width; x++)
                {
                    Node n = nodes[x + z * width];

                    // adjacent nodes exist in all 8 directions if the current node is part of the
                    // interior of the grid; if the node is on an edge, certain spots are empty
                    bool addN = z > 0;
                    bool addS = z < length - 1;
                    bool addW = x > 0;
                    bool addE = x < width - 1;

                    if (addN)
                    {
                        n.AdjacentNodes.Add(nodes[x + (z - 1) * width]);                    // N
                        if (addE) n.AdjacentNodes.Add(nodes[(x + 1) + (z - 1) * width]);    // NE
                        if (addW) n.AdjacentNodes.Add(nodes[(x - 1) + (z - 1) * width]);    // NW
                    }

                    if (addS)
                    {
                        n.AdjacentNodes.Add(nodes[x + (z + 1) * width]);                    // S
                        if (addE) n.AdjacentNodes.Add(nodes[(x + 1) + (z + 1) * width]);    // SE
                        if (addW) n.AdjacentNodes.Add(nodes[(x - 1) + (z + 1) * width]);    // SW
                    }

                    if (addE) n.AdjacentNodes.Add(nodes[(x + 1) + z * width]);              // E
                    if (addW) n.AdjacentNodes.Add(nodes[(x - 1) + z * width]);              // W
                }
            }
        }
        public void Update(Sim.Environment.Terrain t, List<PhysicsBall> pballs)
        {
            float floorHeight = t.CalculateHeight(position.X, position.Z) + radius;
            bool aboveTerrain = position.Y > floorHeight;

            if (aboveTerrain && !rising && !falling)
            {
                if (position.Y - floorHeight > 0.5f) // angle is great enough to slip
                    falling = true;
                else
                    position.Y = floorHeight;

            }

            if (position.Y < floorHeight)
                position.Y = floorHeight;

            if (falling)
            {
                diffuseColor = new Vector3(1, 1, 0);
                info = "Behavior: Falling";
                waitingToJump = false;
                waitToJumpTimer = 0;
                Velocity.Y -= gravity;
                if (position.Y <= floorHeight)
                {
                    Velocity.Y *= -bounceFriction;   // friction takes away some of the energy
                    if (Velocity.Y > 0.09f)
                    {
                        rising = true;
                        Vector3 bounceV = t.CalculateSurfaceNormal(position.X, position.Z) * bounceFriction;
                        Velocity.X += bounceV.X;
                        Velocity.Z += bounceV.Z;
                    }
                    else
                        Velocity.Y = 0;
                    falling = false;
                }
            }
            else if (rising)
            {
                diffuseColor = new Vector3(1, 1, 0);
                info = "Behavior: Rising";
                waitingToJump = false;
                waitToJumpTimer = 0;
                Velocity.Y -= gravity;
                if (Velocity.Y <= 0)
                {
                    rising = false;
                    falling = true;
                }
            }
            else if (Velocity.X == 0 && Velocity.Z == 0)
            {
                info = "Behavior: Waiting to jump";
                waitingToJump = true;
                diffuseColor = new Vector3(1, 0, 0);
            }

            float xzFriction;
            if (aboveTerrain)
                xzFriction = 0;
            else
                xzFriction = slowDown;

            if (Velocity.X < 0)
                Velocity.X = MathHelper.Clamp(Velocity.X + xzFriction, -maxSpeed, 0);
            else if (Velocity.X > 0)
                Velocity.X = MathHelper.Clamp(Velocity.X - xzFriction, 0, maxSpeed);

            if (Velocity.Z < 0)
                Velocity.Z = MathHelper.Clamp(Velocity.Z + xzFriction, -maxSpeed, 0);
            else if (Velocity.Z > 0)
                Velocity.Z = MathHelper.Clamp(Velocity.Z - xzFriction, 0, maxSpeed);

            // should only update if velocity is nonzero
            position += Velocity;

            Rotation.Z -= Velocity.X / radius;
            Rotation.X -= Velocity.Z / radius;

            if (waitingToJump)
            {
                waitToJumpTimer += 16;
                if (waitToJumpTimer > 1500)
                {
                    Vector3 tV = target.Position - position;
                    tV.Normalize();
                    Velocity += tV * new Random().Next(5);
                    info = "Behavior: Jumping!";
                }
            }

            if (Updated != null)
                Updated(this, EventArgs.Empty);
        }