neo/d3xp/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
 599         root = pathNodeAllocator.Alloc();
 600         root->Init();
 601         root->pos = startPos;
 602
 603         root->delta = seekPos - root->pos;
 604         root->numNodes = 0;
 605         pathNodeQueue.Add( root );
 606
 607         for ( node = pathNodeQueue.Get(); node && pathNodeAllocator.GetAllocCount() < MAX_PATH_NODES; node = pathNodeQueue.Get() ) {
 608
 609                 treeQueue.Add( node );
 610
 611                 // if this path has more than twice the number of nodes than the best path so far
 612                 if ( node->numNodes > bestNumNodes * 2 ) {
 613                         continue;
 614                 }
 615
 616                 // don't move outside of the clip bounds
 617                 idVec2 endPos = node->pos + node->delta;
 618                 if ( endPos.x - CLIP_BOUNDS_EPSILON < clipBounds[0].x || endPos.x + CLIP_BOUNDS_EPSILON > clipBounds[1].x ||
 619                                 endPos.y - CLIP_BOUNDS_EPSILON < clipBounds[0].y || endPos.y + CLIP_BOUNDS_EPSILON > clipBounds[1].y ) {
 620                         continue;
 621                 }
 622
 623                 // if an obstacle is blocking the path
 624                 if ( GetFirstBlockingObstacle( obstacles, numObstacles, node->obstacle, node->pos, node->delta, blockingScale, blockingObstacle, blockingEdgeNum ) ) {
 625
 626                         if ( path.firstObstacle == NULL ) {
 627                                 path.firstObstacle = obstacles[blockingObstacle].entity;
 628                         }
 629
 630                         node->delta *= blockingScale;
 631
 632                         if ( node->edgeNum == -1 ) {
 633                                 node->children[0] = pathNodeAllocator.Alloc();
 634                                 node->children[0]->Init();
 635                                 node->children[1] = pathNodeAllocator.Alloc();
 636                                 node->children[1]->Init();
 637                                 node->children[0]->dir = 0;
 638                                 node->children[1]->dir = 1;
 639                                 node->children[0]->parent = node->children[1]->parent = node;
 640                                 node->children[0]->pos = node->children[1]->pos = node->pos + node->delta;
 641                                 node->children[0]->obstacle = node->children[1]->obstacle = blockingObstacle;
 642                                 node->children[0]->edgeNum = node->children[1]->edgeNum = blockingEdgeNum;
 643                                 node->children[0]->numNodes = node->children[1]->numNodes = node->numNodes + 1;
 644                                 if ( GetPathNodeDelta( node->children[0], obstacles, seekPos, true ) ) {
 645                                         pathNodeQueue.Add( node->children[0] );
 646                                 }
 647                                 if ( GetPathNodeDelta( node->children[1], obstacles, seekPos, true ) ) {
 648                                         pathNodeQueue.Add( node->children[1] );
 649                                 }
 650                         } else {
 651                                 node->children[node->dir] = child = pathNodeAllocator.Alloc();
 652                                 child->Init();
 653                                 child->dir = node->dir;
 654                                 child->parent = node;
 655                                 child->pos = node->pos + node->delta;
 656                                 child->obstacle = blockingObstacle;
 657                                 child->edgeNum = blockingEdgeNum;
 658                                 child->numNodes = node->numNodes + 1;
 659                                 if ( GetPathNodeDelta( child, obstacles, seekPos, true ) ) {
 660                                         pathNodeQueue.Add( child );
 661                                 }
 662                         }
 663                 } else {
 664                         node->children[node->dir] = child = pathNodeAllocator.Alloc();
 665                         child->Init();
 666                         child->dir = node->dir;
 667                         child->parent = node;
 668                         child->pos = node->pos + node->delta;
 669                         child->numNodes = node->numNodes + 1;
 670
 671                         // there is a free path towards goal
 672                         if ( node->edgeNum == -1 ) {
 673                                 if ( node->numNodes < bestNumNodes ) {
 674                                         bestNumNodes = node->numNodes;
 675                                 }
 676                                 continue;
 677                         }
 678
 679                         child->obstacle = node->obstacle;
 680                         obstaclePoints = obstacles[node->obstacle].winding.GetNumPoints();
 681                         child->edgeNum = ( node->edgeNum + obstaclePoints + ( 2 * node->dir - 1 ) ) % obstaclePoints;
 682
 683                         if ( GetPathNodeDelta( child, obstacles, seekPos, false ) ) {
 684                                 pathNodeQueue.Add( child );
 685                         }
 686                 }
 687         }
 688
 689         return root;
 690 }
 691
 692 /*
 693 ============
 694 PrunePathTree
 695 ============
 696 */
 697 void PrunePathTree( pathNode_t *root, const idVec2 &seekPos ) {
 698         int i;
 699         float bestDist;
 700         pathNode_t *node, *lastNode, *n, *bestNode;
 701
 702         node = root;
 703         while( node ) {
 704
 705                 node->dist = ( seekPos - node->pos ).LengthSqr();
 706
 707                 if ( node->children[0] ) {
 708                         node = node->children[0];
 709                 } else if ( node->children[1] ) {
 710                         node = node->children[1];
 711                 } else {
 712
 713                         // find the node closest to the goal along this path
 714                         bestDist = idMath::INFINITY;
 715                         bestNode = node;
 716                         for ( n = node; n; n = n->parent ) {
 717                                 if ( n->children[0] && n->children[1] ) {
 718                                         break;
 719                                 }
 720                                 if ( n->dist < bestDist ) {
 721                                         bestDist = n->dist;
 722                                         bestNode = n;
 723                                 }
 724                         }
 725
 726                         // free tree down from the best node
 727                         for ( i = 0; i < 2; i++ ) {
 728                                 if ( bestNode->children[i] ) {
 729                                         FreePathTree_r( bestNode->children[i] );
 730                                         bestNode->children[i] = NULL;
 731                                 }
 732                         }
 733
 734                         for ( lastNode = bestNode, node = bestNode->parent; node; lastNode = node, node = node->parent ) {
 735                                 if ( node->children[1] && ( node->children[1] != lastNode ) ) {
 736                                         node = node->children[1];
 737                                         break;
 738                                 }
 739                         }
 740                 }
 741         }
 742 }
 743
 744 /*
 745 ============
 746 OptimizePath
 747 ============
 748 */
 749 int OptimizePath( const pathNode_t *root, const pathNode_t *leafNode, const obstacle_t *obstacles, int numObstacles, idVec2 optimizedPath[MAX_OBSTACLE_PATH] ) {
 750         int i, numPathPoints, edgeNums[2];
 751         const pathNode_t *curNode, *nextNode;
 752         idVec2 curPos, curDelta, bounds[2];
 753         float scale1, scale2, curLength;
 754
 755         optimizedPath[0] = root->pos;
 756         numPathPoints = 1;
 757
 758         for ( nextNode = curNode = root; curNode != leafNode; curNode = nextNode ) {
 759
 760                 for ( nextNode = leafNode; nextNode->parent != curNode; nextNode = nextNode->parent ) {
 761
 762                         // can only take shortcuts when going from one object to another
 763                         if ( nextNode->obstacle == curNode->obstacle ) {
 764                                 continue;
 765                         }
 766
 767                         curPos = curNode->pos;
 768                         curDelta = nextNode->pos - curPos;
 769                         curLength = curDelta.Length();
 770
 771                         // get bounds for the current movement delta
 772                         bounds[0] = curPos - idVec2( CM_BOX_EPSILON, CM_BOX_EPSILON );
 773                         bounds[1] = curPos + idVec2( CM_BOX_EPSILON, CM_BOX_EPSILON );
 774                         bounds[FLOATSIGNBITNOTSET(curDelta.x)].x += curDelta.x;
 775                         bounds[FLOATSIGNBITNOTSET(curDelta.y)].y += curDelta.y;
 776
 777                         // test if the shortcut intersects with any obstacles
 778                         for ( i = 0; i < numObstacles; i++ ) {
 779                                 if ( bounds[0].x > obstacles[i].bounds[1].x || bounds[0].y > obstacles[i].bounds[1].y ||
 780                                                 bounds[1].x < obstacles[i].bounds[0].x || bounds[1].y < obstacles[i].bounds[0].y ) {
 781                                         continue;
 782                                 }
 783                                 if ( obstacles[i].winding.RayIntersection( curPos, curDelta, scale1, scale2, edgeNums ) ) {
 784                                         if ( scale1 >= 0.0f && scale1 <= 1.0f && ( i != nextNode->obstacle || scale1 * curLength < curLength - 0.5f ) ) {
 785                                                 break;
 786                                         }
 787                                         if ( scale2 >= 0.0f && scale2 <= 1.0f && ( i != nextNode->obstacle || scale2 * curLength < curLength - 0.5f ) ) {
 788                                                 break;
 789                                         }
 790                                 }
 791                         }
 792                         if ( i >= numObstacles ) {
 793                                 break;
 794                         }
 795                 }
 796
 797                 // store the next position along the optimized path
 798                 optimizedPath[numPathPoints++] = nextNode->pos;
 799         }
 800
 801         return numPathPoints;
 802 }
 803
 804 /*
 805 ============
 806 PathLength
 807 ============
 808 */
 809 float PathLength( idVec2 optimizedPath[MAX_OBSTACLE_PATH], int numPathPoints, const idVec2 &curDir ) {
 810         int i;
 811         float pathLength;
 812
 813         // calculate the path length
 814         pathLength = 0.0f;
 815         for ( i = 0; i < numPathPoints-1; i++ ) {
 816                 pathLength += ( optimizedPath[i+1] - optimizedPath[i] ).LengthFast();
 817         }
 818
 819         // add penalty if this path does not go in the current direction
 820         if ( curDir * ( optimizedPath[1] - optimizedPath[0] ) < 0.0f ) {
 821                 pathLength += 100.0f;
 822         }
 823         return pathLength;
 824 }
 825
 826 /*
 827 ============
 828 FindOptimalPath
 829
 830   Returns true if there is a path all the way to the goal.
 831 ============
 832 */
 833 bool FindOptimalPath( const pathNode_t *root, const obstacle_t *obstacles, int numObstacles, const float height, const idVec3 &curDir, idVec3 &seekPos ) {
 834         int i, numPathPoints, bestNumPathPoints;
 835         const pathNode_t *node, *lastNode, *bestNode;
 836         idVec2 optimizedPath[MAX_OBSTACLE_PATH];
 837         float pathLength, bestPathLength;
 838         bool pathToGoalExists, optimizedPathCalculated;
 839
 840         seekPos.Zero();
 841         seekPos.z = height;
 842
 843         pathToGoalExists = false;
 844         optimizedPathCalculated = false;
 845
 846         bestNode = root;
 847         bestNumPathPoints = 0;
 848         bestPathLength = idMath::INFINITY;
 849
 850         node = root;
 851         while( node ) {
 852
 853                 pathToGoalExists |= ( node->dist < 0.1f );
 854
 855                 if ( node->dist <= bestNode->dist ) {
 856
 857                         if ( idMath::Fabs( node->dist - bestNode->dist ) < 0.1f ) {
 858
 859                                 if ( !optimizedPathCalculated ) {
 860                                         bestNumPathPoints = OptimizePath( root, bestNode, obstacles, numObstacles, optimizedPath );
 861                                         bestPathLength = PathLength( optimizedPath, bestNumPathPoints, curDir.ToVec2() );
 862                                         seekPos.ToVec2() = optimizedPath[1];
 863                                 }
 864
 865                                 numPathPoints = OptimizePath( root, node, obstacles, numObstacles, optimizedPath );
 866                                 pathLength = PathLength( optimizedPath, numPathPoints, curDir.ToVec2() );
 867
 868                                 if ( pathLength < bestPathLength ) {
 869                                         bestNode = node;
 870                                         bestNumPathPoints = numPathPoints;
 871                                         bestPathLength = pathLength;
 872                                         seekPos.ToVec2() = optimizedPath[1];
 873                                 }
 874                                 optimizedPathCalculated = true;
 875
 876                         } else {
 877
 878                                 bestNode = node;
 879                                 optimizedPathCalculated = false;
 880                         }
 881                 }
 882
 883                 if ( node->children[0] ) {
 884                         node = node->children[0];
 885                 } else if ( node->children[1] ) {
 886                         node = node->children[1];
 887                 } else {
 888                         for ( lastNode = node, node = node->parent; node; lastNode = node, node = node->parent ) {
 889                                 if ( node->children[1] && node->children[1] != lastNode ) {
 890                                         node = node->children[1];
 891                                         break;
 892                                 }
 893                         }
 894                 }
 895         }
 896
 897         if ( !pathToGoalExists ) {
 898                 seekPos.ToVec2() = root->children[0]->pos;
 899         } else if ( !optimizedPathCalculated ) {
 900                 OptimizePath( root, bestNode, obstacles, numObstacles, optimizedPath );
 901                 seekPos.ToVec2() = optimizedPath[1];
 902         }
 903
 904         if ( ai_showObstacleAvoidance.GetBool() ) {
 905                 idVec3 start, end;
 906                 start.z = end.z = height + 4.0f;
 907                 numPathPoints = OptimizePath( root, bestNode, obstacles, numObstacles, optimizedPath );
 908                 for ( i = 0; i < numPathPoints-1; i++ ) {
 909                         start.ToVec2() = optimizedPath[i];
 910                         end.ToVec2() = optimizedPath[i+1];
 911                         gameRenderWorld->DebugArrow( colorCyan, start, end, 1 );
 912                 }
 913         }
 914
 915         return pathToGoalExists;
 916 }
 917
 918 /*
 919 ============
 920 idAI::FindPathAroundObstacles
 921
 922   Finds a path around dynamic obstacles using a path tree with clockwise and counter clockwise edge walks.
 923 ============
 924 */
 925 bool idAI::FindPathAroundObstacles( const idPhysics *physics, const idAAS *aas, const idEntity *ignore, const idVec3 &startPos, const idVec3 &seekPos, obstaclePath_t &path ) {
 926         int numObstacles, areaNum, insideObstacle;
 927         obstacle_t obstacles[MAX_OBSTACLES];
 928         idBounds clipBounds;
 929         idBounds bounds;
 930         pathNode_t *root;
 931         bool pathToGoalExists;
 932
 933         path.seekPos = seekPos;
 934         path.firstObstacle = NULL;
 935         path.startPosOutsideObstacles = startPos;
 936         path.startPosObstacle = NULL;
 937         path.seekPosOutsideObstacles = seekPos;
 938         path.seekPosObstacle = NULL;
 939
 940         if ( !aas ) {
 941                 return true;
 942         }
 943
 944         bounds[1] = aas->GetSettings()->boundingBoxes[0][1];
 945         bounds[0] = -bounds[1];
 946         bounds[1].z = 32.0f;
 947
 948         // get the AAS area number and a valid point inside that area
 949         areaNum = aas->PointReachableAreaNum( path.startPosOutsideObstacles, bounds, (AREA_REACHABLE_WALK|AREA_REACHABLE_FLY) );
 950         aas->PushPointIntoAreaNum( areaNum, path.startPosOutsideObstacles );
 951
 952         // get all the nearby obstacles
 953         numObstacles = GetObstacles( physics, aas, ignore, areaNum, path.startPosOutsideObstacles, path.seekPosOutsideObstacles, obstacles, MAX_OBSTACLES, clipBounds );
 954
 955         // get a source position outside the obstacles
 956         GetPointOutsideObstacles( obstacles, numObstacles, path.startPosOutsideObstacles.ToVec2(), &insideObstacle, NULL );
 957         if ( insideObstacle != -1 ) {
 958                 path.startPosObstacle = obstacles[insideObstacle].entity;
 959         }
 960
 961         // get a goal position outside the obstacles
 962         GetPointOutsideObstacles( obstacles, numObstacles, path.seekPosOutsideObstacles.ToVec2(), &insideObstacle, NULL );
 963         if ( insideObstacle != -1 ) {
 964                 path.seekPosObstacle = obstacles[insideObstacle].entity;
 965         }
 966
 967         // if start and destination are pushed to the same point, we don't have a path around the obstacle
 968         if ( ( path.seekPosOutsideObstacles.ToVec2() - path.startPosOutsideObstacles.ToVec2() ).LengthSqr() < Square( 1.0f ) ) {
 969                 if ( ( seekPos.ToVec2() - startPos.ToVec2() ).LengthSqr() > Square( 2.0f ) ) {
 970                         return false;
 971                 }
 972         }
 973
 974         // build a path tree
 975         root = BuildPathTree( obstacles, numObstacles, clipBounds, path.startPosOutsideObstacles.ToVec2(), path.seekPosOutsideObstacles.ToVec2(), path );
 976
 977         // draw the path tree
 978         if ( ai_showObstacleAvoidance.GetBool() ) {
 979                 DrawPathTree( root, physics->GetOrigin().z );
 980         }
 981
 982         // prune the tree
 983         PrunePathTree( root, path.seekPosOutsideObstacles.ToVec2() );
 984
 985         // find the optimal path
 986         pathToGoalExists = FindOptimalPath( root, obstacles, numObstacles, physics->GetOrigin().z, physics->GetLinearVelocity(), path.seekPos );
 987
 988         // free the tree
 989         FreePathTree_r( root );
 990
 991         return pathToGoalExists;
 992 }
 993
 994 /*
 995 ============
 996 idAI::FreeObstacleAvoidanceNodes
 997 ============
 998 */
 999 void idAI::FreeObstacleAvoidanceNodes( void ) {
1000         pathNodeAllocator.Shutdown();
1001 }
1002
1003
1004 /*
1005 ===============================================================================
1006
1007         Path Prediction
1008
1009         Uses the AAS to quickly and accurately predict a path for a certain
1010         period of time based on an initial position and velocity.
1011
1012 ===============================================================================
1013 */
1014
1015 const float OVERCLIP                    = 1.001f;
1016 const int MAX_FRAME_SLIDE               = 5;
1017
1018 typedef struct pathTrace_s {
1019         float                                   fraction;
1020         idVec3                                  endPos;
1021         idVec3                                  normal;
1022         const idEntity *                blockingEntity;
1023 } pathTrace_t;
1024
1025 /*
1026 ============
1027 PathTrace
1028
1029   Returns true if a stop event was triggered.
1030 ============
1031 */
1032 bool PathTrace( const idEntity *ent, const idAAS *aas, const idVec3 &start, const idVec3 &end, int stopEvent, struct pathTrace_s &trace, predictedPath_t &path ) {
1033         trace_t clipTrace;
1034         aasTrace_t aasTrace;
1035
1036         memset( &trace, 0, sizeof( trace ) );
1037
1038         if ( !aas || !aas->GetSettings() ) {
1039
1040                 gameLocal.clip.Translation( clipTrace, start, end, ent->GetPhysics()->GetClipModel(),
1041                                                                         ent->GetPhysics()->GetClipModel()->GetAxis(), MASK_MONSTERSOLID, ent );
1042
1043                 // NOTE: could do (expensive) ledge detection here for when there is no AAS file
1044
1045                 trace.fraction = clipTrace.fraction;
1046                 trace.endPos = clipTrace.endpos;
1047                 trace.normal = clipTrace.c.normal;
1048                 trace.blockingEntity = gameLocal.entities[ clipTrace.c.entityNum ];
1049         } else {
1050                 aasTrace.getOutOfSolid = true;
1051                 if ( stopEvent & SE_ENTER_LEDGE_AREA ) {
1052                         aasTrace.flags |= AREA_LEDGE;
1053                 }
1054                 if ( stopEvent & SE_ENTER_OBSTACLE ) {
1055                         aasTrace.travelFlags |= TFL_INVALID;
1056                 }
1057
1058                 aas->Trace( aasTrace, start, end );
1059
1060                 gameLocal.clip.TranslationEntities( clipTrace, start, aasTrace.endpos, ent->GetPhysics()->GetClipModel(),
1061                                                                                         ent->GetPhysics()->GetClipModel()->GetAxis(), MASK_MONSTERSOLID, ent );
1062
1063                 if ( clipTrace.fraction >= 1.0f ) {
1064
1065                         trace.fraction = aasTrace.fraction;
1066                         trace.endPos = aasTrace.endpos;
1067                         trace.normal = aas->GetPlane( aasTrace.planeNum ).Normal();
1068                         trace.blockingEntity = gameLocal.world;
1069
1070                         if ( aasTrace.fraction < 1.0f ) {
1071                                 if ( stopEvent & SE_ENTER_LEDGE_AREA ) {
1072                                         if ( aas->AreaFlags( aasTrace.blockingAreaNum ) & AREA_LEDGE ) {
1073                                                 path.endPos = trace.endPos;
1074                                                 path.endNormal = trace.normal;
1075                                                 path.endEvent = SE_ENTER_LEDGE_AREA;
1076                                                 path.blockingEntity = trace.blockingEntity;
1077
1078                                                 if ( ai_debugMove.GetBool() ) {
1079                                                         gameRenderWorld->DebugLine( colorRed, start, aasTrace.endpos );
1080                                                 }
1081                                                 return true;
1082                                         }
1083                                 }
1084                                 if ( stopEvent & SE_ENTER_OBSTACLE ) {
1085                                         if ( aas->AreaTravelFlags( aasTrace.blockingAreaNum ) & TFL_INVALID ) {
1086                                                 path.endPos = trace.endPos;
1087                                                 path.endNormal = trace.normal;
1088                                                 path.endEvent = SE_ENTER_OBSTACLE;
1089                                                 path.blockingEntity = trace.blockingEntity;
1090
1091                                                 if ( ai_debugMove.GetBool() ) {
1092                                                         gameRenderWorld->DebugLine( colorRed, start, aasTrace.endpos );
1093                                                 }
1094                                                 return true;
1095                                         }
1096                                 }
1097                         }
1098                 } else {
1099                         trace.fraction = clipTrace.fraction;
1100                         trace.endPos = clipTrace.endpos;
1101                         trace.normal = clipTrace.c.normal;
1102                         trace.blockingEntity = gameLocal.entities[ clipTrace.c.entityNum ];
1103                 }
1104         }
1105
1106         if ( trace.fraction >= 1.0f ) {
1107                 trace.blockingEntity = NULL;
1108         }
1109
1110         return false;
1111 }
1112
1113 /*
1114 ============
1115 idAI::PredictPath
1116
1117   Can also be used when there is no AAS file available however ledges are not detected.
1118 ============
1119 */
1120 bool idAI::PredictPath( const idEntity *ent, const idAAS *aas, const idVec3 &start, const idVec3 &velocity, int totalTime, int frameTime, int stopEvent, predictedPath_t &path ) {
1121         int i, j, step, numFrames, curFrameTime;
1122         idVec3 delta, curStart, curEnd, curVelocity, lastEnd, stepUp, tmpStart;
1123         idVec3 gravity, gravityDir, invGravityDir;
1124         float maxStepHeight, minFloorCos;
1125         pathTrace_t trace;
1126
1127         if ( aas && aas->GetSettings() ) {
1128                 gravity = aas->GetSettings()->gravity;
1129                 gravityDir = aas->GetSettings()->gravityDir;
1130                 invGravityDir = aas->GetSettings()->invGravityDir;
1131                 maxStepHeight = aas->GetSettings()->maxStepHeight;
1132                 minFloorCos = aas->GetSettings()->minFloorCos;
1133         } else {
1134                 gravity = DEFAULT_GRAVITY_VEC3;
1135                 gravityDir = idVec3( 0, 0, -1 );
1136                 invGravityDir = idVec3( 0, 0, 1 );
1137                 maxStepHeight = 14.0f;
1138                 minFloorCos = 0.7f;
1139         }
1140
1141         path.endPos = start;
1142         path.endVelocity = velocity;
1143         path.endNormal.Zero();
1144         path.endEvent = 0;
1145         path.endTime = 0;
1146         path.blockingEntity = NULL;
1147
1148         curStart = start;
1149         curVelocity = velocity;
1150
1151         numFrames = ( totalTime + frameTime - 1 ) / frameTime;
1152         curFrameTime = frameTime;
1153         for ( i = 0; i < numFrames; i++ ) {
1154
1155                 if ( i == numFrames-1 ) {
1156                         curFrameTime = totalTime - i * curFrameTime;
1157                 }
1158
1159                 delta = curVelocity * curFrameTime * 0.001f;
1160
1161                 path.endVelocity = curVelocity;
1162                 path.endTime = i * frameTime;
1163
1164                 // allow sliding along a few surfaces per frame
1165                 for ( j = 0; j < MAX_FRAME_SLIDE; j++ ) {
1166
1167                         idVec3 lineStart = curStart;
1168
1169                         // allow stepping up three times per frame
1170                         for ( step = 0; step < 3; step++ ) {
1171
1172                                 curEnd = curStart + delta;
1173                                 if ( PathTrace( ent, aas, curStart, curEnd, stopEvent, trace, path ) ) {
1174                                         return true;
1175                                 }
1176
1177                                 if ( step ) {
1178
1179                                         // step down at end point
1180                                         tmpStart = trace.endPos;
1181                                         curEnd = tmpStart - stepUp;
1182                                         if ( PathTrace( ent, aas, tmpStart, curEnd, stopEvent, trace, path ) ) {
1183                                                 return true;
1184                                         }
1185
1186                                         // if not moved any further than without stepping up, or if not on a floor surface
1187                                         if ( (lastEnd - start).LengthSqr() > (trace.endPos - start).LengthSqr() - 0.1f ||
1188                                                                 ( trace.normal * invGravityDir ) < minFloorCos ) {
1189                                                 if ( stopEvent & SE_BLOCKED ) {
1190                                                         path.endPos = lastEnd;
1191                                                         path.endEvent = SE_BLOCKED;
1192
1193                                                         if ( ai_debugMove.GetBool() ) {
1194                                                                 gameRenderWorld->DebugLine( colorRed, lineStart, lastEnd );
1195                                                         }
1196
1197                                                         return true;
1198                                                 }
1199
1200                                                 curStart = lastEnd;
1201                                                 break;
1202                                         }
1203                                 }
1204
1205                                 path.endNormal = trace.normal;
1206                                 path.blockingEntity = trace.blockingEntity;
1207
1208                                 // if the trace is not blocked or blocked by a floor surface
1209                                 if ( trace.fraction >= 1.0f || ( trace.normal * invGravityDir ) > minFloorCos ) {
1210                                         curStart = trace.endPos;
1211                                         break;
1212                                 }
1213
1214                                 // save last result
1215                                 lastEnd = trace.endPos;
1216
1217                                 // step up
1218                                 stepUp = invGravityDir * maxStepHeight;
1219                                 if ( PathTrace( ent, aas, curStart, curStart + stepUp, stopEvent, trace, path ) ) {
1220                                         return true;
1221                                 }
1222                                 stepUp *= trace.fraction;
1223                                 curStart = trace.endPos;
1224                         }
1225
1226                         if ( ai_debugMove.GetBool() ) {
1227                                 gameRenderWorld->DebugLine( colorRed, lineStart, curStart );
1228                         }
1229
1230                         if ( trace.fraction >= 1.0f ) {
1231                                 break;
1232                         }
1233
1234                         delta.ProjectOntoPlane( trace.normal, OVERCLIP );
1235                         curVelocity.ProjectOntoPlane( trace.normal, OVERCLIP );
1236
1237                         if ( stopEvent & SE_BLOCKED ) {
1238                                 // if going backwards
1239                                 if ( (curVelocity - gravityDir * curVelocity * gravityDir ) *
1240                                                 (velocity - gravityDir * velocity * gravityDir) < 0.0f ) {
1241                                         path.endPos = curStart;
1242                                         path.endEvent = SE_BLOCKED;
1243
1244                                         return true;
1245                                 }
1246                         }
1247                 }
1248
1249                 if ( j >= MAX_FRAME_SLIDE ) {
1250                         if ( stopEvent & SE_BLOCKED ) {
1251                                 path.endPos = curStart;
1252                                 path.endEvent = SE_BLOCKED;
1253                                 return true;
1254                         }
1255                 }
1256
1257                 // add gravity
1258                 curVelocity += gravity * frameTime * 0.001f;
1259         }
1260
1261         path.endTime = totalTime;
1262         path.endVelocity = curVelocity;
1263         path.endPos = curStart;
1264         path.endEvent = 0;
1265
1266         return false;
1267 }
1268
1269
1270 /*
1271 ===============================================================================
1272
1273         Trajectory Prediction
1274
1275         Finds the best collision free trajectory for a clip model based on an
1276         initial position, target position and speed.
1277
1278 ===============================================================================
1279 */
1280
1281 /*
1282 =====================
1283 Ballistics
1284
1285   get the ideal aim pitch angle in order to hit the target
1286   also get the time it takes for the projectile to arrive at the target
1287 =====================
1288 */
1289 typedef struct ballistics_s {
1290         float                           angle;          // angle in degrees in the range [-180, 180]
1291         float                           time;           // time it takes before the projectile arrives
1292 } ballistics_t;
1293
1294 static int Ballistics( const idVec3 &start, const idVec3 &end, float speed, float gravity, ballistics_t bal[2] ) {
1295         int n, i;
1296         float x, y, a, b, c, d, sqrtd, inva, p[2];
1297
1298         x = ( end.ToVec2() - start.ToVec2() ).Length();
1299         y = end[2] - start[2];
1300
1301         a = 4.0f * y * y + 4.0f * x * x;
1302         b = -4.0f * speed * speed - 4.0f * y * gravity;
1303         c = gravity * gravity;
1304
1305         d = b * b - 4.0f * a * c;
1306         if ( d <= 0.0f || a == 0.0f ) {
1307                 return 0;
1308         }
1309         sqrtd = idMath::Sqrt( d );
1310         inva = 0.5f / a;
1311         p[0] = ( - b + sqrtd ) * inva;
1312         p[1] = ( - b - sqrtd ) * inva;
1313         n = 0;
1314         for ( i = 0; i < 2; i++ ) {
1315                 if ( p[i] <= 0.0f ) {
1316                         continue;
1317                 }
1318                 d = idMath::Sqrt( p[i] );
1319                 bal[n].angle = atan2( 0.5f * ( 2.0f * y * p[i] - gravity ) / d, d * x );
1320                 bal[n].time = x / ( cos( bal[n].angle ) * speed );
1321                 bal[n].angle = idMath::AngleNormalize180( RAD2DEG( bal[n].angle ) );
1322                 n++;
1323         }
1324
1325         return n;
1326 }
1327
1328 /*
1329 =====================
1330 HeightForTrajectory
1331
1332 Returns the maximum hieght of a given trajectory
1333 =====================
1334 */
1335 static float HeightForTrajectory( const idVec3 &start, float zVel, float gravity ) {
1336         float maxHeight, t;
1337
1338         t = zVel / gravity;
1339         // maximum height of projectile
1340         maxHeight = start.z - 0.5f * gravity * ( t * t );
1341
1342         return maxHeight;
1343 }
1344
1345 /*
1346 =====================
1347 idAI::TestTrajectory
1348 =====================
1349 */
1350 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 ) {
1351         int i, numSegments;
1352         float maxHeight, t, t2;
1353         idVec3 points[5];
1354         trace_t trace;
1355         bool result;
1356
1357         t = zVel / gravity;
1358         // maximum height of projectile
1359         maxHeight = start.z - 0.5f * gravity * ( t * t );
1360         // time it takes to fall from the top to the end height
1361         t = idMath::Sqrt( ( maxHeight - end.z ) / ( 0.5f * -gravity ) );
1362
1363         // start of parabolic
1364         points[0] = start;
1365
1366         if ( t < time ) {
1367                 numSegments = 4;
1368                 // point in the middle between top and start
1369                 t2 = ( time - t ) * 0.5f;
1370                 points[1].ToVec2() = start.ToVec2() + (end.ToVec2() - start.ToVec2()) * ( t2 / time );
1371                 points[1].z = start.z + t2 * zVel + 0.5f * gravity * t2 * t2;
1372                 // top of parabolic
1373                 t2 = time - t;
1374                 points[2].ToVec2() = start.ToVec2() + (end.ToVec2() - start.ToVec2()) * ( t2 / time );
1375                 points[2].z = start.z + t2 * zVel + 0.5f * gravity * t2 * t2;
1376                 // point in the middel between top and end
1377                 t2 = time - t * 0.5f;
1378                 points[3].ToVec2() = start.ToVec2() + (end.ToVec2() - start.ToVec2()) * ( t2 / time );
1379                 points[3].z = start.z + t2 * zVel + 0.5f * gravity * t2 * t2;
1380         } else {
1381                 numSegments = 2;
1382                 // point halfway through
1383                 t2 = time * 0.5f;
1384                 points[1].ToVec2() = start.ToVec2() + ( end.ToVec2() - start.ToVec2() ) * 0.5f;
1385                 points[1].z = start.z + t2 * zVel + 0.5f * gravity * t2 * t2;
1386         }
1387
1388         // end of parabolic
1389         points[numSegments] = end;
1390
1391         if ( drawtime ) {
1392                 for ( i = 0; i < numSegments; i++ ) {
1393                         gameRenderWorld->DebugLine( colorRed, points[i], points[i+1], drawtime );
1394                 }
1395         }
1396
1397         // make sure projectile doesn't go higher than we want it to go
1398         for ( i = 0; i < numSegments; i++ ) {
1399                 if ( points[i].z > max_height ) {
1400                         // goes higher than we want to allow
1401                         return false;
1402                 }
1403         }
1404
1405         result = true;
1406         for ( i = 0; i < numSegments; i++ ) {
1407                 gameLocal.clip.Translation( trace, points[i], points[i+1], clip, mat3_identity, clipmask, ignore );
1408                 if ( trace.fraction < 1.0f ) {
1409                         if ( gameLocal.GetTraceEntity( trace ) == targetEntity ) {
1410                                 result = true;
1411                         } else {
1412                                 result = false;
1413                         }
1414                         break;
1415                 }
1416         }
1417
1418         if ( drawtime ) {
1419                 if ( clip ) {
1420                         gameRenderWorld->DebugBounds( result ? colorGreen : colorYellow, clip->GetBounds().Expand( 1.0f ), trace.endpos, drawtime );
1421                 } else {
1422                         idBounds bnds( trace.endpos );
1423                         bnds.ExpandSelf( 1.0f );
1424                         gameRenderWorld->DebugBounds( result ? colorGreen : colorYellow, bnds, vec3_zero, drawtime );
1425                 }
1426         }
1427
1428         return result;
1429 }
1430
1431 /*
1432 =====================
1433 idAI::PredictTrajectory
1434
1435   returns true if there is a collision free trajectory for the clip model
1436   aimDir is set to the ideal aim direction in order to hit the target
1437 =====================
1438 */
1439 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 ) {
1440         int n, i, j;
1441         float zVel, a, t, pitch, s, c;
1442         trace_t trace;
1443         ballistics_t ballistics[2];
1444         idVec3 dir[2];
1445         idVec3 velocity;
1446         idVec3 lastPos, pos;
1447
1448         assert( targetEntity );
1449
1450         // check if the projectile starts inside the target
1451         if ( targetEntity->GetPhysics()->GetAbsBounds().IntersectsBounds( clip->GetBounds().Translate( firePos ) ) ) {
1452                 aimDir = target - firePos;
1453                 aimDir.Normalize();
1454                 return true;
1455         }
1456
1457         // if no velocity or the projectile is not affected by gravity
1458         if ( projectileSpeed <= 0.0f || projGravity == vec3_origin ) {
1459
1460                 aimDir = target - firePos;
1461                 aimDir.Normalize();
1462
1463                 gameLocal.clip.Translation( trace, firePos, target, clip, mat3_identity, clipmask, ignore );
1464
1465                 if ( drawtime ) {
1466                         gameRenderWorld->DebugLine( colorRed, firePos, target, drawtime );
1467                         idBounds bnds( trace.endpos );
1468                         bnds.ExpandSelf( 1.0f );
1469                         gameRenderWorld->DebugBounds( ( trace.fraction >= 1.0f || ( gameLocal.GetTraceEntity( trace ) == targetEntity ) ) ? colorGreen : colorYellow, bnds, vec3_zero, drawtime );
1470                 }
1471
1472                 return ( trace.fraction >= 1.0f || ( gameLocal.GetTraceEntity( trace ) == targetEntity ) );
1473         }
1474
1475         n = Ballistics( firePos, target, projectileSpeed, projGravity[2], ballistics );
1476         if ( n == 0 ) {
1477                 // there is no valid trajectory
1478                 aimDir = target - firePos;
1479                 aimDir.Normalize();
1480                 return false;
1481         }
1482
1483         // make sure the first angle is the smallest
1484         if ( n == 2 ) {
1485                 if ( ballistics[1].angle < ballistics[0].angle ) {
1486                         a = ballistics[0].angle; ballistics[0].angle = ballistics[1].angle; ballistics[1].angle = a;
1487                         t = ballistics[0].time; ballistics[0].time = ballistics[1].time; ballistics[1].time = t;
1488                 }
1489         }
1490
1491         // test if there is a collision free trajectory
1492         for ( i = 0; i < n; i++ ) {
1493                 pitch = DEG2RAD( ballistics[i].angle );
1494                 idMath::SinCos( pitch, s, c );
1495                 dir[i] = target - firePos;
1496                 dir[i].z = 0.0f;
1497                 dir[i] *= c * idMath::InvSqrt( dir[i].LengthSqr() );
1498                 dir[i].z = s;
1499
1500                 zVel = projectileSpeed * dir[i].z;
1501
1502                 if ( ai_debugTrajectory.GetBool() ) {
1503                         t = ballistics[i].time / 100.0f;
1504                         velocity = dir[i] * projectileSpeed;
1505                         lastPos = firePos;
1506                         pos = firePos;
1507                         for ( j = 1; j < 100; j++ ) {
1508                                 pos += velocity * t;
1509                                 velocity += projGravity * t;
1510                                 gameRenderWorld->DebugLine( colorCyan, lastPos, pos );
1511                                 lastPos = pos;
1512                         }
1513                 }
1514
1515                 if ( TestTrajectory( firePos, target, zVel, projGravity[2], ballistics[i].time, firePos.z + max_height, clip, clipmask, ignore, targetEntity, drawtime ) ) {
1516                         aimDir = dir[i];
1517                         return true;
1518                 }
1519         }
1520
1521         aimDir = dir[0];
1522
1523         // there is no collision free trajectory
1524         return false;
1525 }