Mercurial > projects > openmelee
view ai/steer.d @ 23:e79347dd38a3
steering
author | zzzzrrr <mason.green@gmail.com> |
---|---|
date | Thu, 26 Mar 2009 20:35:58 -0400 |
parents | 4fce5596d1f6 |
children | 441eb7672404 |
line wrap: on
line source
/* * Copyright (c) 2009, Mason Green (zzzzrrr) * http://www.dsource.org/projects/openmelee * Based on OpenSteer, Copyright (c) 2002-2003, Sony Computer Entertainment America * Original author: Craig Reynolds * * All rights reserved. * * Redistribution and use in source and binary forms, with or without modification, * are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright notice, * this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. * * Neither the name of the polygonal nor the names of its contributors may be * used to endorse or promote products derived from this software without specific * prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ module openmelee.ai.steer; import tango.io.Stdout : Stdout; import blaze.common.bzMath: bzDot, bzClamp, bzVec2; import blaze.collision.shapes.bzShape : bzShape; import tango.math.Math : sqrt; import blaze.dynamics.bzBody: bzBody; import openmelee.ships.ship : Ship, State; import openmelee.ai.utilities; class Steer { // Constructor: initializes state this (Ship ship) { m_ship = ship; m_body = ship.rBody; } struct PathIntersection { bool intersect; float distance; bzVec2 surfacePoint; bzVec2 surfaceNormal; bzBody obstacle; } // reset state void reset () { // initial state of wander behavior m_wanderSide = 0; m_wanderUp = 0; } void update() { m_position = m_ship.state.position; m_velocity = m_ship.state.velocity; m_speed = m_ship.state.speed; m_maxForce = m_ship.state.maxForce; m_forward = m_ship.state.forward; m_radius = m_ship.state.radius; } // -------------------------------------------------- steering behaviors bzVec2 steerForWander (float dt) { // random walk m_wanderSide and m_wanderUp between -1 and +1 float speed = 12 * dt; // maybe this (12) should be an argument? m_wanderSide = scalarRandomWalk (m_wanderSide, speed, -1, +1); m_wanderUp = scalarRandomWalk (m_wanderUp, speed, -1, +1); // return a pure lateral steering vector: (+/-Side) + (+/-Up) return (m_side * m_wanderSide) + (m_up * m_wanderUp); } // Seek behavior bzVec2 steerForSeek (bzVec2 target) { bzVec2 desiredVelocity = target - m_position; return desiredVelocity - m_velocity; } // Flee behavior bzVec2 steerForFlee (bzVec2 target) { bzVec2 desiredVelocity = m_position - target; return desiredVelocity - m_velocity; } /* // xxx proposed, experimental new seek/flee [cwr 9-16-02] bzVec2 xxxsteerForFlee (bzVec2 target) { bzVec2 offset = m_position - target; bzVec2 desiredVelocity = bzClamp(offset.truncateLength (maxSpeed ()); return desiredVelocity - m_velocity; } bzVec2 xxxsteerForSeek (bzVec2 target) { // bzVec2 offset = target - position; bzVec2 offset = target - m_position; bzVec2 desiredVelocity = offset.truncateLength (maxSpeed ()); //xxxnew return desiredVelocity - m_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); return avoidance; } // avoids all obstacles in an ObstacleGroup */ bzVec2 steerToAvoidObstacles (float minTimeToCollision, bzBody obstacles) { bzVec2 avoidance; PathIntersection nearest, next; float minDistanceToCollision = 10; //minTimeToCollision * m_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 (bzBody o = obstacles; o; o = o.next) { if(o is m_body) continue; // This code which presumes the obstacle is spherical findNextIntersectionWithSphere(o, next); if (!nearest.intersect || (next.intersect && next.distance < nearest.distance)) { nearest = next; } } // when a nearest intersection was found if (nearest.intersect && (nearest.distance < 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 = m_position - nearest.obstacle.position; avoidance = perpendicularComponent(offset, m_forward); avoidance.normalize; avoidance = m_body.localPoint(avoidance); ///avoidance *= m_maxForce; //avoidance += m_forward * m_maxForce * 0.75f; } auto state = cast(State) m_body.userData; state.avoid = avoidance; return avoidance; } bzVec2 getSteering(bzBody obstacles) { // 1. Find the target that’s closest to collision float radius = m_radius; float rad; // Store the first collision time float shortestTime = 0.5; // Store the target that collides then, and other data // that we will need and can avoid recalculating bzBody firstTarget = null; float firstMinSeparation = 0; float firstDistance = 0; bzVec2 firstRelativePos; bzVec2 firstRelativeVel; bzVec2 relativePos; // Loop through each target for (bzBody target = obstacles; target; target = target.next) { if(target is m_body) continue; // Calculate the time to collision relativePos = m_body.localPoint(target.position); // - m_position; bzVec2 relativeVel = target.linearVelocity - m_velocity; float relativeSpeed = relativeVel.length; float timeToCollision = bzDot(relativePos, relativeVel) / (relativeSpeed * relativeSpeed); // Check if it is going to be a collision at all float distance = relativePos.length; float minSeparation = distance - relativeSpeed * shortestTime; float eRad; for (bzShape s = target.shapeList; s; s = s.next) { if(s.sweepRadius > eRad) { eRad = s.sweepRadius; } } if (minSeparation > radius + eRad) continue; // Check if it is the shortest if (timeToCollision > 0 && timeToCollision < shortestTime) { // Store the time, target and other data shortestTime = timeToCollision; firstTarget = target; firstMinSeparation = minSeparation; firstDistance = distance; firstRelativePos = relativePos; firstRelativeVel = relativeVel; rad = eRad; } } // 2. Calculate the steering // If we have no target, then exit if(!firstTarget) return bzVec2.zeroVect; Stdout(shortestTime).newline; // If we’re going to hit exactly, or if we’re already // colliding, then do the steering based on current // position. if(firstMinSeparation <= 0 || firstDistance < radius + rad) { relativePos = firstTarget.position - m_position; } else { // Otherwise calculate the future relative position: relativePos = firstRelativePos + firstRelativeVel * shortestTime; } // Avoid the target bzVec2 steering = relativePos; steering.normalize(); //steering.linear = relativePos * maxAcceleration return steering; } bzVec2 avoid(float maxLookAhead, bzBody obstacles) { float avoidMargin = 1.0f; float maxLookahead = maxLookAhead * m_speed; // Make sure we're moving if (m_velocity.length > 0) { for (bzBody o = obstacles; o; o = o.next) { if(o is m_body) continue; // Find the distance from the line we're moving along to the obstacle. bzVec2 movementNormal = m_velocity; movementNormal.normalize(); bzVec2 characterToObstacle = o.position - m_position; real distanceSquared = bzDot(characterToObstacle, movementNormal); distanceSquared = characterToObstacle.lengthSquared - distanceSquared*distanceSquared; // Check for collision float oRad = 0; for (bzShape s = o.shapeList; s; s = s.next) { if(s.sweepRadius > oRad) { oRad = s.sweepRadius; } } real radius = oRad + avoidMargin; if (distanceSquared < radius*radius) { // Find how far along our movement vector the closest pass is real distanceToClosest = bzDot(characterToObstacle, movementNormal); // Make sure this isn't behind us and is closer than our lookahead. if (distanceToClosest > 0 && distanceToClosest < maxLookahead) { // Find the closest point bzVec2 closestPoint = o.position + movementNormal * distanceToClosest; // Find the point of avoidance bzVec2 target = o.position + (closestPoint - o.position).length * radius; target -= m_position; auto state = cast(State) m_body.userData; state.avoid = m_body.localPoint(target); return target; } } } } auto state = cast(State) m_body.userData; state.avoid = bzVec2.zeroVect; return bzVec2.zeroVect; } void findNextIntersectionWithSphere(bzBody obs, inout PathIntersection intersection) { // This routine is based on the Paul Bourke's derivation in: // Intersection of a Line and a Sphere (or circle) // http://local.wasp.uwa.edu.au/~pbourke/geometry/sphereline/raysphere.c float b, c, d, p, q, s; bzVec2 lc; // initialize pathIntersection object intersection.intersect = false; intersection.obstacle = obs; // find "local center" (lc) of sphere in coordinate space lc = m_body.localPoint(obs.position); // computer line-sphere intersection parameters b = 0; // Find radius of obstacle float obsRadius = 0; for (bzShape shape = obs.shapeList; shape; shape = shape.next) { if(shape.sweepRadius > obsRadius) { obsRadius = shape.sweepRadius; } } c = square(lc.x) + square(lc.y) - square(obsRadius + m_radius); d = (b * b) - (4 * c); // when the path does not intersect the sphere if (d < 0) return; // otherwise, the path intersects the sphere in two points with // parametric coordinates of "p" and "q". // (If "d" is zero the two points are coincident, the path is tangent) s = sqrt(d); p = (-b + s) * 0.5f; q = (-b - s) * 0.5f; // both intersections are behind us, so no potential collisions if ((p < 0) && (q < 0)) return; // at least one intersection is in front of us intersection.intersect = true; intersection.distance = ((p > 0) && (q > 0)) ? // both intersections are in front of us, find nearest one ((p < q) ? p : q) : // otherwise only one intersections is in front, select it ((p > 0) ? p : q); } /* // ------------------------------------------------------------------------ // 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; // 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 !is 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 = m_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 - m_position; float sideDot = offset.dot(m_side()); steer = (sideDot > 0) ? -1.0f : 1.0f; } else { if (parallelness > angle) { // parallel paths: steer away from threat bzVec2 offset = threat.position - m_position; float sideDot = bzDot(offset, m_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 = bzDot(m_side, threat.velocity); steer = (sideDot > 0) ? -1.0f : 1.0f; } } } } return m_side() * steer; } */ // Given two vehicles, based on their current positions and velocities, // determine the time until nearest approach float predictNearestApproachTime (State other) { // imagine we are at the origin with no velocity, // compute the relative velocity of the other vehicle bzVec2 myVelocity = m_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 = m_position - other.position; float projection = bzDot(relTangent, 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 (State other, float time) { bzVec2 myTravel = m_forward * m_speed * time; bzVec2 otherTravel = other.forward * other.speed * time; bzVec2 myFinal = m_position + myTravel; bzVec2 otherFinal = other.position + otherTravel; return (myFinal - otherFinal).length; } // ------------------------------------------------------------------------ // pursuit of another vehicle ( version with ceiling on prediction time) bzVec2 steerForPursuit (State quarry) { return steerForPursuit (quarry, float.max); } bzVec2 steerForPursuit (State quarry, float maxPredictionTime) { // offset from this to quarry, that distance, unit vector toward quarry bzVec2 offset = quarry.position - m_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 = bzDot(m_forward , 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 = bzDot(m_forward , unitOffset); float directTravelTime = distance / m_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 // 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; break; case 0: // ahead, perpendicular timeFactor = 1.8f; break; case -1: // ahead, anti-parallel timeFactor = 0.85f; break; } break; case 0: switch (p) { case +1: // aside, parallel timeFactor = 1; break; case 0: // aside, perpendicular timeFactor = 0.8f; break; case -1: // aside, anti-parallel timeFactor = 4; break; } break; case -1: switch (p) { case +1: // behind, parallel timeFactor = 0.5f; break; case 0: // behind, perpendicular timeFactor = 2; break; case -1: // behind, anti-parallel timeFactor = 2; 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); return target; //steerForSeek (target); } // ------------------------------------------------------------------------ // evasion of another vehicle bzVec2 steerForEvasion (State menace, float maxPredictionTime) { // offset from this to menace, that distance, unit vector toward menace bzVec2 offset = menace.position - m_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 = m_maxForce; float speedError = targetSpeed - m_speed; return m_forward * bzClamp(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 - m_position; targetDirection.normalize(); return bzDot(m_forward, targetDirection) > cosThreshold; } bool isAside (bzVec2 target, float cosThreshold) { bzVec2 targetDirection = target - m_position; targetDirection.normalize(); float dp = bzDot(m_forward, targetDirection); return (dp < cosThreshold) && (dp > -cosThreshold); } bool isBehind (bzVec2 target, float cosThreshold) { bzVec2 targetDirection = target - m_position; targetDirection.normalize(); return bzDot(m_forward, targetDirection) < cosThreshold; } private: Ship m_ship; bzVec2 m_position; bzVec2 m_velocity; bzVec2 m_up; bzVec2 m_side; bzVec2 m_forward; float m_radius; bzBody m_body; float m_speed = 0; float m_maxForce = 0; // Wander behavior float m_wanderSide; float m_wanderUp; }