terrCollision.cpp

Public Functions

calcInterceptNone(F32 , F32 , F32 )

calcInterceptV(F32 vStart, F32 invDeltaV, F32 intercept)

clrbuf(U32 * p, U32 s)

isOnPlane(Point3F & p, PlaneF & plane)

swap(U32 *& a, U32 *& b)

   1
   2//-----------------------------------------------------------------------------
   3// Copyright (c) 2012 GarageGames, LLC
   4//
   5// Permission is hereby granted, free of charge, to any person obtaining a copy
   6// of this software and associated documentation files (the "Software"), to
   7// deal in the Software without restriction, including without limitation the
   8// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
   9// sell copies of the Software, and to permit persons to whom the Software is
  10// furnished to do so, subject to the following conditions:
  11//
  12// The above copyright notice and this permission notice shall be included in
  13// all copies or substantial portions of the Software.
  14//
  15// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  16// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  17// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  18// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  19// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
  20// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
  21// IN THE SOFTWARE.
  22//-----------------------------------------------------------------------------
  23
  24#include "platform/platform.h"
  25#include "terrain/terrCollision.h"
  26
  27#include "terrain/terrData.h"
  28#include "collision/abstractPolyList.h"
  29#include "collision/collision.h"
  30
  31
  32const F32 TerrainThickness = 0.5f;
  33static const U32 MaxExtent = 256;
  34#define MAX_FLOAT 1e20f
  35
  36
  37//----------------------------------------------------------------------------
  38
  39Convex sTerrainConvexList;
  40
  41// Number of vertices followed by point index
  42S32 sVertexList[5][5] = {
  43   { 3, 1,2,3 },  // 135 B
  44   { 3, 0,1,3 },  // 135 A
  45   { 3, 0,2,3 },  // 45 B
  46   { 3, 0,1,2 },  // 45 A
  47   { 4, 0,1,2,3 } // Convex square
  48};
  49
  50// Number of edges followed by edge index pairs
  51S32 sEdgeList45[16][11] = {
  52   { 0 },                  //
  53   { 0 },
  54   { 0 },
  55   { 1, 0,1 },             // 0-1
  56   { 0 },
  57   { 1, 0,1 },             // 0-2
  58   { 1, 0,1 },             // 1-2
  59   { 3, 0,1,1,2,2,0 },     // 0-1,1-2,2-0
  60   { 0 },
  61   { 0,},                  //
  62   { 0 },
  63   { 1, 0,1 },             // 0-1,
  64   { 0, },                 //
  65   { 1, 0,1 },             // 0-2,
  66   { 1, 0,1 },             // 1-2
  67   { 3, 0,1,1,2,0,2 },
  68};
  69
  70S32 sEdgeList135[16][11] = {
  71   { 0 },
  72   { 0 },
  73   { 0 },
  74   { 1, 0,1 },             // 0-1
  75   { 0 },
  76   { 0 },
  77   { 1, 0,1 },             // 1-2
  78   { 2, 0,1,1,2 },         // 0-1,1-2
  79   { 0 },
  80   { 0, },                 //
  81   { 1, 0,1 },             // 1-3
  82   { 2, 0,1,1,2 },         // 0-1,1-3,
  83   { 0 },                  //
  84   { 0 },                  //
  85   { 2, 0,1,2,0 },         // 1-2,3-1
  86   { 3, 0,1,1,2,1,3 },
  87};
  88
  89// On split squares, the FaceA diagnal is also removed
  90S32 sEdgeList45A[16][11] = {
  91   { 0 },                  //
  92   { 0 },
  93   { 0 },
  94   { 1, 0,1 },             // 0-1
  95   { 0 },
  96   { 0 },                  //
  97   { 1, 0,1 },             // 1-2
  98   { 2, 0,1,1,2 },         // 0-1,1-2
  99   { 0 },
 100   { 0,},                  //
 101   { 0 },
 102   { 1, 0,1 },             // 0-1
 103   { 0, },                 //
 104   { 0, 0,1 },             //
 105   { 1, 0,1 },             // 1-2
 106   { 3, 0,1,1,2 },
 107};
 108
 109S32 sEdgeList135A[16][11] = {
 110   { 0 },
 111   { 0 },
 112   { 0 },
 113   { 1, 0,1 },             // 0-1
 114   { 0 },
 115   { 0 },
 116   { 1, 0,1 },             // 1-2
 117   { 2, 0,1,1,2 },         // 0-1,1-2
 118   { 0 },
 119   { 0 },                  //
 120   { 0 },                  //
 121   { 1, 0,1 },             // 0-1
 122   { 0 },                  //
 123   { 0 },                  //
 124   { 1, 0,1 },             // 1-2
 125   { 3, 0,1,1,2 },
 126};
 127
 128
 129// Number of faces followed by normal index and vertices
 130S32 sFaceList45[16][9] = {
 131   { 0 },
 132   { 0 },
 133   { 0 },
 134   { 0 },
 135   { 0 },
 136   { 0 },
 137   { 0 },
 138   { 1, 0,0,1,2 },
 139   { 0 },
 140   { 0 },
 141   { 0 },
 142   { 0 },
 143   { 0 },
 144   { 1, 1,0,1,2 },
 145   { 0 },
 146   { 2, 0,0,1,2, 1,0,2,3 },
 147};
 148
 149S32 sFaceList135[16][9] = {
 150   { 0 },
 151   { 0 },
 152   { 0 },
 153   { 0 },
 154   { 0 },
 155   { 0 },
 156   { 0 },
 157   { 0 },
 158   { 0 },
 159   { 0 },
 160   { 0 },
 161   { 1, 0,0,1,2 },
 162   { 0 },
 163   { 0 },
 164   { 1, 1,0,1,2 },
 165   { 2, 0,0,1,3, 1,1,2,3 },
 166};
 167
 168
 169TerrainConvex::TerrainConvex() 
 170{
 171   halfA = true;
 172   square = NULL;
 173   squareId = 0;
 174   material = 0;
 175   split45 = false;
 176
 177   mType = TerrainConvexType; 
 178}
 179
 180TerrainConvex::TerrainConvex( const TerrainConvex &cv ) 
 181{
 182   mType = TerrainConvexType;
 183   halfA = false;
 184   square = NULL;
 185   // Only a partial copy...
 186   mObject = cv.mObject;
 187   split45 = cv.split45;
 188   squareId = cv.squareId;
 189   material = cv.material;
 190   point[0] = cv.point[0];
 191   point[1] = cv.point[1];
 192   point[2] = cv.point[2];
 193   point[3] = cv.point[3];
 194   normal[0] = cv.normal[0];
 195   normal[1] = cv.normal[1];
 196   box = cv.box;
 197}
 198
 199Box3F TerrainConvex::getBoundingBox() const
 200{
 201   return box;
 202}
 203
 204Box3F TerrainConvex::getBoundingBox(const MatrixF&, const Point3F& ) const
 205{
 206   // Function should not be called....
 207   return box;
 208}
 209
 210Point3F TerrainConvex::support(const VectorF& v) const
 211{
 212   S32 *vp;
 213   if (halfA)
 214      vp = square ? sVertexList[(split45 << 1) | 1]: sVertexList[4];
 215   else
 216      vp = square ? sVertexList[(split45 << 1)]    : sVertexList[4];
 217
 218   S32 *ve = vp + vp[0] + 1;
 219   const Point3F *bp = &point[vp[1]];
 220   F32 bd = mDot(*bp,v);
 221   for (vp += 2; vp < ve; vp++) {
 222      const Point3F* cp = &point[*vp];
 223      F32 dd = mDot(*cp,v);
 224      if (dd > bd) {
 225         bd = dd;
 226         bp = cp;
 227      }
 228   }
 229   return *bp;
 230}
 231
 232inline bool isOnPlane(Point3F& p,PlaneF& plane)
 233{
 234   F32 dist = mDot(plane,p) + plane.d;
 235   return dist < 0.1 && dist > -0.1;
 236}
 237
 238void TerrainConvex::getFeatures(const MatrixF& mat,const VectorF& n, ConvexFeature* cf)
 239{
 240   U32 i;
 241   cf->material = 0;
 242   cf->mObject = mObject;
 243
 244   // Plane is normal n + support point
 245   PlaneF plane;
 246   plane.set(support(n),n);
 247   S32 vertexCount = cf->mVertexList.size();
 248
 249   // Emit vertices on the plane
 250   S32* vertexListPointer;
 251   if (halfA)
 252      vertexListPointer = square ? sVertexList[(split45 << 1) | 1]: sVertexList[4];
 253   else
 254      vertexListPointer = square ? sVertexList[(split45 << 1)]    : sVertexList[4];
 255
 256   S32 pm = 0;
 257   S32 numVerts = *vertexListPointer;
 258   vertexListPointer += 1;
 259   for (i = 0; i < numVerts; i++)
 260   {
 261      Point3F& cp = point[vertexListPointer[i]];
 262      cf->mVertexList.increment();
 263      mat.mulP(cp,&cf->mVertexList.last());
 264      pm |= 1 << vertexListPointer[i];
 265   }
 266
 267   // Emit Edges
 268   S32* ep = (square && halfA)?
 269      (split45 ? sEdgeList45A[pm]: sEdgeList135A[pm]):
 270      (split45 ? sEdgeList45[pm]: sEdgeList135[pm]);
 271
 272   S32 numEdges = *ep;
 273   S32 edgeListStart = cf->mEdgeList.size();
 274   cf->mEdgeList.increment(numEdges);
 275   ep += 1;
 276   for (i = 0; i < numEdges; i++)
 277   {
 278      cf->mEdgeList[edgeListStart + i].vertex[0] = vertexCount + ep[i * 2 + 0];
 279      cf->mEdgeList[edgeListStart + i].vertex[1] = vertexCount + ep[i * 2 + 1];
 280   }
 281
 282   // Emit faces
 283   S32* fp = split45 ? sFaceList45[pm]: sFaceList135[pm];
 284   S32 numFaces = *fp;
 285   fp += 1;
 286   S32 faceListStart = cf->mFaceList.size();
 287   cf->mFaceList.increment(numFaces);
 288   for (i = 0; i < numFaces; i++)
 289   {
 290      ConvexFeature::Face& face = cf->mFaceList[faceListStart + i];
 291      face.normal = normal[fp[i * 4 + 0]];
 292      face.vertex[0] = vertexCount + fp[i * 4 + 1];
 293      face.vertex[1] = vertexCount + fp[i * 4 + 2];
 294      face.vertex[2] = vertexCount + fp[i * 4 + 3];
 295   }
 296}
 297
 298
 299void TerrainConvex::getPolyList(AbstractPolyList* list)
 300{
 301   list->setTransform(&mObject->getTransform(), mObject->getScale());
 302   list->setObject(mObject);
 303
 304   // Emit vertices
 305   U32 array[4];
 306   U32 curr = 0;
 307
 308   S32 numVerts;
 309   S32* vertsStart;
 310   if (halfA)
 311   {
 312      numVerts   = square ?  sVertexList[(split45 << 1) | 1][0] :  sVertexList[4][0];
 313      vertsStart = square ? &sVertexList[(split45 << 1) | 1][1] : &sVertexList[4][1];
 314   }
 315   else
 316   {
 317      numVerts   = square ?  sVertexList[(split45 << 1)][0] :  sVertexList[4][0];
 318      vertsStart = square ? &sVertexList[(split45 << 1)][1] : &sVertexList[4][1];
 319   }
 320
 321   S32 pointMask = 0;
 322   for (U32 i = 0; i < numVerts; i++) {
 323      const Point3F& cp = point[vertsStart[i]];
 324      array[curr++] = list->addPoint(cp);
 325      pointMask |= (1 << vertsStart[i]);
 326   }
 327
 328   S32  numFaces  = split45 ?  sFaceList45[pointMask][0] :  sFaceList135[pointMask][0];
 329   S32* faceStart = split45 ? &sFaceList45[pointMask][1] : &sFaceList135[pointMask][1];
 330   for (U32 j = 0; j < numFaces; j++) {
 331      S32 plane = faceStart[0];
 332      S32 v0    = faceStart[1];
 333      S32 v1    = faceStart[2];
 334      S32 v2    = faceStart[3];
 335
 336      list->begin(0, plane);
 337      list->vertex(array[v0]);
 338      list->vertex(array[v1]);
 339      list->vertex(array[v2]);
 340      list->plane(array[v0], array[v1], array[v2]);
 341      list->end();
 342
 343      faceStart += 4;
 344   }
 345}
 346
 347
 348//----------------------------------------------------------------------------
 349
 350void TerrainBlock::buildConvex(const Box3F& box,Convex* convex)
 351{
 352   PROFILE_SCOPE( TerrainBlock_buildConvex );
 353   
 354   sTerrainConvexList.collectGarbage();
 355
 356   // First check to see if the query misses the 
 357   // terrain elevation range.
 358   const Point3F &terrainPos = getPosition();
 359   if (  box.maxExtents.z - terrainPos.z < -TerrainThickness || 
 360         box.minExtents.z - terrainPos.z > fixedToFloat( mFile->getMaxHeight() ) )
 361      return;
 362
 363   // Transform the bounding sphere into the object's coord space.  Note that this
 364   // not really optimal.
 365   Box3F osBox = box;
 366   mWorldToObj.mul(osBox);
 367   AssertWarn(mObjScale == Point3F(1, 1, 1), "Error, handle the scale transform on the terrain");
 368
 369   S32 xStart = (S32)mFloor( osBox.minExtents.x / mSquareSize );
 370   S32 xEnd   = (S32)mCeil ( osBox.maxExtents.x / mSquareSize );
 371   S32 yStart = (S32)mFloor( osBox.minExtents.y / mSquareSize );
 372   S32 yEnd   = (S32)mCeil ( osBox.maxExtents.y / mSquareSize );
 373   S32 xExt = xEnd - xStart;
 374   if (xExt > MaxExtent)
 375      xExt = MaxExtent;
 376
 377   U16 heightMax = floatToFixed(osBox.maxExtents.z);
 378   U16 heightMin = (osBox.minExtents.z < 0)? 0: floatToFixed(osBox.minExtents.z);
 379
 380   const U32 BlockMask = mFile->mSize - 1;
 381
 382   for ( S32 y = yStart; y < yEnd; y++ ) 
 383   {
 384      S32 yi = y & BlockMask;
 385
 386      //
 387      for ( S32 x = xStart; x < xEnd; x++ ) 
 388      {
 389         S32 xi = x & BlockMask;
 390
 391         const TerrainSquare *sq = mFile->findSquare( 0, xi, yi );
 392
 393         if ( x != xi || y != yi )
 394            continue;
 395
 396         // holes only in the primary terrain block
 397         if (  ( ( sq->flags & TerrainSquare::Empty ) && x == xi && y == yi ) ||
 398               sq->minHeight > heightMax || 
 399               sq->maxHeight < heightMin )
 400            continue;
 401
 402         U32 sid = (x << 16) + (y & ((1 << 16) - 1));
 403         Convex *cc = 0;
 404
 405         // See if the square already exists as part of the working set.
 406         CollisionWorkingList& wl = convex->getWorkingList();
 407         for (CollisionWorkingList* itr = wl.wLink.mNext; itr != &wl; itr = itr->wLink.mNext)
 408            if (itr->mConvex->getType() == TerrainConvexType &&
 409                static_cast<TerrainConvex*>(itr->mConvex)->squareId == sid) {
 410               cc = itr->mConvex;
 411               break;
 412            }
 413
 414         if (cc)
 415            continue;
 416
 417         // Create a new convex.
 418         TerrainConvex* cp = new TerrainConvex;
 419         sTerrainConvexList.registerObject(cp);
 420         convex->addToWorkingList(cp);
 421         cp->halfA = true;
 422         cp->square = 0;
 423         cp->mObject = this;
 424         cp->squareId = sid;
 425         cp->material = mFile->getLayerIndex( xi, yi );
 426         cp->box.minExtents.set((F32)(x * mSquareSize), (F32)(y * mSquareSize), fixedToFloat( sq->minHeight ));
 427         cp->box.maxExtents.x = cp->box.minExtents.x + mSquareSize;
 428         cp->box.maxExtents.y = cp->box.minExtents.y + mSquareSize;
 429         cp->box.maxExtents.z = fixedToFloat( sq->maxHeight );
 430         mObjToWorld.mul(cp->box);
 431
 432         // Build points
 433         Point3F* pos = cp->point;
 434         for (S32 i = 0; i < 4 ; i++,pos++) {
 435            S32 dx = i >> 1;
 436            S32 dy = dx ^ (i & 1);
 437            pos->x = (F32)((x + dx) * mSquareSize);
 438            pos->y = (F32)((y + dy) * mSquareSize);
 439            pos->z = fixedToFloat( mFile->getHeight(xi + dx, yi + dy) );
 440         }
 441
 442         // Build normals, then split into two Convex objects if the
 443         // square is concave
 444         if ((cp->split45 = sq->flags & TerrainSquare::Split45) == true) {
 445            VectorF *vp = cp->point;
 446            mCross(vp[0] - vp[1],vp[2] - vp[1],&cp->normal[0]);
 447            cp->normal[0].normalize();
 448            mCross(vp[2] - vp[3],vp[0] - vp[3],&cp->normal[1]);
 449            cp->normal[1].normalize();
 450            if (mDot(vp[3] - vp[1],cp->normal[0]) > 0) {
 451               TerrainConvex* nc = new TerrainConvex(*cp);
 452               sTerrainConvexList.registerObject(nc);
 453               convex->addToWorkingList(nc);
 454               nc->halfA = false;
 455               nc->square = cp;
 456               cp->square = nc;
 457            }
 458         }
 459         else {
 460            VectorF *vp = cp->point;
 461            mCross(vp[3] - vp[0],vp[1] - vp[0],&cp->normal[0]);
 462            cp->normal[0].normalize();
 463            mCross(vp[1] - vp[2],vp[3] - vp[2],&cp->normal[1]);
 464            cp->normal[1].normalize();
 465            if (mDot(vp[2] - vp[0],cp->normal[0]) > 0) {
 466               TerrainConvex* nc = new TerrainConvex(*cp);
 467               sTerrainConvexList.registerObject(nc);
 468               convex->addToWorkingList(nc);
 469               nc->halfA = false;
 470               nc->square = cp;
 471               cp->square = nc;
 472            }
 473         }
 474      }
 475   }
 476}
 477
 478static inline void swap(U32*& a,U32*& b)
 479{
 480   U32* t = b;
 481   b = a;
 482   a = t;
 483}
 484
 485static void clrbuf(U32* p, U32 s)
 486{
 487   U32* e = p + s;
 488   while (p != e)
 489      *p++ = U32_MAX;
 490}
 491
 492bool TerrainBlock::buildPolyList(PolyListContext context, AbstractPolyList* polyList, const Box3F &box, const SphereF&)
 493{
 494   PROFILE_SCOPE( TerrainBlock_buildPolyList );
 495
 496   // First check to see if the query misses the 
 497   // terrain elevation range.
 498   const Point3F &terrainPos = getPosition();
 499   if (  box.maxExtents.z - terrainPos.z < -TerrainThickness || 
 500         box.minExtents.z - terrainPos.z > fixedToFloat( mFile->getMaxHeight() ) )
 501      return false;
 502
 503   // Transform the bounding sphere into the object's coord 
 504   // space.  Note that this is really optimal.
 505   Box3F osBox = box;
 506   mWorldToObj.mul(osBox);
 507   AssertWarn(mObjScale == Point3F::One, "Error, handle the scale transform on the terrain");
 508
 509   // Setup collision state data
 510   polyList->setTransform(&getTransform(), getScale());
 511   polyList->setObject(this);
 512
 513   S32 xStart = (S32)mFloor( osBox.minExtents.x / mSquareSize );
 514   S32 xEnd   = (S32)mCeil ( osBox.maxExtents.x / mSquareSize );
 515   S32 yStart = (S32)mFloor( osBox.minExtents.y / mSquareSize );
 516   S32 yEnd   = (S32)mCeil ( osBox.maxExtents.y / mSquareSize );
 517   if ( xStart < 0 )
 518      xStart = 0;
 519   S32 xExt = xEnd - xStart;
 520   if ( xExt > MaxExtent )
 521      xExt = MaxExtent;
 522   xEnd = xStart + xExt;
 523
 524   U32 heightMax = floatToFixed(osBox.maxExtents.z);
 525   U32 heightMin = (osBox.minExtents.z < 0.0f)? 0.0f: floatToFixed(osBox.minExtents.z);
 526
 527   // Index of shared points
 528   U32 bp[(MaxExtent + 1) * 2],*vb[2];
 529   vb[0] = &bp[0];
 530   vb[1] = &bp[xExt + 1];
 531   clrbuf(vb[1],xExt + 1);
 532
 533   const U32 BlockMask = mFile->mSize - 1;
 534
 535   bool emitted = false;
 536   for (S32 y = yStart; y < yEnd; y++) 
 537   {
 538      S32 yi = y & BlockMask;
 539
 540      swap(vb[0],vb[1]);
 541      clrbuf(vb[1],xExt + 1);
 542
 543      F32 wy1 = y * mSquareSize, wy2 = (y + 1) * mSquareSize;
 544      if(context == PLC_Navigation &&
 545         ((wy1 > osBox.maxExtents.y && wy2 > osBox.maxExtents.y) ||
 546          (wy1 < osBox.minExtents.y && wy2 < osBox.minExtents.y)))
 547         continue;
 548
 549      //
 550      for (S32 x = xStart; x < xEnd; x++) 
 551      {
 552         S32 xi = x & BlockMask;
 553         const TerrainSquare *sq = mFile->findSquare( 0, xi, yi );
 554
 555         F32 wx1 = x * mSquareSize, wx2 = (x + 1) * mSquareSize;
 556         if(context == PLC_Navigation &&
 557            ((wx1 > osBox.maxExtents.x && wx2 > osBox.maxExtents.x) ||
 558             (wx1 < osBox.minExtents.x && wx2 < osBox.minExtents.x)))
 559            continue;
 560
 561         if ( x != xi || y != yi )
 562            continue;
 563
 564         // holes only in the primary terrain block
 565         if (  ( ( sq->flags & TerrainSquare::Empty ) && x == xi && y == yi ) || 
 566               sq->minHeight > heightMax || 
 567               sq->maxHeight < heightMin )
 568            continue;
 569
 570         emitted = true;
 571
 572         // Add the missing points
 573         U32 vi[5];
 574         for (int i = 0; i < 4 ; i++) 
 575         {
 576            S32 dx = i >> 1;
 577            S32 dy = dx ^ (i & 1);
 578            U32* vp = &vb[dy][x - xStart + dx];
 579            if (*vp == U32_MAX) 
 580            {
 581               Point3F pos;
 582               pos.x = (F32)((x + dx) * mSquareSize);
 583               pos.y = (F32)((y + dy) * mSquareSize);
 584               pos.z = fixedToFloat( mFile->getHeight(xi + dx, yi + dy) );
 585               *vp = polyList->addPoint(pos);
 586            }
 587            vi[i] = *vp;
 588         }
 589
 590         U32* vp = &vi[0];
 591         if ( !( sq->flags & TerrainSquare::Split45 ) )
 592            vi[4] = vi[0], vp++;
 593
 594         BaseMatInstance *material = NULL; //getMaterialInst( xi, yi );
 595         U32 surfaceKey = ((xi << 16) + yi) << 1;
 596         polyList->begin(material,surfaceKey);
 597         polyList->vertex(vp[0]);
 598         polyList->vertex(vp[1]);
 599         polyList->vertex(vp[2]);
 600         polyList->plane(vp[0],vp[1],vp[2]);
 601         polyList->end();
 602         polyList->begin(material,surfaceKey + 1);
 603         polyList->vertex(vp[0]);
 604         polyList->vertex(vp[2]);
 605         polyList->vertex(vp[3]);
 606         polyList->plane(vp[0],vp[2],vp[3]);
 607         polyList->end();
 608      }
 609   }
 610
 611   return emitted;
 612}
 613
 614//----------------------------------------------------------------------------
 615
 616static F32 calcInterceptV(F32 vStart, F32 invDeltaV, F32 intercept)
 617{
 618   return (intercept - vStart) * invDeltaV;
 619}
 620
 621static F32 calcInterceptNone(F32, F32, F32)
 622{
 623   return MAX_FLOAT;
 624}
 625
 626static F32 (*calcInterceptX)(F32, F32, F32);
 627static F32 (*calcInterceptY)(F32, F32, F32);
 628
 629static U32 lineCount;
 630static Point3F lineStart, lineEnd;
 631
 632bool TerrainBlock::castRay(const Point3F &start, const Point3F &end, RayInfo *info)
 633{
 634   PROFILE_SCOPE( TerrainBlock_castRay );
 635
 636   if ( !castRayI(start, end, info, false) )
 637      return false;
 638      
 639   // Set intersection point.
 640   info->setContactPoint( start, end );
 641   getTransform().mulP( info->point );    // transform to world coordinates for getGridPos
 642
 643   // Set material at contact point.
 644   Point2I gridPos = getGridPos( info->point );
 645   U8 layer = mFile->getLayerIndex( gridPos.x, gridPos.y );
 646   info->material = mFile->getMaterialMapping( layer );
 647
 648   return true;
 649}
 650
 651bool TerrainBlock::castRayI(const Point3F &start, const Point3F &end, RayInfo *info, bool collideEmpty)
 652{
 653   lineCount = 0;
 654   lineStart = start;
 655   lineEnd = end;
 656
 657   info->object = this;
 658
 659   if(start.x == end.x && start.y == end.y)
 660   {
 661      if (end.z == start.z)
 662         return false;
 663
 664      F32 height;
 665      if(!getNormalAndHeight(Point2F(start.x, start.y), &info->normal, &height, true))
 666         return false;
 667
 668      F32 t = (height - start.z) / (end.z - start.z);
 669      if(t < 0 || t > 1)
 670         return false;
 671      info->t = t;
 672
 673      return true;
 674   }
 675
 676   F32 invBlockWorldSize = 1 / getWorldBlockSize();
 677
 678   Point3F pStart(start.x * invBlockWorldSize, start.y * invBlockWorldSize, start.z);
 679   Point3F pEnd(end.x * invBlockWorldSize, end.y * invBlockWorldSize, end.z);
 680
 681   S32 blockX = (S32)mFloor(pStart.x);
 682   S32 blockY = (S32)mFloor(pStart.y);
 683
 684   S32 dx, dy;
 685
 686   F32 invDeltaX;
 687   if(pEnd.x == pStart.x)
 688   {
 689      calcInterceptX = calcInterceptNone;
 690      invDeltaX = 0;
 691      dx = 0;
 692   }
 693   else
 694   {
 695      invDeltaX = 1 / (pEnd.x - pStart.x);
 696      calcInterceptX = calcInterceptV;
 697      if(pEnd.x < pStart.x)
 698         dx = -1;
 699      else
 700         dx = 1;
 701   }
 702
 703   F32 invDeltaY;
 704   if(pEnd.y == pStart.y)
 705   {
 706      calcInterceptY = calcInterceptNone;
 707      invDeltaY = 0;
 708      dy = 0;
 709   }
 710   else
 711   {
 712      invDeltaY = 1 / (pEnd.y - pStart.y);
 713      calcInterceptY = calcInterceptV;
 714      if(pEnd.y < pStart.y)
 715         dy = -1;
 716      else
 717         dy = 1;
 718   }
 719
 720   const U32 BlockSquareWidth = mFile->mSize;
 721   const U32 GridLevels = mFile->mGridLevels;
 722
 723   F32 startT = 0;
 724   for(;;)
 725   {
 726      F32 nextXInt = calcInterceptX(pStart.x, invDeltaX, (F32)(blockX + (dx == 1)));
 727      F32 nextYInt = calcInterceptY(pStart.y, invDeltaY, (F32)(blockY + (dy == 1)));
 728
 729      F32 intersectT = 1;
 730
 731      if(nextXInt < intersectT)
 732         intersectT = nextXInt;
 733      if(nextYInt < intersectT)
 734         intersectT = nextYInt;
 735
 736      if ( castRayBlock(   pStart, 
 737                           pEnd, 
 738                           Point2I( blockX * BlockSquareWidth, 
 739                                    blockY * BlockSquareWidth ), 
 740                           GridLevels, 
 741                           invDeltaX, 
 742                           invDeltaY, 
 743                           startT, 
 744                           intersectT, 
 745                           info, 
 746                           collideEmpty ) ) 
 747      {
 748         info->normal.z *= BlockSquareWidth * mSquareSize;
 749         info->normal.normalize();
 750         return true;
 751      }
 752
 753      startT = intersectT;
 754      if(intersectT >= 1)
 755         break;
 756      if(nextXInt < nextYInt)
 757         blockX += dx;
 758      else if(nextYInt < nextXInt)
 759         blockY += dy;
 760      else
 761      {
 762         blockX += dx;
 763         blockY += dy;
 764      }
 765   }
 766
 767   return false;
 768}
 769
 770struct TerrLOSStackNode
 771{
 772   F32 startT;
 773   F32 endT;
 774   Point2I blockPos;
 775   U32 level;
 776};
 777
 778bool TerrainBlock::castRayBlock( const Point3F &pStart, 
 779                                 const Point3F &pEnd, 
 780                                 const Point2I &aBlockPos, 
 781                                 U32 aLevel, 
 782                                 F32 invDeltaX, 
 783                                 F32 invDeltaY, 
 784                                 F32 aStartT, 
 785                                 F32 aEndT, 
 786                                 RayInfo *info, 
 787                                 bool collideEmpty )
 788{
 789   const U32 BlockSquareWidth = mFile->mSize;
 790   const U32 GridLevels = mFile->mGridLevels;
 791   const U32 BlockMask = mFile->mSize - 1;
 792
 793   F32 invBlockSize = 1 / F32( BlockSquareWidth );
 794
 795   static Vector<TerrLOSStackNode> stack;
 796   stack.setSize( GridLevels * 3 + 1 );
 797   U32 stackSize = 1;
 798
 799   stack[0].startT = aStartT;
 800   stack[0].endT = aEndT;
 801   stack[0].blockPos = aBlockPos;
 802   stack[0].level = aLevel;
 803   
 804   if( !aBlockPos.isZero() )
 805      return false;
 806
 807   while(stackSize--)
 808   {
 809      TerrLOSStackNode *sn = stack.address() + stackSize;
 810      U32 level  = sn->level;
 811      F32 startT = sn->startT;
 812      F32 endT   = sn->endT;
 813      Point2I blockPos = sn->blockPos;
 814
 815      const TerrainSquare *sq = mFile->findSquare( level, blockPos.x, blockPos.y );
 816
 817      F32 startZ = startT * (pEnd.z - pStart.z) + pStart.z;
 818      F32 endZ = endT * (pEnd.z - pStart.z) + pStart.z;
 819
 820      F32 minHeight = fixedToFloat(sq->minHeight);
 821      if(startZ <= minHeight && endZ <= minHeight)
 822         continue;
 823
 824      F32 maxHeight = fixedToFloat(sq->maxHeight);
 825      if(startZ >= maxHeight && endZ >= maxHeight)
 826         continue;
 827
 828      if (  !collideEmpty && ( sq->flags & TerrainSquare::Empty ) &&
 829           blockPos.x == ( blockPos.x & BlockMask ) && blockPos.y == ( blockPos.y & BlockMask ))
 830         continue;
 831
 832      if(level == 0)
 833      {
 834         F32 xs = blockPos.x * invBlockSize;
 835         F32 ys = blockPos.y * invBlockSize;
 836
 837         F32 zBottomLeft = fixedToFloat( mFile->getHeight(blockPos.x, blockPos.y) );
 838         F32 zBottomRight= fixedToFloat( mFile->getHeight(blockPos.x + 1, blockPos.y) );
 839         F32 zTopLeft =    fixedToFloat( mFile->getHeight(blockPos.x, blockPos.y + 1) );
 840         F32 zTopRight =   fixedToFloat( mFile->getHeight(blockPos.x + 1, blockPos.y + 1) );
 841
 842         PlaneF p1, p2;
 843         PlaneF divider;
 844         Point3F planePoint;
 845
 846         if(sq->flags & TerrainSquare::Split45)
 847         {
 848            p1.set(zBottomLeft - zBottomRight, zBottomRight - zTopRight, invBlockSize);
 849            p2.set(zTopLeft - zTopRight, zBottomLeft - zTopLeft, invBlockSize);
 850            planePoint.set(xs, ys, zBottomLeft);
 851            divider.x = 1;
 852            divider.y = -1;
 853            divider.z = 0;
 854         }
 855         else
 856         {
 857            p1.set(zTopLeft - zTopRight, zBottomRight - zTopRight, invBlockSize);
 858            p2.set(zBottomLeft - zBottomRight, zBottomLeft - zTopLeft, invBlockSize);
 859            planePoint.set(xs + invBlockSize, ys, zBottomRight);
 860            divider.x = 1;
 861            divider.y = 1;
 862            divider.z = 0;
 863         }
 864         p1.setPoint(planePoint);
 865         p2.setPoint(planePoint);
 866         divider.setPoint(planePoint);
 867
 868         F32 t1 = p1.intersect(pStart, pEnd);
 869         F32 t2 = p2.intersect(pStart, pEnd);
 870         F32 td = divider.intersect(pStart, pEnd);
 871
 872         F32 dStart = divider.distToPlane(pStart);
 873         F32 dEnd = divider.distToPlane(pEnd);
 874
 875         // see if the line crosses the divider
 876         if((dStart >= 0 && dEnd < 0) || (dStart < 0 && dEnd >= 0))
 877         {
 878            if(dStart < 0)
 879            {
 880               F32 temp = t1;
 881               t1 = t2;
 882               t2 = temp;
 883            }
 884            if(t1 >= startT && t1 && t1 <= td && t1 <= endT)
 885            {
 886               info->t = t1;
 887               info->normal = p1;
 888               return true;
 889            }
 890            if(t2 >= td && t2 >= startT && t2 <= endT)
 891            {
 892               info->t = t2;
 893               info->normal = p2;
 894               return true;
 895            }
 896         }
 897         else
 898         {
 899            F32 t;
 900            if(dStart >= 0) {
 901               t = t1;
 902               info->normal = p1;
 903            }
 904            else {
 905               t = t2;
 906               info->normal = p2;
 907            }
 908            if(t >= startT && t <= endT)
 909            {
 910               info->t = t;
 911               return true;
 912            }
 913         }
 914         continue;
 915      }
 916      S32 subSqWidth = 1 << (level - 1);
 917      F32 xIntercept = (blockPos.x + subSqWidth) * invBlockSize;
 918      F32 xInt = calcInterceptX(pStart.x, invDeltaX, xIntercept);
 919      F32 yIntercept = (blockPos.y + subSqWidth) * invBlockSize;
 920      F32 yInt = calcInterceptY(pStart.y, invDeltaY, yIntercept);
 921
 922      F32 startX = startT * (pEnd.x - pStart.x) + pStart.x;
 923      F32 startY = startT * (pEnd.y - pStart.y) + pStart.y;
 924
 925      if(xInt < startT)
 926         xInt = MAX_FLOAT;
 927      if(yInt < startT)
 928         yInt = MAX_FLOAT;
 929
 930      U32 x0 = (startX > xIntercept) * subSqWidth;
 931      U32 y0 = (startY > yIntercept) * subSqWidth;
 932      U32 x1 = subSqWidth - x0;
 933      U32 y1 = subSqWidth - y0;
 934      U32 nextLevel = level - 1;
 935
 936      // push the items on the stack in reverse order of processing
 937      if(xInt > endT && yInt > endT)
 938      {
 939         // only test the square the point started in:
 940         stack[stackSize].blockPos.set(blockPos.x + x0, blockPos.y + y0);
 941         stack[stackSize].level = nextLevel;
 942         stackSize++;
 943      }
 944      else if(xInt < yInt)
 945      {
 946         F32 nextIntersect = endT;
 947         if(yInt <= endT)
 948         {
 949            stack[stackSize].blockPos.set(blockPos.x + x1, blockPos.y + y1);
 950            stack[stackSize].startT = yInt;
 951            stack[stackSize].endT = endT;
 952            stack[stackSize].level = nextLevel;
 953            nextIntersect = yInt;
 954            stackSize++;
 955         }
 956         stack[stackSize].blockPos.set(blockPos.x + x1, blockPos.y + y0);
 957         stack[stackSize].startT = xInt;
 958         stack[stackSize].endT = nextIntersect;
 959         stack[stackSize].level = nextLevel;
 960
 961         stack[stackSize+1].blockPos.set(blockPos.x + x0, blockPos.y + y0);
 962         stack[stackSize+1].startT = startT;
 963         stack[stackSize+1].endT = xInt;
 964         stack[stackSize+1].level = nextLevel;
 965         stackSize += 2;
 966      }
 967      else if(yInt < xInt)
 968      {
 969         F32 nextIntersect = endT;
 970         if(xInt <= endT)
 971         {
 972            stack[stackSize].blockPos.set(blockPos.x + x1, blockPos.y + y1);
 973            stack[stackSize].startT = xInt;
 974            stack[stackSize].endT = endT;
 975            stack[stackSize].level = nextLevel;
 976            nextIntersect = xInt;
 977            stackSize++;
 978         }
 979         stack[stackSize].blockPos.set(blockPos.x + x0, blockPos.y + y1);
 980         stack[stackSize].startT = yInt;
 981         stack[stackSize].endT = nextIntersect;
 982         stack[stackSize].level = nextLevel;
 983
 984         stack[stackSize+1].blockPos.set(blockPos.x + x0, blockPos.y + y0);
 985         stack[stackSize+1].startT = startT;
 986         stack[stackSize+1].endT = yInt;
 987         stack[stackSize+1].level = nextLevel;
 988         stackSize += 2;
 989      }
 990      else
 991      {
 992         stack[stackSize].blockPos.set(blockPos.x + x1, blockPos.y + y1);
 993         stack[stackSize].startT = xInt;
 994         stack[stackSize].endT = endT;
 995         stack[stackSize].level = nextLevel;
 996
 997         stack[stackSize+1].blockPos.set(blockPos.x + x0, blockPos.y + y0);
 998         stack[stackSize+1].startT = startT;
 999         stack[stackSize+1].endT = xInt;
1000         stack[stackSize+1].level = nextLevel;
1001         stackSize += 2;
1002      }
1003   }
1004
1005   return false;
1006}
1007