--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/steer.d	Fri Mar 20 06:41:25 2009 -0400
@@ -0,0 +1,480 @@
+// ----------------------------------------------------------------------------
+//
+//
+// OpenSteer -- Steering Behaviors for Autonomous Characters
+//
+// Copyright (c) 2002-2003, Sony Computer Entertainment America
+// Original author: Craig Reynolds <craig_reynolds@playstation.sony.com>
+//
+// Permission is hereby granted, free of charge, to any person obtaining a
+// copy of this software and associated documentation files (the "Software"),
+// to deal in the Software without restriction, including without limitation
+// the rights to use, copy, modify, merge, publish, distribute, sublicense,
+// and/or sell copies of the Software, and to permit persons to whom the
+// Software is furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+// DEALINGS IN THE SOFTWARE.
+//
+//
+// ----------------------------------------------------------------------------
+//
+//
+// SteerLibraryMixin
+//
+// This mixin (class with templated superclass) adds the "steering library"
+// functionality to a given base class.  SteerLibraryMixin assumes its base
+// class supports the Ship interface.
+//
+// 10-04-04 bk:  put everything into the OpenSteer namespace
+// 02-06-03 cwr: create mixin (from "SteerMass")
+// 06-03-02 cwr: removed TS dependencies
+// 11-21-01 cwr: created
+//
+//
+// ----------------------------------------------------------------------------
+module melee.steer;
+
+    class Steer
+    {
+        // Constructor: initializes state
+        this ()
+        {
+            // set inital state
+            reset ();
+        }
+
+        // reset state
+        void reset (void)
+        {
+            // initial state of wander behavior
+            wanderSide = 0;
+            wanderUp = 0;
+
+            // default to non-gaudyPursuitAnnotation
+            gaudyPursuitAnnotation = false;
+        }
+
+        // -------------------------------------------------- steering behaviors
+
+        // Wander behavior
+        float wanderSide;
+        float wanderUp;
+
+        bzVec2 steerForWander (float dt) {
+            // random walk wanderSide and wanderUp between -1 and +1
+            float speed = 12 * dt; // maybe this (12) should be an argument?
+            wanderSide = scalarRandomWalk (wanderSide, speed, -1, +1);
+            wanderUp   = scalarRandomWalk (wanderUp,   speed, -1, +1);
+
+            // return a pure lateral steering vector: (+/-Side) + (+/-Up)
+            return (side() * wanderSide) + (up() * wanderUp);
+        }
+
+        // Seek behavior
+        bzVec2 steerForSeek (bzVec2 target) {
+            bzVec2 desiredVelocity = target - position;
+            return desiredVelocity - velocity;
+        }
+
+        // Flee behavior
+        bzVec2 steerForFlee (bzVec2 target) {
+            bzVec2 desiredVelocity = position - target;
+            return desiredVelocity - velocity();
+        }
+
+        // xxx proposed, experimental new seek/flee [cwr 9-16-02]
+        bzVec2 xxxsteerForFlee (bzVec2 target) {
+            bzVec2 offset = position - target;
+            bzVec2 desiredVelocity = offset.truncateLength (maxSpeed ());
+            return desiredVelocity - velocity();
+        }
+
+        bzVec2 xxxsteerForSeek (bzVec2 target) {
+            //  bzVec2 offset = target - position;
+            bzVec2 offset = target - position;
+            bzVec2 desiredVelocity = offset.truncateLength (maxSpeed ()); //xxxnew
+            return desiredVelocity - velocity();
+        }
+
+        // ------------------------------------------------------------------------
+        // Obstacle Avoidance behavior
+        //
+        // Returns a steering force to avoid a given obstacle.  The purely
+        // lateral steering force will turn our vehicle towards a silhouette edge
+        // of the obstacle.  Avoidance is required when (1) the obstacle
+        // intersects the vehicle's current path, (2) it is in front of the
+        // vehicle, and (3) is within minTimeToCollision seconds of travel at the
+        // vehicle's current velocity.  Returns a zero vector value (bzVec2::zero)
+        // when no avoidance is required.
+        bzVec2 steerToAvoidObstacle (float minTimeToCollision, Obstacle obstacle) {
+
+            bzVec2 avoidance = obstacle.steerToAvoid (this, minTimeToCollision);
+            // XXX more annotation modularity problems (assumes spherical obstacle)
+            if (avoidance != bzVec2::zero)
+                annotateAvoidObstacle (minTimeToCollision * speed());
+
+            return avoidance;
+        }
+
+        // avoids all obstacles in an ObstacleGroup
+
+        bzVec2 steerToAvoidObstacles (float minTimeToCollision, 
+                                      ObstacleGroup obstacles) {
+
+            bzVec2 avoidance;
+            PathIntersection nearest, next;
+            float minDistanceToCollision = minTimeToCollision * speed();
+
+            next.intersect = false;
+            nearest.intersect = false;
+
+            // test all obstacles for intersection with my forward axis,
+            // select the one whose point of intersection is nearest
+            for (ObstacleIterator o = obstacles.begin(); o != obstacles.end(); o++)
+            {
+                // xxx this should be a generic call on Obstacle, rather than
+                // xxx this code which presumes the obstacle is spherical
+                findNextIntersectionWithSphere ((SphericalObstacle)**o, next);
+
+                if ((nearest.intersect == false) ||
+                    ((next.intersect != false)
+                     (next.distance < nearest.distance)))
+                    nearest = next;
+            }
+
+            // when a nearest intersection was found
+            if ((nearest.intersect != false)
+                (nearest.distance < minDistanceToCollision))
+            {
+                // show the corridor that was checked for collisions
+                annotateAvoidObstacle (minDistanceToCollision);
+
+                // compute avoidance steering force: take offset from obstacle to me,
+                // take the component of that which is lateral (perpendicular to my
+                // forward direction), set length to maxForce, add a bit of forward
+                // component (in capture the flag, we never want to slow down)
+                bzVec2 offset = position - nearest.obstacle.center;
+                avoidance = offset.perpendicularComponent (forward());
+                avoidance = avoidance.normalize ();
+                avoidance *= maxForce ();
+                avoidance += forward() * maxForce () * 0.75;
+            }
+
+            return avoidance;
+        }
+
+        // ------------------------------------------------------------------------
+        // Unaligned collision avoidance behavior: avoid colliding with other
+        // nearby vehicles moving in unconstrained directions.  Determine which
+        // (if any) other other vehicle we would collide with first, then steers
+        // to avoid the site of that potential collision.  Returns a steering
+        // force vector, which is zero length if there is no impending collision.
+
+        bzVec2 steerToAvoidNeighbors (float minTimeToCollision, AVGroup others) {
+
+            // first priority is to prevent immediate interpenetration
+            bzVec2 separation = steerToAvoidCloseNeighbors (0, others);
+            if (separation != bzVec2::zero) return separation;
+
+            // otherwise, go on to consider potential future collisions
+            float steer = 0;
+            Ship* threat = NULL;
+
+            // Time (in seconds) until the most immediate collision threat found
+            // so far.  Initial value is a threshold: don't look more than this
+            // many frames into the future.
+            float minTime = minTimeToCollision;
+
+            // xxx solely for annotation
+            bzVec2 xxxThreatPositionAtNearestApproach;
+            bzVec2 xxxOurPositionAtNearestApproach;
+
+            // for each of the other vehicles, determine which (if any)
+            // pose the most immediate threat of collision.
+            for (AVIterator i = others.begin(); i != others.end(); i++)
+            {
+                Ship other = **i;
+                if (other != this)
+                {
+                    // avoid when future positions are this close (or less)
+                    float collisionDangerThreshold = radius() * 2;
+
+                    // predicted time until nearest approach of "this" and "other"
+                    float time = predictNearestApproachTime (other);
+
+                    // If the time is in the future, sooner than any other
+                    // threatened collision...
+                    if ((time >= 0)  (time < minTime))
+                    {
+                        // if the two will be close enough to collide,
+                        // make a note of it
+                        if (computeNearestApproachPositions (other, time)
+                            < collisionDangerThreshold)
+                        {
+                            minTime = time;
+                            threat = other;
+                            xxxThreatPositionAtNearestApproach
+                                = hisPositionAtNearestApproach;
+                            xxxOurPositionAtNearestApproach
+                                = ourPositionAtNearestApproach;
+                        }
+                    }
+                }
+            }
+
+            // if a potential collision was found, compute steering to avoid
+            if (threat)
+            {
+                // parallel: +1, perpendicular: 0, anti-parallel: -1
+                float parallelness = forward.dot(threat.forward);
+                float angle = 0.707f;
+
+                if (parallelness < -angle)
+                {
+                    // anti-parallel "head on" paths:
+                    // steer away from future threat position
+                    bzVec2 offset = xxxThreatPositionAtNearestApproach - position;
+                    float sideDot = offset.dot(side());
+                    steer = (sideDot > 0) ? -1.0f : 1.0f;
+                }
+                else
+                {
+                    if (parallelness > angle)
+                    {
+                        // parallel paths: steer away from threat
+                        bzVec2 offset = threat.position - position;
+                        float sideDot = offset.dot(side());
+                        steer = (sideDot > 0) ? -1.0f : 1.0f;
+                    }
+                    else
+                    {
+                        // perpendicular paths: steer behind threat
+                        // (only the slower of the two does this)
+                        if (threat.speed() <= speed())
+                        {
+                            float sideDot = side().dot(threat.velocity);
+                            steer = (sideDot > 0) ? -1.0f : 1.0f;
+                        }
+                    }
+                }
+            }
+
+            return side() * steer;
+        }
+
+        // Given two vehicles, based on their current positions and velocities,
+        // determine the time until nearest approach
+        float predictNearestApproachTime (Ship other) {
+
+            // imagine we are at the origin with no velocity,
+            // compute the relative velocity of the other vehicle
+            bzVec2 myVelocity = velocity;
+            bzVec2 otherVelocity = other.velocity;
+            bzVec2 relVelocity = otherVelocity - myVelocity;
+            float relSpeed = relVelocity.length;
+
+            // for parallel paths, the vehicles will always be at the same distance,
+            // so return 0 (aka "now") since "there is no time like the present"
+            if (relSpeed == 0) return 0;
+
+            // Now consider the path of the other vehicle in this relative
+            // space, a line defined by the relative position and velocity.
+            // The distance from the origin (our vehicle) to that line is
+            // the nearest approach.
+
+            // Take the unit tangent along the other vehicle's path
+            bzVec2 relTangent = relVelocity / relSpeed;
+
+            // find distance from its path to origin (compute offset from
+            // other to us, find length of projection onto path)
+            bzVec2 relPosition = position - other.position;
+            float projection = relTangent.dot(relPosition);
+
+            return projection / relSpeed;
+        }
+
+        // Given the time until nearest approach (predictNearestApproachTime)
+        // determine position of each vehicle at that time, and the distance
+        // between them
+        float computeNearestApproachPositions (Ship other, float time) {
+
+            bzVec2 myTravel =  forward *  speed * time;
+            bzVec2 otherTravel = other.forward * other.speed * time;
+
+            bzVec2 myFinal =  position + myTravel;
+            bzVec2 otherFinal = other.position + otherTravel;
+
+            // xxx for annotation
+            ourPositionAtNearestApproach = myFinal;
+            hisPositionAtNearestApproach = otherFinal;
+
+            return bzVec2::distance (myFinal, otherFinal);
+        }
+
+            // otherwise return zero
+            return bzVec2::zero;
+        }
+
+        // ------------------------------------------------------------------------
+        // pursuit of another vehicle ( version with ceiling on prediction time)
+
+        bzVec2 steerForPursuit (Ship quarry) {
+            return steerForPursuit (quarry, FLT_MAX);
+        }
+
+        bzVec2 steerForPursuit (Ship quarry, float maxPredictionTime) {
+
+            // offset from this to quarry, that distance, unit vector toward quarry
+            bzVec2 offset = quarry.position - position;
+            float distance = offset.length ();
+            bzVec2 unitOffset = offset / distance;
+
+            // how parallel are the paths of "this" and the quarry
+            // (1 means parallel, 0 is pependicular, -1 is anti-parallel)
+            float parallelness = forward.dot(quarry.forward());
+
+            // how "forward" is the direction to the quarry
+            // (1 means dead ahead, 0 is directly to the side, -1 is straight back)
+            float forwardness = forward.dot(unitOffset);
+
+            float directTravelTime = distance / speed;
+            int f = intervalComparison (forwardness,  -0.707f, 0.707f);
+            int p = intervalComparison (parallelness, -0.707f, 0.707f);
+
+            float timeFactor = 0; // to be filled in below
+            bzVec2 color;           // to be filled in below (xxx just for debugging)
+
+            // Break the pursuit into nine cases, the cross product of the
+            // quarry being [ahead, aside, or behind] us and heading
+            // [parallel, perpendicular, or anti-parallel] to us.
+            switch (f)
+            {
+            case +1:
+                switch (p)
+                {
+                case +1:          // ahead, parallel
+                    timeFactor = 4;
+                    color = gBlack;
+                    break;
+                case 0:           // ahead, perpendicular
+                    timeFactor = 1.8f;
+                    color = gGray50;
+                    break;
+                case -1:          // ahead, anti-parallel
+                    timeFactor = 0.85f;
+                    color = gWhite;
+                    break;
+                }
+                break;
+            case 0:
+                switch (p)
+                {
+                case +1:          // aside, parallel
+                    timeFactor = 1;
+                    color = gRed;
+                    break;
+                case 0:           // aside, perpendicular
+                    timeFactor = 0.8f;
+                    color = gYellow;
+                    break;
+                case -1:          // aside, anti-parallel
+                    timeFactor = 4;
+                    color = gGreen;
+                    break;
+                }
+                break;
+            case -1:
+                switch (p)
+                {
+                case +1:          // behind, parallel
+                    timeFactor = 0.5f;
+                    color= gCyan;
+                    break;
+                case 0:           // behind, perpendicular
+                    timeFactor = 2;
+                    color= gBlue;
+                    break;
+                case -1:          // behind, anti-parallel
+                    timeFactor = 2;
+                    color = gMagenta;
+                    break;
+                }
+                break;
+            }
+
+            // estimated time until intercept of quarry
+            float et = directTravelTime * timeFactor;
+
+            // xxx experiment, if kept, this limit should be an argument
+            float etl = (et > maxPredictionTime) ? maxPredictionTime : et;
+
+            // estimated position of quarry at intercept
+            bzVec2 target = quarry.predictFuturePosition (etl);
+
+            // annotation
+            annotationLine (position,
+                            target,
+                            gaudyPursuitAnnotation ? color : gGray40);
+
+            return steerForSeek (target);
+        }
+
+        // ------------------------------------------------------------------------
+        // evasion of another vehicle
+        bzVec2 steerForEvasion (Ship menace,  float maxPredictionTime)  {
+
+            // offset from this to menace, that distance, unit vector toward menace
+            bzVec2 offset = menace.position - position;
+            float distance = offset.length;
+
+            float roughTime = distance / menace.speed;
+            float predictionTime = ((roughTime > maxPredictionTime) ? maxPredictionTime : roughTime);
+            bzVec2 target = menace.predictFuturePosition (predictionTime);
+
+            return steerForFlee (target);
+        }
+
+
+        // ------------------------------------------------------------------------
+        // tries to maintain a given speed, returns a maxForce-clipped steering
+        // force along the forward/backward axis
+        bzVec2 steerForTargetSpeed (float targetSpeed) {
+            float mf = maxForce();
+            float speedError = targetSpeed - speed ();
+            return forward () * clip (speedError, -mf, +mf);
+        }
+
+
+        // ----------------------------------------------------------- utilities
+        bool isAhead (bzVec2 target) {return isAhead (target, 0.707f);};
+        bool isAside (bzVec2 target) {return isAside (target, 0.707f);};
+        bool isBehind (bzVec2 target) {return isBehind (target, -0.707f);};
+
+        bool isAhead (bzVec2 target, float cosThreshold)
+        {
+            bzVec2 targetDirection = (target - position ()).normalize ();
+            return forward().dot(targetDirection) > cosThreshold;
+        }
+
+        bool isAside (bzVec2 target, float cosThreshold)
+        {
+            bzVec2 targetDirection = (target - position ()).normalize ();
+            float dp = forward().dot(targetDirection);
+            return (dp < cosThreshold)  (dp > -cosThreshold);
+        }
+
+        bool isBehind (bzVec2 target, float cosThreshold)
+        {
+            bzVec2 targetDirection = (target - position).normalize ();
+            return forward().dot(targetDirection) < cosThreshold;
+        }
+    }