neo/game/ai/AI_pathing.cpp

   1 /*
   2 ===========================================================================
   3
   4 Doom 3 GPL Source Code
   5 Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company.
   6
   7 This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).
   8
   9 Doom 3 Source Code is free software: you can redistribute it and/or modify
  10 it under the terms of the GNU General Public License as published by
  11 the Free Software Foundation, either version 3 of the License, or
  12 (at your option) any later version.
  13
  14 Doom 3 Source Code is distributed in the hope that it will be useful,
  15 but WITHOUT ANY WARRANTY; without even the implied warranty of
  16 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  17 GNU General Public License for more details.
  18
  19 You should have received a copy of the GNU General Public License
  20 along with Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
  21
  22 In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
  23
  24 If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
  25
  26 ===========================================================================
  27 */
  28
  29 #include "../../idlib/precompiled.h"
  30 #pragma hdrstop
  31
  32 #include "../Game_local.h"
  33
  34 /*
  35 ===============================================================================
  36
  37         Dynamic Obstacle Avoidance
  38
  39         - assumes the AI lives inside a bounding box aligned with the gravity direction
  40         - obstacles in proximity of the AI are gathered
  41         - if obstacles are found the AAS walls are also considered as obstacles
  42         - every obstacle is represented by an oriented bounding box (OBB)
  43         - an OBB is projected onto a 2D plane orthogonal to AI's gravity direction
  44         - the 2D windings of the projections are expanded for the AI bbox
  45         - a path tree is build using clockwise and counter clockwise edge walks along the winding edges
  46         - the path tree is pruned and optimized
  47         - the shortest path is chosen for navigation
  48
  49 ===============================================================================
  50 */
  51
  52 const float MAX_OBSTACLE_RADIUS                 = 256.0f;
  53 const float PUSH_OUTSIDE_OBSTACLES              = 0.5f;
  54 const float CLIP_BOUNDS_EPSILON                 = 10.0f;
  55 const int       MAX_AAS_WALL_EDGES                      = 256;
  56 const int       MAX_OBSTACLES                           = 256;
  57 const int       MAX_PATH_NODES                          = 256;
  58 const int       MAX_OBSTACLE_PATH                       = 64;
  59
  60 typedef struct obstacle_s {
  61         idVec2                          bounds[2];
  62         idWinding2D                     winding;
  63         idEntity *                      entity;
  64 } obstacle_t;
  65
  66 typedef struct pathNode_s {
  67         int                                     dir;
  68         idVec2                          pos;
  69         idVec2                          delta;
  70         float                           dist;
  71         int                                     obstacle;
  72         int                                     edgeNum;
  73         int                                     numNodes;
  74         struct pathNode_s *     parent;
  75         struct pathNode_s *     children[2];
  76         struct pathNode_s *     next;
  77         void                            Init();
  78 } pathNode_t;
  79
  80 void pathNode_s::Init() {
  81         dir = 0;
  82         pos.Zero();
  83         delta.Zero();
  84         obstacle = -1;
  85         edgeNum = -1;
  86         numNodes = 0;
  87         parent = children[0] = children[1] = next = NULL;
  88 }
  89
  90 idBlockAlloc<pathNode_t, 128>   pathNodeAllocator;
  91
  92
  93 /*
  94 ============
  95 LineIntersectsPath
  96 ============
  97 */
  98 bool LineIntersectsPath( const idVec2 &start, const idVec2 &end, const pathNode_t *node ) {
  99         float d0, d1, d2, d3;
 100         idVec3 plane1, plane2;
 101
 102         plane1 = idWinding2D::Plane2DFromPoints( start, end );
 103         d0 = plane1.x * node->pos.x + plane1.y * node->pos.y + plane1.z;
 104         while( node->parent ) {
 105                 d1 = plane1.x * node->parent->pos.x + plane1.y * node->parent->pos.y + plane1.z;
 106                 if ( FLOATSIGNBITSET( d0 ) ^ FLOATSIGNBITSET( d1 ) ) {
 107                         plane2 = idWinding2D::Plane2DFromPoints( node->pos, node->parent->pos );
 108                         d2 = plane2.x * start.x + plane2.y * start.y + plane2.z;
 109                         d3 = plane2.x * end.x + plane2.y * end.y + plane2.z;
 110                         if ( FLOATSIGNBITSET( d2 ) ^ FLOATSIGNBITSET( d3 ) ) {
 111                                 return true;
 112                         }
 113                 }
 114                 d0 = d1;
 115                 node = node->parent;
 116         }
 117         return false;
 118 }
 119
 120 /*
 121 ============
 122 PointInsideObstacle
 123 ============
 124 */
 125 int PointInsideObstacle( const obstacle_t *obstacles, const int numObstacles, const idVec2 &point ) {
 126         int i;
 127
 128         for ( i = 0; i < numObstacles; i++ ) {
 129
 130                 const idVec2 *bounds = obstacles[i].bounds;
 131                 if ( point.x < bounds[0].x || point.y < bounds[0].y || point.x > bounds[1].x || point.y > bounds[1].y ) {
 132                         continue;
 133                 }
 134
 135                 if ( !obstacles[i].winding.PointInside( point, 0.1f ) ) {
 136                         continue;
 137                 }
 138
 139                 return i;
 140         }
 141
 142         return -1;
 143 }
 144
 145 /*
 146 ============
 147 GetPointOutsideObstacles
 148 ============
 149 */
 150 void GetPointOutsideObstacles( const obstacle_t *obstacles, const int numObstacles, idVec2 &point, int *obstacle, int *edgeNum ) {
 151         int i, j, k, n, bestObstacle, bestEdgeNum, queueStart, queueEnd, edgeNums[2];
 152         float d, bestd, scale[2];
 153         idVec3 plane, bestPlane;
 154         idVec2 newPoint, dir, bestPoint;
 155         int *queue;
 156         bool *obstacleVisited;
 157         idWinding2D w1, w2;
 158
 159         if ( obstacle ) {
 160                 *obstacle = -1;
 161         }
 162         if ( edgeNum ) {
 163                 *edgeNum = -1;
 164         }
 165
 166         bestObstacle = PointInsideObstacle( obstacles, numObstacles, point );
 167         if ( bestObstacle == -1 ) {
 168                 return;
 169         }
 170
 171         const idWinding2D &w = obstacles[bestObstacle].winding;
 172         bestd = idMath::INFINITY;
 173         bestEdgeNum = 0;
 174         for ( i = 0; i < w.GetNumPoints(); i++ ) {
 175                 plane = idWinding2D::Plane2DFromPoints( w[(i+1)%w.GetNumPoints()], w[i], true );
 176                 d = plane.x * point.x + plane.y * point.y + plane.z;
 177                 if ( d < bestd ) {
 178                         bestd = d;
 179                         bestPlane = plane;
 180                         bestEdgeNum = i;
 181                 }
 182                 // if this is a wall always try to pop out at the first edge
 183                 if ( obstacles[bestObstacle].entity == NULL ) {
 184                         break;
 185                 }
 186         }
 187
 188         newPoint = point - ( bestd + PUSH_OUTSIDE_OBSTACLES ) * bestPlane.ToVec2();
 189         if ( PointInsideObstacle( obstacles, numObstacles, newPoint ) == -1 ) {
 190                 point = newPoint;
 191                 if ( obstacle ) {
 192                         *obstacle = bestObstacle;
 193                 }
 194                 if ( edgeNum ) {
 195                         *edgeNum = bestEdgeNum;
 196                 }
 197                 return;
 198         }
 199
 200         queue = (int *) _alloca( numObstacles * sizeof( queue[0] ) );
 201         obstacleVisited = (bool *) _alloca( numObstacles * sizeof( obstacleVisited[0] ) );
 202
 203         queueStart = 0;
 204         queueEnd = 1;
 205         queue[0] = bestObstacle;
 206
 207         memset( obstacleVisited, 0, numObstacles * sizeof( obstacleVisited[0] ) );
 208         obstacleVisited[bestObstacle] = true;
 209
 210         bestd = idMath::INFINITY;
 211         for ( i = queue[0]; queueStart < queueEnd; i = queue[++queueStart] ) {
 212                 w1 = obstacles[i].winding;
 213                 w1.Expand( PUSH_OUTSIDE_OBSTACLES );
 214
 215                 for ( j = 0; j < numObstacles; j++ ) {
 216                         // if the obstacle has been visited already
 217                         if ( obstacleVisited[j] ) {
 218                                 continue;
 219                         }
 220                         // if the bounds do not intersect
 221                         if ( obstacles[j].bounds[0].x > obstacles[i].bounds[1].x || obstacles[j].bounds[0].y > obstacles[i].bounds[1].y ||
 222                                         obstacles[j].bounds[1].x < obstacles[i].bounds[0].x || obstacles[j].bounds[1].y < obstacles[i].bounds[0].y ) {
 223                                 continue;
 224                         }
 225
 226                         queue[queueEnd++] = j;
 227                         obstacleVisited[j] = true;
 228
 229                         w2 = obstacles[j].winding;
 230                         w2.Expand( 0.2f );
 231
 232                         for ( k = 0; k < w1.GetNumPoints(); k++ ) {
 233                                 dir = w1[(k+1)%w1.GetNumPoints()] - w1[k];
 234                                 if ( !w2.RayIntersection( w1[k], dir, scale[0], scale[1], edgeNums ) ) {
 235                                         continue;
 236                                 }
 237                                 for ( n = 0; n < 2; n++ ) {
 238                                         newPoint = w1[k] + scale[n] * dir;
 239                                         if ( PointInsideObstacle( obstacles, numObstacles, newPoint ) == -1 ) {
 240                                                 d = ( newPoint - point ).LengthSqr();
 241                                                 if ( d < bestd ) {
 242                                                         bestd = d;
 243                                                         bestPoint = newPoint;
 244                                                         bestEdgeNum = edgeNums[n];
 245                                                         bestObstacle = j;
 246                                                 }
 247                                         }
 248                                 }
 249                         }
 250                 }
 251
 252                 if ( bestd < idMath::INFINITY ) {
 253                         point = bestPoint;
 254                         if ( obstacle ) {
 255                                 *obstacle = bestObstacle;
 256                         }
 257                         if ( edgeNum ) {
 258                                 *edgeNum = bestEdgeNum;
 259                         }
 260                         return;
 261                 }
 262         }
 263         gameLocal.Warning( "GetPointOutsideObstacles: no valid point found" );
 264 }
 265
 266 /*
 267 ============
 268 GetFirstBlockingObstacle
 269 ============
 270 */
 271 bool GetFirstBlockingObstacle( const obstacle_t *obstacles, int numObstacles, int skipObstacle, const idVec2 &startPos, const idVec2 &delta, float &blockingScale, int &blockingObstacle, int &blockingEdgeNum ) {
 272         int i, edgeNums[2];
 273         float dist, scale1, scale2;
 274         idVec2 bounds[2];
 275
 276         // get bounds for the current movement delta
 277         bounds[0] = startPos - idVec2( CM_BOX_EPSILON, CM_BOX_EPSILON );
 278         bounds[1] = startPos + idVec2( CM_BOX_EPSILON, CM_BOX_EPSILON );
 279         bounds[FLOATSIGNBITNOTSET(delta.x)].x += delta.x;
 280         bounds[FLOATSIGNBITNOTSET(delta.y)].y += delta.y;
 281
 282         // test for obstacles blocking the path
 283         blockingScale = idMath::INFINITY;
 284         dist = delta.Length();
 285         for ( i = 0; i < numObstacles; i++ ) {
 286                 if ( i == skipObstacle ) {
 287                         continue;
 288                 }
 289                 if ( bounds[0].x > obstacles[i].bounds[1].x || bounds[0].y > obstacles[i].bounds[1].y ||
 290                                 bounds[1].x < obstacles[i].bounds[0].x || bounds[1].y < obstacles[i].bounds[0].y ) {
 291                         continue;
 292                 }
 293                 if ( obstacles[i].winding.RayIntersection( startPos, delta, scale1, scale2, edgeNums ) ) {
 294                         if ( scale1 < blockingScale && scale1 * dist > -0.01f && scale2 * dist > 0.01f ) {
 295                                 blockingScale = scale1;
 296                                 blockingObstacle = i;
 297                                 blockingEdgeNum = edgeNums[0];
 298                         }
 299                 }
 300         }
 301         return ( blockingScale < 1.0f );
 302 }
 303
 304 /*
 305 ============
 306 GetObstacles
 307 ============
 308 */
 309 int GetObstacles( const idPhysics *physics, const idAAS *aas, const idEntity *ignore, int areaNum, const idVec3 &startPos, const idVec3 &seekPos, obstacle_t *obstacles, int maxObstacles, idBounds &clipBounds ) {
 310         int i, j, numListedClipModels, numObstacles, numVerts, clipMask, blockingObstacle, blockingEdgeNum;
 311         int wallEdges[MAX_AAS_WALL_EDGES], numWallEdges, verts[2], lastVerts[2], nextVerts[2];
 312         float stepHeight, headHeight, blockingScale, min, max;
 313         idVec3 seekDelta, silVerts[32], start, end, nextStart, nextEnd;
 314         idVec2 expBounds[2], edgeDir, edgeNormal, nextEdgeDir, nextEdgeNormal, lastEdgeNormal;
 315         idVec2 obDelta;
 316         idPhysics *obPhys;
 317         idBox box;
 318         idEntity *obEnt;
 319         idClipModel *clipModel;
 320         idClipModel *clipModelList[ MAX_GENTITIES ];
 321
 322         numObstacles = 0;
 323
 324         seekDelta = seekPos - startPos;
 325         expBounds[0] = physics->GetBounds()[0].ToVec2() - idVec2( CM_BOX_EPSILON, CM_BOX_EPSILON );
 326         expBounds[1] = physics->GetBounds()[1].ToVec2() + idVec2( CM_BOX_EPSILON, CM_BOX_EPSILON );
 327
 328         physics->GetAbsBounds().AxisProjection( -physics->GetGravityNormal(), stepHeight, headHeight );
 329         stepHeight += aas->GetSettings()->maxStepHeight;
 330
 331         // clip bounds for the obstacle search space
 332         clipBounds[0] = clipBounds[1] = startPos;
 333         clipBounds.AddPoint( seekPos );
 334         clipBounds.ExpandSelf( MAX_OBSTACLE_RADIUS );
 335         clipMask = physics->GetClipMask();
 336
 337         // find all obstacles touching the clip bounds
 338         numListedClipModels = gameLocal.clip.ClipModelsTouchingBounds( clipBounds, clipMask, clipModelList, MAX_GENTITIES );
 339
 340         for ( i = 0; i < numListedClipModels && numObstacles < MAX_OBSTACLES; i++ ) {
 341                 clipModel = clipModelList[i];
 342                 obEnt = clipModel->GetEntity();
 343
 344                 if ( !clipModel->IsTraceModel() ) {
 345                         continue;
 346                 }
 347
 348                 if ( obEnt->IsType( idActor::Type ) ) {
 349                         obPhys = obEnt->GetPhysics();
 350                         // ignore myself, my enemy, and dead bodies
 351                         if ( ( obPhys == physics ) || ( obEnt == ignore ) || ( obEnt->health <= 0 ) ) {
 352                                 continue;
 353                         }
 354                         // if the actor is moving
 355                         idVec3 v1 = obPhys->GetLinearVelocity();
 356                         if ( v1.LengthSqr() > Square( 10.0f ) ) {
 357                                 idVec3 v2 = physics->GetLinearVelocity();
 358                                 if ( v2.LengthSqr() > Square( 10.0f ) ) {
 359                                         // if moving in about the same direction
 360                                         if ( v1 * v2 > 0.0f ) {
 361                                                 continue;
 362                                         }
 363                                 }
 364                         }
 365                 } else if ( obEnt->IsType( idMoveable::Type ) ) {
 366                         // moveables are considered obstacles
 367                 } else {
 368                         // ignore everything else
 369                         continue;
 370                 }
 371
 372                 // check if we can step over the object
 373                 clipModel->GetAbsBounds().AxisProjection( -physics->GetGravityNormal(), min, max );
 374                 if ( max < stepHeight || min > headHeight ) {
 375                         // can step over this one
 376                         continue;
 377                 }
 378
 379                 // project a box containing the obstacle onto the floor plane
 380                 box = idBox( clipModel->GetBounds(), clipModel->GetOrigin(), clipModel->GetAxis() );
 381                 numVerts = box.GetParallelProjectionSilhouetteVerts( physics->GetGravityNormal(), silVerts );
 382
 383                 // create a 2D winding for the obstacle;
 384                 obstacle_t &obstacle = obstacles[numObstacles++];
 385                 obstacle.winding.Clear();
 386                 for ( j = 0; j < numVerts; j++ ) {
 387                         obstacle.winding.AddPoint( silVerts[j].ToVec2() );
 388                 }
 389
 390                 if ( ai_showObstacleAvoidance.GetBool() ) {
 391                         for ( j = 0; j < numVerts; j++ ) {
 392                                 silVerts[j].z = startPos.z;
 393                         }
 394                         for ( j = 0; j < numVerts; j++ ) {
 395                                 gameRenderWorld->DebugArrow( colorWhite, silVerts[j], silVerts[(j+1)%numVerts], 4 );
 396                         }
 397                 }
 398
 399                 // expand the 2D winding for collision with a 2D box
 400                 obstacle.winding.ExpandForAxialBox( expBounds );
 401                 obstacle.winding.GetBounds( obstacle.bounds );
 402                 obstacle.entity = obEnt;
 403         }
 404
 405         // if there are no dynamic obstacles the path should be through valid AAS space
 406         if ( numObstacles == 0 ) {
 407                 return 0;
 408         }
 409
 410         // if the current path doesn't intersect any dynamic obstacles the path should be through valid AAS space
 411         if ( PointInsideObstacle( obstacles, numObstacles, startPos.ToVec2() ) == -1 ) {
 412                 if ( !GetFirstBlockingObstacle( obstacles, numObstacles, -1, startPos.ToVec2(), seekDelta.ToVec2(), blockingScale, blockingObstacle, blockingEdgeNum ) ) {
 413                         return 0;
 414                 }
 415         }
 416
 417         // create obstacles for AAS walls
 418         if ( aas ) {
 419                 float halfBoundsSize = ( expBounds[ 1 ].x - expBounds[ 0 ].x ) * 0.5f;
 420
 421                 numWallEdges = aas->GetWallEdges( areaNum, clipBounds, TFL_WALK, wallEdges, MAX_AAS_WALL_EDGES );
 422                 aas->SortWallEdges( wallEdges, numWallEdges );
 423
 424                 lastVerts[0] = lastVerts[1] = 0;
 425                 lastEdgeNormal.Zero();
 426                 nextVerts[0] = nextVerts[1] = 0;
 427                 for ( i = 0; i < numWallEdges && numObstacles < MAX_OBSTACLES; i++ ) {
 428             aas->GetEdge( wallEdges[i], start, end );
 429                         aas->GetEdgeVertexNumbers( wallEdges[i], verts );
 430                         edgeDir = end.ToVec2() - start.ToVec2();
 431                         edgeDir.Normalize();
 432                         edgeNormal.x = edgeDir.y;
 433                         edgeNormal.y = -edgeDir.x;
 434                         if ( i < numWallEdges-1 ) {
 435                                 aas->GetEdge( wallEdges[i+1], nextStart, nextEnd );
 436                                 aas->GetEdgeVertexNumbers( wallEdges[i+1], nextVerts );
 437                                 nextEdgeDir = nextEnd.ToVec2() - nextStart.ToVec2();
 438                                 nextEdgeDir.Normalize();
 439                                 nextEdgeNormal.x = nextEdgeDir.y;
 440                                 nextEdgeNormal.y = -nextEdgeDir.x;
 441                         }
 442
 443                         obstacle_t &obstacle = obstacles[numObstacles++];
 444                         obstacle.winding.Clear();
 445                         obstacle.winding.AddPoint( end.ToVec2() );
 446                         obstacle.winding.AddPoint( start.ToVec2() );
 447                         obstacle.winding.AddPoint( start.ToVec2() - edgeDir - edgeNormal * halfBoundsSize );
 448                         obstacle.winding.AddPoint( end.ToVec2() + edgeDir - edgeNormal * halfBoundsSize );
 449                         if ( lastVerts[1] == verts[0] ) {
 450                                 obstacle.winding[2] -= lastEdgeNormal * halfBoundsSize;
 451                         } else {
 452                                 obstacle.winding[1] -= edgeDir;
 453                         }
 454                         if ( verts[1] == nextVerts[0] ) {
 455                                 obstacle.winding[3] -= nextEdgeNormal * halfBoundsSize;
 456                         } else {
 457                                 obstacle.winding[0] += edgeDir;
 458                         }
 459                         obstacle.winding.GetBounds( obstacle.bounds );
 460                         obstacle.entity = NULL;
 461
 462                         memcpy( lastVerts, verts, sizeof( lastVerts ) );
 463                         lastEdgeNormal = edgeNormal;
 464                 }
 465         }
 466
 467         // show obstacles
 468         if ( ai_showObstacleAvoidance.GetBool() ) {
 469                 for ( i = 0; i < numObstacles; i++ ) {
 470                         obstacle_t &obstacle = obstacles[i];
 471                         for ( j = 0; j < obstacle.winding.GetNumPoints(); j++ ) {
 472                                 silVerts[j].ToVec2() = obstacle.winding[j];
 473                                 silVerts[j].z = startPos.z;
 474                         }
 475                         for ( j = 0; j < obstacle.winding.GetNumPoints(); j++ ) {
 476                                 gameRenderWorld->DebugArrow( colorGreen, silVerts[j], silVerts[(j+1)%obstacle.winding.GetNumPoints()], 4 );
 477                         }
 478                 }
 479         }
 480
 481         return numObstacles;
 482 }
 483
 484 /*
 485 ============
 486 FreePathTree_r
 487 ============
 488 */
 489 void FreePathTree_r( pathNode_t *node ) {
 490         if ( node->children[0] ) {
 491                 FreePathTree_r( node->children[0] );
 492         }
 493         if ( node->children[1] ) {
 494                 FreePathTree_r( node->children[1] );
 495         }
 496         pathNodeAllocator.Free( node );
 497 }
 498
 499 /*
 500 ============
 501 DrawPathTree
 502 ============
 503 */
 504 void DrawPathTree( const pathNode_t *root, const float height ) {
 505         int i;
 506         idVec3 start, end;
 507         const pathNode_t *node;
 508
 509         for ( node = root; node; node = node->next ) {
 510                 for ( i = 0; i < 2; i++ ) {
 511                         if ( node->children[i] ) {
 512                                 start.ToVec2() = node->pos;
 513                                 start.z = height;
 514                                 end.ToVec2() = node->children[i]->pos;
 515                                 end.z = height;
 516                                 gameRenderWorld->DebugArrow( node->edgeNum == -1 ? colorYellow : i ? colorBlue : colorRed, start, end, 1 );
 517                                 break;
 518                         }
 519                 }
 520         }
 521 }
 522
 523 /*
 524 ============
 525 GetPathNodeDelta
 526 ============
 527 */
 528 bool GetPathNodeDelta( pathNode_t *node, const obstacle_t *obstacles, const idVec2 &seekPos, bool blocked ) {
 529         int numPoints, edgeNum;
 530         bool facing;
 531         idVec2 seekDelta, dir;
 532         pathNode_t *n;
 533
 534         numPoints = obstacles[node->obstacle].winding.GetNumPoints();
 535
 536         // get delta along the current edge
 537         while( 1 ) {
 538                 edgeNum = ( node->edgeNum + node->dir ) % numPoints;
 539                 node->delta = obstacles[node->obstacle].winding[edgeNum] - node->pos;
 540                 if ( node->delta.LengthSqr() > 0.01f ) {
 541                         break;
 542                 }
 543                 node->edgeNum = ( node->edgeNum + numPoints + ( 2 * node->dir - 1 ) ) % numPoints;
 544         }
 545
 546         // if not blocked
 547         if ( !blocked ) {
 548
 549                 // test if the current edge faces the goal
 550                 seekDelta = seekPos - node->pos;
 551                 facing = ( ( 2 * node->dir - 1 ) * ( node->delta.x * seekDelta.y - node->delta.y * seekDelta.x ) ) >= 0.0f;
 552
 553                 // if the current edge faces goal and the line from the current
 554                 // position to the goal does not intersect the current path
 555                 if ( facing && !LineIntersectsPath( node->pos, seekPos, node->parent ) ) {
 556                         node->delta = seekPos - node->pos;
 557                         node->edgeNum = -1;
 558                 }
 559         }
 560
 561         // if the delta is along the obstacle edge
 562         if ( node->edgeNum != -1 ) {
 563                 // if the edge is found going from this node to the root node
 564                 for ( n = node->parent; n; n = n->parent ) {
 565
 566                         if ( node->obstacle != n->obstacle || node->edgeNum != n->edgeNum ) {
 567                                 continue;
 568                         }
 569
 570                         // test whether or not the edge segments actually overlap
 571                         if ( n->pos * node->delta > ( node->pos + node->delta ) * node->delta ) {
 572                                 continue;
 573                         }
 574                         if ( node->pos * node->delta > ( n->pos + n->delta ) * node->delta ) {
 575                                 continue;
 576                         }
 577
 578                         break;
 579                 }
 580                 if ( n ) {
 581                         return false;
 582                 }
 583         }
 584         return true;
 585 }
 586
 587 /*
 588 ============
 589 BuildPathTree
 590 ============
 591 */
 592 pathNode_t *BuildPathTree( const obstacle_t *obstacles, int numObstacles, const idBounds &clipBounds, const idVec2 &startPos, const idVec2 &seekPos, obstaclePath_t &path ) {
 593         int blockingEdgeNum, blockingObstacle, obstaclePoints, bestNumNodes = MAX_OBSTACLE_PATH;
 594         float blockingScale;
 595         pathNode_t *root, *node, *child;
 596         // gcc 4.0
 597         idQueueTemplate<pathNode_t, offsetof( pathNode_t, next ) > pathNodeQueue, treeQueue;
 598         root = pathNodeAllocator.Alloc();
 599         root->Init();
 600         root->pos = startPos;
 601
 602         root->delta = seekPos - root->pos;
 603         root->numNodes = 0;
 604         pathNodeQueue.Add( root );
 605
 606         for ( node = pathNodeQueue.Get(); node && pathNodeAllocator.GetAllocCount() < MAX_PATH_NODES; node = pathNodeQueue.Get() ) {
 607
 608                 treeQueue.Add( node );
 609
 610                 // if this path has more than twice the number of nodes than the best path so far
 611                 if ( node->numNodes > bestNumNodes * 2 ) {
 612                         continue;
 613                 }
 614
 615                 // don't move outside of the clip bounds
 616                 idVec2 endPos = node->pos + node->delta;
 617                 if ( endPos.x - CLIP_BOUNDS_EPSILON < clipBounds[0].x || endPos.x + CLIP_BOUNDS_EPSILON > clipBounds[1].x ||
 618                                 endPos.y - CLIP_BOUNDS_EPSILON < clipBounds[0].y || endPos.y + CLIP_BOUNDS_EPSILON > clipBounds[1].y ) {
 619                         continue;
 620                 }
 621
 622                 // if an obstacle is blocking the path
 623                 if ( GetFirstBlockingObstacle( obstacles, numObstacles, node->obstacle, node->pos, node->delta, blockingScale, blockingObstacle, blockingEdgeNum ) ) {
 624
 625                         if ( path.firstObstacle == NULL ) {
 626                                 path.firstObstacle = obstacles[blockingObstacle].entity;
 627                         }
 628
 629                         node->delta *= blockingScale;
 630
 631                         if ( node->edgeNum == -1 ) {
 632                                 node->children[0] = pathNodeAllocator.Alloc();
 633                                 node->children[0]->Init();
 634                                 node->children[1] = pathNodeAllocator.Alloc();
 635                                 node->children[1]->Init();
 636                                 node->children[0]->dir = 0;
 637                                 node->children[1]->dir = 1;
 638                                 node->children[0]->parent = node->children[1]->parent = node;
 639                                 node->children[0]->pos = node->children[1]->pos = node->pos + node->delta;
 640                                 node->children[0]->obstacle = node->children[1]->obstacle = blockingObstacle;
 641                                 node->children[0]->edgeNum = node->children[1]->edgeNum = blockingEdgeNum;
 642                                 node->children[0]->numNodes = node->children[1]->numNodes = node->numNodes + 1;
 643                                 if ( GetPathNodeDelta( node->children[0], obstacles, seekPos, true ) ) {
 644                                         pathNodeQueue.Add( node->children[0] );
 645                                 }
 646                                 if ( GetPathNodeDelta( node->children[1], obstacles, seekPos, true ) ) {
 647                                         pathNodeQueue.Add( node->children[1] );
 648                                 }
 649                         } else {
 650                                 node->children[node->dir] = child = pathNodeAllocator.Alloc();
 651                                 child->Init();
 652                                 child->dir = node->dir;
 653                                 child->parent = node;
 654                                 child->pos = node->pos + node->delta;
 655                                 child->obstacle = blockingObstacle;
 656                                 child->edgeNum = blockingEdgeNum;
 657                                 child->numNodes = node->numNodes + 1;
 658                                 if ( GetPathNodeDelta( child, obstacles, seekPos, true ) ) {
 659                                         pathNodeQueue.Add( child );
 660                                 }
 661                         }
 662                 } else {
 663                         node->children[node->dir] = child = pathNodeAllocator.Alloc();
 664                         child->Init();
 665                         child->dir = node->dir;
 666                         child->parent = node;
 667                         child->pos = node->pos + node->delta;
 668                         child->numNodes = node->numNodes + 1;
 669
 670                         // there is a free path towards goal
 671                         if ( node->edgeNum == -1 ) {
 672                                 if ( node->numNodes < bestNumNodes ) {
 673                                         bestNumNodes = node->numNodes;
 674                                 }
 675                                 continue;
 676                         }
 677
 678                         child->obstacle = node->obstacle;
 679                         obstaclePoints = obstacles[node->obstacle].winding.GetNumPoints();
 680                         child->edgeNum = ( node->edgeNum + obstaclePoints + ( 2 * node->dir - 1 ) ) % obstaclePoints;
 681
 682                         if ( GetPathNodeDelta( child, obstacles, seekPos, false ) ) {
 683                                 pathNodeQueue.Add( child );
 684                         }
 685                 }
 686         }
 687
 688         return root;
 689 }
 690
 691 /*
 692 ============
 693 PrunePathTree
 694 ============
 695 */
 696 void PrunePathTree( pathNode_t *root, const idVec2 &seekPos ) {
 697         int i;
 698         float bestDist;
 699         pathNode_t *node, *lastNode, *n, *bestNode;
 700
 701         node = root;
 702         while( node ) {
 703
 704                 node->dist = ( seekPos - node->pos ).LengthSqr();
 705
 706                 if ( node->children[0] ) {
 707                         node = node->children[0];
 708                 } else if ( node->children[1] ) {
 709                         node = node->children[1];
 710                 } else {
 711
 712                         // find the node closest to the goal along this path
 713                         bestDist = idMath::INFINITY;
 714                         bestNode = node;
 715                         for ( n = node; n; n = n->parent ) {
 716                                 if ( n->children[0] && n->children[1] ) {
 717                                         break;
 718                                 }
 719                                 if ( n->dist < bestDist ) {
 720                                         bestDist = n->dist;
 721                                         bestNode = n;
 722                                 }
 723                         }
 724
 725                         // free tree down from the best node
 726                         for ( i = 0; i < 2; i++ ) {
 727                                 if ( bestNode->children[i] ) {
 728                                         FreePathTree_r( bestNode->children[i] );
 729                                         bestNode->children[i] = NULL;
 730                                 }
 731                         }
 732
 733                         for ( lastNode = bestNode, node = bestNode->parent; node; lastNode = node, node = node->parent ) {
 734                                 if ( node->children[1] && ( node->children[1] != lastNode ) ) {
 735                                         node = node->children[1];
 736                                         break;
 737                                 }
 738                         }
 739                 }
 740         }
 741 }
 742
 743 /*
 744 ============
 745 OptimizePath
 746 ============
 747 */
 748 int OptimizePath( const pathNode_t *root, const pathNode_t *leafNode, const obstacle_t *obstacles, int numObstacles, idVec2 optimizedPath[MAX_OBSTACLE_PATH] ) {
 749         int i, numPathPoints, edgeNums[2];
 750         const pathNode_t *curNode, *nextNode;
 751         idVec2 curPos, curDelta, bounds[2];
 752         float scale1, scale2, curLength;
 753
 754         optimizedPath[0] = root->pos;
 755         numPathPoints = 1;
 756
 757         for ( nextNode = curNode = root; curNode != leafNode; curNode = nextNode ) {
 758
 759                 for ( nextNode = leafNode; nextNode->parent != curNode; nextNode = nextNode->parent ) {
 760
 761                         // can only take shortcuts when going from one object to another
 762                         if ( nextNode->obstacle == curNode->obstacle ) {
 763                                 continue;
 764                         }
 765
 766                         curPos = curNode->pos;
 767                         curDelta = nextNode->pos - curPos;
 768                         curLength = curDelta.Length();
 769
 770                         // get bounds for the current movement delta
 771                         bounds[0] = curPos - idVec2( CM_BOX_EPSILON, CM_BOX_EPSILON );
 772                         bounds[1] = curPos + idVec2( CM_BOX_EPSILON, CM_BOX_EPSILON );
 773                         bounds[FLOATSIGNBITNOTSET(curDelta.x)].x += curDelta.x;
 774                         bounds[FLOATSIGNBITNOTSET(curDelta.y)].y += curDelta.y;
 775
 776                         // test if the shortcut intersects with any obstacles
 777                         for ( i = 0; i < numObstacles; i++ ) {
 778                                 if ( bounds[0].x > obstacles[i].bounds[1].x || bounds[0].y > obstacles[i].bounds[1].y ||
 779                                                 bounds[1].x < obstacles[i].bounds[0].x || bounds[1].y < obstacles[i].bounds[0].y ) {
 780                                         continue;
 781                                 }
 782                                 if ( obstacles[i].winding.RayIntersection( curPos, curDelta, scale1, scale2, edgeNums ) ) {
 783                                         if ( scale1 >= 0.0f && scale1 <= 1.0f && ( i != nextNode->obstacle || scale1 * curLength < curLength - 0.5f ) ) {
 784                                                 break;
 785                                         }
 786                                         if ( scale2 >= 0.0f && scale2 <= 1.0f && ( i != nextNode->obstacle || scale2 * curLength < curLength - 0.5f ) ) {
 787                                                 break;
 788                                         }
 789                                 }
 790                         }
 791                         if ( i >= numObstacles ) {
 792                                 break;
 793                         }
 794                 }
 795
 796                 // store the next position along the optimized path
 797                 optimizedPath[numPathPoints++] = nextNode->pos;
 798         }
 799
 800         return numPathPoints;
 801 }
 802
 803 /*
 804 ============
 805 PathLength
 806 ============
 807 */
 808 float PathLength( idVec2 optimizedPath[MAX_OBSTACLE_PATH], int numPathPoints, const idVec2 &curDir ) {
 809         int i;
 810         float pathLength;
 811
 812         // calculate the path length
 813         pathLength = 0.0f;
 814         for ( i = 0; i < numPathPoints-1; i++ ) {
 815                 pathLength += ( optimizedPath[i+1] - optimizedPath[i] ).LengthFast();
 816         }
 817
 818         // add penalty if this path does not go in the current direction
 819         if ( curDir * ( optimizedPath[1] - optimizedPath[0] ) < 0.0f ) {
 820                 pathLength += 100.0f;
 821         }
 822         return pathLength;
 823 }
 824
 825 /*
 826 ============
 827 FindOptimalPath
 828
 829   Returns true if there is a path all the way to the goal.
 830 ============
 831 */
 832 bool FindOptimalPath( const pathNode_t *root, const obstacle_t *obstacles, int numObstacles, const float height, const idVec3 &curDir, idVec3 &seekPos ) {
 833         int i, numPathPoints, bestNumPathPoints;
 834         const pathNode_t *node, *lastNode, *bestNode;
 835         idVec2 optimizedPath[MAX_OBSTACLE_PATH];
 836         float pathLength, bestPathLength;
 837         bool pathToGoalExists, optimizedPathCalculated;
 838
 839         seekPos.Zero();
 840         seekPos.z = height;
 841
 842         pathToGoalExists = false;
 843         optimizedPathCalculated = false;
 844
 845         bestNode = root;
 846         bestNumPathPoints = 0;
 847         bestPathLength = idMath::INFINITY;
 848
 849         node = root;
 850         while( node ) {
 851
 852                 pathToGoalExists |= ( node->dist < 0.1f );
 853
 854                 if ( node->dist <= bestNode->dist ) {
 855
 856                         if ( idMath::Fabs( node->dist - bestNode->dist ) < 0.1f ) {
 857
 858                                 if ( !optimizedPathCalculated ) {
 859                                         bestNumPathPoints = OptimizePath( root, bestNode, obstacles, numObstacles, optimizedPath );
 860                                         bestPathLength = PathLength( optimizedPath, bestNumPathPoints, curDir.ToVec2() );
 861                                         seekPos.ToVec2() = optimizedPath[1];
 862                                 }
 863
 864                                 numPathPoints = OptimizePath( root, node, obstacles, numObstacles, optimizedPath );
 865                                 pathLength = PathLength( optimizedPath, numPathPoints, curDir.ToVec2() );
 866
 867                                 if ( pathLength < bestPathLength ) {
 868                                         bestNode = node;
 869                                         bestNumPathPoints = numPathPoints;
 870                                         bestPathLength = pathLength;
 871                                         seekPos.ToVec2() = optimizedPath[1];
 872                                 }
 873                                 optimizedPathCalculated = true;
 874
 875                         } else {
 876
 877                                 bestNode = node;
 878                                 optimizedPathCalculated = false;
 879                         }
 880                 }
 881
 882                 if ( node->children[0] ) {
 883                         node = node->children[0];
 884                 } else if ( node->children[1] ) {
 885                         node = node->children[1];
 886                 } else {
 887                         for ( lastNode = node, node = node->parent; node; lastNode = node, node = node->parent ) {
 888                                 if ( node->children[1] && node->children[1] != lastNode ) {
 889                                         node = node->children[1];
 890                                         break;
 891                                 }
 892                         }
 893                 }
 894         }
 895
 896         if ( !pathToGoalExists ) {
 897                 seekPos.ToVec2() = root->children[0]->pos;
 898         } else if ( !optimizedPathCalculated ) {
 899                 OptimizePath( root, bestNode, obstacles, numObstacles, optimizedPath );
 900                 seekPos.ToVec2() = optimizedPath[1];
 901         }
 902
 903         if ( ai_showObstacleAvoidance.GetBool() ) {
 904                 idVec3 start, end;
 905                 start.z = end.z = height + 4.0f;
 906                 numPathPoints = OptimizePath( root, bestNode, obstacles, numObstacles, optimizedPath );
 907                 for ( i = 0; i < numPathPoints-1; i++ ) {
 908                         start.ToVec2() = optimizedPath[i];
 909                         end.ToVec2() = optimizedPath[i+1];
 910                         gameRenderWorld->DebugArrow( colorCyan, start, end, 1 );
 911                 }
 912         }
 913
 914         return pathToGoalExists;
 915 }
 916
 917 /*
 918 ============
 919 idAI::FindPathAroundObstacles
 920
 921   Finds a path around dynamic obstacles using a path tree with clockwise and counter clockwise edge walks.
 922 ============
 923 */
 924 bool idAI::FindPathAroundObstacles( const idPhysics *physics, const idAAS *aas, const idEntity *ignore, const idVec3 &startPos, const idVec3 &seekPos, obstaclePath_t &path ) {
 925         int numObstacles, areaNum, insideObstacle;
 926         obstacle_t obstacles[MAX_OBSTACLES];
 927         idBounds clipBounds;
 928         idBounds bounds;
 929         pathNode_t *root;
 930         bool pathToGoalExists;
 931
 932         path.seekPos = seekPos;
 933         path.firstObstacle = NULL;
 934         path.startPosOutsideObstacles = startPos;
 935         path.startPosObstacle = NULL;
 936         path.seekPosOutsideObstacles = seekPos;
 937         path.seekPosObstacle = NULL;
 938
 939         if ( !aas ) {
 940                 return true;
 941         }
 942
 943         bounds[1] = aas->GetSettings()->boundingBoxes[0][1];
 944         bounds[0] = -bounds[1];
 945         bounds[1].z = 32.0f;
 946
 947         // get the AAS area number and a valid point inside that area
 948         areaNum = aas->PointReachableAreaNum( path.startPosOutsideObstacles, bounds, (AREA_REACHABLE_WALK|AREA_REACHABLE_FLY) );
 949         aas->PushPointIntoAreaNum( areaNum, path.startPosOutsideObstacles );
 950
 951         // get all the nearby obstacles
 952         numObstacles = GetObstacles( physics, aas, ignore, areaNum, path.startPosOutsideObstacles, path.seekPosOutsideObstacles, obstacles, MAX_OBSTACLES, clipBounds );
 953
 954         // get a source position outside the obstacles
 955         GetPointOutsideObstacles( obstacles, numObstacles, path.startPosOutsideObstacles.ToVec2(), &insideObstacle, NULL );
 956         if ( insideObstacle != -1 ) {
 957                 path.startPosObstacle = obstacles[insideObstacle].entity;
 958         }
 959
 960         // get a goal position outside the obstacles
 961         GetPointOutsideObstacles( obstacles, numObstacles, path.seekPosOutsideObstacles.ToVec2(), &insideObstacle, NULL );
 962         if ( insideObstacle != -1 ) {
 963                 path.seekPosObstacle = obstacles[insideObstacle].entity;
 964         }
 965
 966         // if start and destination are pushed to the same point, we don't have a path around the obstacle
 967         if ( ( path.seekPosOutsideObstacles.ToVec2() - path.startPosOutsideObstacles.ToVec2() ).LengthSqr() < Square( 1.0f ) ) {
 968                 if ( ( seekPos.ToVec2() - startPos.ToVec2() ).LengthSqr() > Square( 2.0f ) ) {
 969                         return false;
 970                 }
 971         }
 972
 973         // build a path tree
 974         root = BuildPathTree( obstacles, numObstacles, clipBounds, path.startPosOutsideObstacles.ToVec2(), path.seekPosOutsideObstacles.ToVec2(), path );
 975
 976         // draw the path tree
 977         if ( ai_showObstacleAvoidance.GetBool() ) {
 978                 DrawPathTree( root, physics->GetOrigin().z );
 979         }
 980
 981         // prune the tree
 982         PrunePathTree( root, path.seekPosOutsideObstacles.ToVec2() );
 983
 984         // find the optimal path
 985         pathToGoalExists = FindOptimalPath( root, obstacles, numObstacles, physics->GetOrigin().z, physics->GetLinearVelocity(), path.seekPos );
 986
 987         // free the tree
 988         FreePathTree_r( root );
 989
 990         return pathToGoalExists;
 991 }
 992
 993 /*
 994 ============
 995 idAI::FreeObstacleAvoidanceNodes
 996 ============
 997 */
 998 void idAI::FreeObstacleAvoidanceNodes( void ) {
 999         pathNodeAllocator.Shutdown();
1000 }
1001
1002
1003 /*
1004 ===============================================================================
1005
1006         Path Prediction
1007
1008         Uses the AAS to quickly and accurately predict a path for a certain
1009         period of time based on an initial position and velocity.
1010
1011 ===============================================================================
1012 */
1013
1014 const float OVERCLIP                    = 1.001f;
1015 const int MAX_FRAME_SLIDE               = 5;
1016
1017 typedef struct pathTrace_s {
1018         float                                   fraction;
1019         idVec3                                  endPos;
1020         idVec3                                  normal;
1021         const idEntity *                blockingEntity;
1022 } pathTrace_t;
1023
1024 /*
1025 ============
1026 PathTrace
1027
1028   Returns true if a stop event was triggered.
1029 ============
1030 */
1031 bool PathTrace( const idEntity *ent, const idAAS *aas, const idVec3 &start, const idVec3 &end, int stopEvent, struct pathTrace_s &trace, predictedPath_t &path ) {
1032         trace_t clipTrace;
1033         aasTrace_t aasTrace;
1034
1035         memset( &trace, 0, sizeof( trace ) );
1036
1037         if ( !aas || !aas->GetSettings() ) {
1038
1039                 gameLocal.clip.Translation( clipTrace, start, end, ent->GetPhysics()->GetClipModel(),
1040                                                                         ent->GetPhysics()->GetClipModel()->GetAxis(), MASK_MONSTERSOLID, ent );
1041
1042                 // NOTE: could do (expensive) ledge detection here for when there is no AAS file
1043
1044                 trace.fraction = clipTrace.fraction;
1045                 trace.endPos = clipTrace.endpos;
1046                 trace.normal = clipTrace.c.normal;
1047                 trace.blockingEntity = gameLocal.entities[ clipTrace.c.entityNum ];
1048         } else {
1049                 aasTrace.getOutOfSolid = true;
1050                 if ( stopEvent & SE_ENTER_LEDGE_AREA ) {
1051                         aasTrace.flags |= AREA_LEDGE;
1052                 }
1053                 if ( stopEvent & SE_ENTER_OBSTACLE ) {
1054                         aasTrace.travelFlags |= TFL_INVALID;
1055                 }
1056
1057                 aas->Trace( aasTrace, start, end );
1058
1059                 gameLocal.clip.TranslationEntities( clipTrace, start, aasTrace.endpos, ent->GetPhysics()->GetClipModel(),
1060                                                                                         ent->GetPhysics()->GetClipModel()->GetAxis(), MASK_MONSTERSOLID, ent );
1061
1062                 if ( clipTrace.fraction >= 1.0f ) {
1063
1064                         trace.fraction = aasTrace.fraction;
1065                         trace.endPos = aasTrace.endpos;
1066                         trace.normal = aas->GetPlane( aasTrace.planeNum ).Normal();
1067                         trace.blockingEntity = gameLocal.world;
1068
1069                         if ( aasTrace.fraction < 1.0f ) {
1070                                 if ( stopEvent & SE_ENTER_LEDGE_AREA ) {
1071                                         if ( aas->AreaFlags( aasTrace.blockingAreaNum ) & AREA_LEDGE ) {
1072                                                 path.endPos = trace.endPos;
1073                                                 path.endNormal = trace.normal;
1074                                                 path.endEvent = SE_ENTER_LEDGE_AREA;
1075                                                 path.blockingEntity = trace.blockingEntity;
1076
1077                                                 if ( ai_debugMove.GetBool() ) {
1078                                                         gameRenderWorld->DebugLine( colorRed, start, aasTrace.endpos );
1079                                                 }
1080                                                 return true;
1081                                         }
1082                                 }
1083                                 if ( stopEvent & SE_ENTER_OBSTACLE ) {
1084                                         if ( aas->AreaTravelFlags( aasTrace.blockingAreaNum ) & TFL_INVALID ) {
1085                                                 path.endPos = trace.endPos;
1086                                                 path.endNormal = trace.normal;
1087                                                 path.endEvent = SE_ENTER_OBSTACLE;
1088                                                 path.blockingEntity = trace.blockingEntity;
1089
1090                                                 if ( ai_debugMove.GetBool() ) {
1091                                                         gameRenderWorld->DebugLine( colorRed, start, aasTrace.endpos );
1092                                                 }
1093                                                 return true;
1094                                         }
1095                                 }
1096                         }
1097                 } else {
1098                         trace.fraction = clipTrace.fraction;
1099                         trace.endPos = clipTrace.endpos;
1100                         trace.normal = clipTrace.c.normal;
1101                         trace.blockingEntity = gameLocal.entities[ clipTrace.c.entityNum ];
1102                 }
1103         }
1104
1105         if ( trace.fraction >= 1.0f ) {
1106                 trace.blockingEntity = NULL;
1107         }
1108
1109         return false;
1110 }
1111
1112 /*
1113 ============
1114 idAI::PredictPath
1115
1116   Can also be used when there is no AAS file available however ledges are not detected.
1117 ============
1118 */
1119 bool idAI::PredictPath( const idEntity *ent, const idAAS *aas, const idVec3 &start, const idVec3 &velocity, int totalTime, int frameTime, int stopEvent, predictedPath_t &path ) {
1120         int i, j, step, numFrames, curFrameTime;
1121         idVec3 delta, curStart, curEnd, curVelocity, lastEnd, stepUp, tmpStart;
1122         idVec3 gravity, gravityDir, invGravityDir;
1123         float maxStepHeight, minFloorCos;
1124         pathTrace_t trace;
1125
1126         if ( aas && aas->GetSettings() ) {
1127                 gravity = aas->GetSettings()->gravity;
1128                 gravityDir = aas->GetSettings()->gravityDir;
1129                 invGravityDir = aas->GetSettings()->invGravityDir;
1130                 maxStepHeight = aas->GetSettings()->maxStepHeight;
1131                 minFloorCos = aas->GetSettings()->minFloorCos;
1132         } else {
1133                 gravity = DEFAULT_GRAVITY_VEC3;
1134                 gravityDir = idVec3( 0, 0, -1 );
1135                 invGravityDir = idVec3( 0, 0, 1 );
1136                 maxStepHeight = 14.0f;
1137                 minFloorCos = 0.7f;
1138         }
1139
1140         path.endPos = start;
1141         path.endVelocity = velocity;
1142         path.endNormal.Zero();
1143         path.endEvent = 0;
1144         path.endTime = 0;
1145         path.blockingEntity = NULL;
1146
1147         curStart = start;
1148         curVelocity = velocity;
1149
1150         numFrames = ( totalTime + frameTime - 1 ) / frameTime;
1151         curFrameTime = frameTime;
1152         for ( i = 0; i < numFrames; i++ ) {
1153
1154                 if ( i == numFrames-1 ) {
1155                         curFrameTime = totalTime - i * curFrameTime;
1156                 }
1157
1158                 delta = curVelocity * curFrameTime * 0.001f;
1159
1160                 path.endVelocity = curVelocity;
1161                 path.endTime = i * frameTime;
1162
1163                 // allow sliding along a few surfaces per frame
1164                 for ( j = 0; j < MAX_FRAME_SLIDE; j++ ) {
1165
1166                         idVec3 lineStart = curStart;
1167
1168                         // allow stepping up three times per frame
1169                         for ( step = 0; step < 3; step++ ) {
1170
1171                                 curEnd = curStart + delta;
1172                                 if ( PathTrace( ent, aas, curStart, curEnd, stopEvent, trace, path ) ) {
1173                                         return true;
1174                                 }
1175
1176                                 if ( step ) {
1177
1178                                         // step down at end point
1179                                         tmpStart = trace.endPos;
1180                                         curEnd = tmpStart - stepUp;
1181                                         if ( PathTrace( ent, aas, tmpStart, curEnd, stopEvent, trace, path ) ) {
1182                                                 return true;
1183                                         }
1184
1185                                         // if not moved any further than without stepping up, or if not on a floor surface
1186                                         if ( (lastEnd - start).LengthSqr() > (trace.endPos - start).LengthSqr() - 0.1f ||
1187                                                                 ( trace.normal * invGravityDir ) < minFloorCos ) {
1188                                                 if ( stopEvent & SE_BLOCKED ) {
1189                                                         path.endPos = lastEnd;
1190                                                         path.endEvent = SE_BLOCKED;
1191
1192                                                         if ( ai_debugMove.GetBool() ) {
1193                                                                 gameRenderWorld->DebugLine( colorRed, lineStart, lastEnd );
1194                                                         }
1195
1196                                                         return true;
1197                                                 }
1198
1199                                                 curStart = lastEnd;
1200                                                 break;
1201                                         }
1202                                 }
1203
1204                                 path.endNormal = trace.normal;
1205                                 path.blockingEntity = trace.blockingEntity;
1206
1207                                 // if the trace is not blocked or blocked by a floor surface
1208                                 if ( trace.fraction >= 1.0f || ( trace.normal * invGravityDir ) > minFloorCos ) {
1209                                         curStart = trace.endPos;
1210                                         break;
1211                                 }
1212
1213                                 // save last result
1214                                 lastEnd = trace.endPos;
1215
1216                                 // step up
1217                                 stepUp = invGravityDir * maxStepHeight;
1218                                 if ( PathTrace( ent, aas, curStart, curStart + stepUp, stopEvent, trace, path ) ) {
1219                                         return true;
1220                                 }
1221                                 stepUp *= trace.fraction;
1222                                 curStart = trace.endPos;
1223                         }
1224
1225                         if ( ai_debugMove.GetBool() ) {
1226                                 gameRenderWorld->DebugLine( colorRed, lineStart, curStart );
1227                         }
1228
1229                         if ( trace.fraction >= 1.0f ) {
1230                                 break;
1231                         }
1232
1233                         delta.ProjectOntoPlane( trace.normal, OVERCLIP );
1234                         curVelocity.ProjectOntoPlane( trace.normal, OVERCLIP );
1235
1236                         if ( stopEvent & SE_BLOCKED ) {
1237                                 // if going backwards
1238                                 if ( (curVelocity - gravityDir * curVelocity * gravityDir ) *
1239                                                 (velocity - gravityDir * velocity * gravityDir) < 0.0f ) {
1240                                         path.endPos = curStart;
1241                                         path.endEvent = SE_BLOCKED;
1242
1243                                         return true;
1244                                 }
1245                         }
1246                 }
1247
1248                 if ( j >= MAX_FRAME_SLIDE ) {
1249                         if ( stopEvent & SE_BLOCKED ) {
1250                                 path.endPos = curStart;
1251                                 path.endEvent = SE_BLOCKED;
1252                                 return true;
1253                         }
1254                 }
1255
1256                 // add gravity
1257                 curVelocity += gravity * frameTime * 0.001f;
1258         }
1259
1260         path.endTime = totalTime;
1261         path.endVelocity = curVelocity;
1262         path.endPos = curStart;
1263         path.endEvent = 0;
1264
1265         return false;
1266 }
1267
1268
1269 /*
1270 ===============================================================================
1271
1272         Trajectory Prediction
1273
1274         Finds the best collision free trajectory for a clip model based on an
1275         initial position, target position and speed.
1276
1277 ===============================================================================
1278 */
1279
1280 /*
1281 =====================
1282 Ballistics
1283
1284   get the ideal aim pitch angle in order to hit the target
1285   also get the time it takes for the projectile to arrive at the target
1286 =====================
1287 */
1288 typedef struct ballistics_s {
1289         float                           angle;          // angle in degrees in the range [-180, 180]
1290         float                           time;           // time it takes before the projectile arrives
1291 } ballistics_t;
1292
1293 static int Ballistics( const idVec3 &start, const idVec3 &end, float speed, float gravity, ballistics_t bal[2] ) {
1294         int n, i;
1295         float x, y, a, b, c, d, sqrtd, inva, p[2];
1296
1297         x = ( end.ToVec2() - start.ToVec2() ).Length();
1298         y = end[2] - start[2];
1299
1300         a = 4.0f * y * y + 4.0f * x * x;
1301         b = -4.0f * speed * speed - 4.0f * y * gravity;
1302         c = gravity * gravity;
1303
1304         d = b * b - 4.0f * a * c;
1305         if ( d <= 0.0f || a == 0.0f ) {
1306                 return 0;
1307         }
1308         sqrtd = idMath::Sqrt( d );
1309         inva = 0.5f / a;
1310         p[0] = ( - b + sqrtd ) * inva;
1311         p[1] = ( - b - sqrtd ) * inva;
1312         n = 0;
1313         for ( i = 0; i < 2; i++ ) {
1314                 if ( p[i] <= 0.0f ) {
1315                         continue;
1316                 }
1317                 d = idMath::Sqrt( p[i] );
1318                 bal[n].angle = atan2( 0.5f * ( 2.0f * y * p[i] - gravity ) / d, d * x );
1319                 bal[n].time = x / ( cos( bal[n].angle ) * speed );
1320                 bal[n].angle = idMath::AngleNormalize180( RAD2DEG( bal[n].angle ) );
1321                 n++;
1322         }
1323
1324         return n;
1325 }
1326
1327 /*
1328 =====================
1329 HeightForTrajectory
1330
1331 Returns the maximum hieght of a given trajectory
1332 =====================
1333 */
1334 static float HeightForTrajectory( const idVec3 &start, float zVel, float gravity ) {
1335         float maxHeight, t;
1336
1337         t = zVel / gravity;
1338         // maximum height of projectile
1339         maxHeight = start.z - 0.5f * gravity * ( t * t );
1340
1341         return maxHeight;
1342 }
1343
1344 /*
1345 =====================
1346 idAI::TestTrajectory
1347 =====================
1348 */
1349 bool idAI::TestTrajectory( const idVec3 &start, const idVec3 &end, float zVel, float gravity, float time, float max_height, const idClipModel *clip, int clipmask, const idEntity *ignore, const idEntity *targetEntity, int drawtime ) {
1350         int i, numSegments;
1351         float maxHeight, t, t2;
1352         idVec3 points[5];
1353         trace_t trace;
1354         bool result;
1355
1356         t = zVel / gravity;
1357         // maximum height of projectile
1358         maxHeight = start.z - 0.5f * gravity * ( t * t );
1359         // time it takes to fall from the top to the end height
1360         t = idMath::Sqrt( ( maxHeight - end.z ) / ( 0.5f * -gravity ) );
1361
1362         // start of parabolic
1363         points[0] = start;
1364
1365         if ( t < time ) {
1366                 numSegments = 4;
1367                 // point in the middle between top and start
1368                 t2 = ( time - t ) * 0.5f;
1369                 points[1].ToVec2() = start.ToVec2() + (end.ToVec2() - start.ToVec2()) * ( t2 / time );
1370                 points[1].z = start.z + t2 * zVel + 0.5f * gravity * t2 * t2;
1371                 // top of parabolic
1372                 t2 = time - t;
1373                 points[2].ToVec2() = start.ToVec2() + (end.ToVec2() - start.ToVec2()) * ( t2 / time );
1374                 points[2].z = start.z + t2 * zVel + 0.5f * gravity * t2 * t2;
1375                 // point in the middel between top and end
1376                 t2 = time - t * 0.5f;
1377                 points[3].ToVec2() = start.ToVec2() + (end.ToVec2() - start.ToVec2()) * ( t2 / time );
1378                 points[3].z = start.z + t2 * zVel + 0.5f * gravity * t2 * t2;
1379         } else {
1380                 numSegments = 2;
1381                 // point halfway through
1382                 t2 = time * 0.5f;
1383                 points[1].ToVec2() = start.ToVec2() + ( end.ToVec2() - start.ToVec2() ) * 0.5f;
1384                 points[1].z = start.z + t2 * zVel + 0.5f * gravity * t2 * t2;
1385         }
1386
1387         // end of parabolic
1388         points[numSegments] = end;
1389
1390         if ( drawtime ) {
1391                 for ( i = 0; i < numSegments; i++ ) {
1392                         gameRenderWorld->DebugLine( colorRed, points[i], points[i+1], drawtime );
1393                 }
1394         }
1395
1396         // make sure projectile doesn't go higher than we want it to go
1397         for ( i = 0; i < numSegments; i++ ) {
1398                 if ( points[i].z > max_height ) {
1399                         // goes higher than we want to allow
1400                         return false;
1401                 }
1402         }
1403
1404         result = true;
1405         for ( i = 0; i < numSegments; i++ ) {
1406                 gameLocal.clip.Translation( trace, points[i], points[i+1], clip, mat3_identity, clipmask, ignore );
1407                 if ( trace.fraction < 1.0f ) {
1408                         if ( gameLocal.GetTraceEntity( trace ) == targetEntity ) {
1409                                 result = true;
1410                         } else {
1411                                 result = false;
1412                         }
1413                         break;
1414                 }
1415         }
1416
1417         if ( drawtime ) {
1418                 if ( clip ) {
1419                         gameRenderWorld->DebugBounds( result ? colorGreen : colorYellow, clip->GetBounds().Expand( 1.0f ), trace.endpos, drawtime );
1420                 } else {
1421                         idBounds bnds( trace.endpos );
1422                         bnds.ExpandSelf( 1.0f );
1423                         gameRenderWorld->DebugBounds( result ? colorGreen : colorYellow, bnds, vec3_zero, drawtime );
1424                 }
1425         }
1426
1427         return result;
1428 }
1429
1430 /*
1431 =====================
1432 idAI::PredictTrajectory
1433
1434   returns true if there is a collision free trajectory for the clip model
1435   aimDir is set to the ideal aim direction in order to hit the target
1436 =====================
1437 */
1438 bool idAI::PredictTrajectory( const idVec3 &firePos, const idVec3 &target, float projectileSpeed, const idVec3 &projGravity, const idClipModel *clip, int clipmask, float max_height, const idEntity *ignore, const idEntity *targetEntity, int drawtime, idVec3 &aimDir ) {
1439         int n, i, j;
1440         float zVel, a, t, pitch, s, c;
1441         trace_t trace;
1442         ballistics_t ballistics[2];
1443         idVec3 dir[2];
1444         idVec3 velocity;
1445         idVec3 lastPos, pos;
1446
1447         assert( targetEntity );
1448
1449         // check if the projectile starts inside the target
1450         if ( targetEntity->GetPhysics()->GetAbsBounds().IntersectsBounds( clip->GetBounds().Translate( firePos ) ) ) {
1451                 aimDir = target - firePos;
1452                 aimDir.Normalize();
1453                 return true;
1454         }
1455
1456         // if no velocity or the projectile is not affected by gravity
1457         if ( projectileSpeed <= 0.0f || projGravity == vec3_origin ) {
1458
1459                 aimDir = target - firePos;
1460                 aimDir.Normalize();
1461
1462                 gameLocal.clip.Translation( trace, firePos, target, clip, mat3_identity, clipmask, ignore );
1463
1464                 if ( drawtime ) {
1465                         gameRenderWorld->DebugLine( colorRed, firePos, target, drawtime );
1466                         idBounds bnds( trace.endpos );
1467                         bnds.ExpandSelf( 1.0f );
1468                         gameRenderWorld->DebugBounds( ( trace.fraction >= 1.0f || ( gameLocal.GetTraceEntity( trace ) == targetEntity ) ) ? colorGreen : colorYellow, bnds, vec3_zero, drawtime );
1469                 }
1470
1471                 return ( trace.fraction >= 1.0f || ( gameLocal.GetTraceEntity( trace ) == targetEntity ) );
1472         }
1473
1474         n = Ballistics( firePos, target, projectileSpeed, projGravity[2], ballistics );
1475         if ( n == 0 ) {
1476                 // there is no valid trajectory
1477                 aimDir = target - firePos;
1478                 aimDir.Normalize();
1479                 return false;
1480         }
1481
1482         // make sure the first angle is the smallest
1483         if ( n == 2 ) {
1484                 if ( ballistics[1].angle < ballistics[0].angle ) {
1485                         a = ballistics[0].angle; ballistics[0].angle = ballistics[1].angle; ballistics[1].angle = a;
1486                         t = ballistics[0].time; ballistics[0].time = ballistics[1].time; ballistics[1].time = t;
1487                 }
1488         }
1489
1490         // test if there is a collision free trajectory
1491         for ( i = 0; i < n; i++ ) {
1492                 pitch = DEG2RAD( ballistics[i].angle );
1493                 idMath::SinCos( pitch, s, c );
1494                 dir[i] = target - firePos;
1495                 dir[i].z = 0.0f;
1496                 dir[i] *= c * idMath::InvSqrt( dir[i].LengthSqr() );
1497                 dir[i].z = s;
1498
1499                 zVel = projectileSpeed * dir[i].z;
1500
1501                 if ( ai_debugTrajectory.GetBool() ) {
1502                         t = ballistics[i].time / 100.0f;
1503                         velocity = dir[i] * projectileSpeed;
1504                         lastPos = firePos;
1505                         pos = firePos;
1506                         for ( j = 1; j < 100; j++ ) {
1507                                 pos += velocity * t;
1508                                 velocity += projGravity * t;
1509                                 gameRenderWorld->DebugLine( colorCyan, lastPos, pos );
1510                                 lastPos = pos;
1511                         }
1512                 }
1513
1514                 if ( TestTrajectory( firePos, target, zVel, projGravity[2], ballistics[i].time, firePos.z + max_height, clip, clipmask, ignore, targetEntity, drawtime ) ) {
1515                         aimDir = dir[i];
1516                         return true;
1517                 }
1518         }
1519
1520         aimDir = dir[0];
1521
1522         // there is no collision free trajectory
1523         return false;
1524 }