src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 #include "btHeightfieldTerrainShape.h"
  17
  18 #include "LinearMath/btTransformUtil.h"
  19
  20
  21
  22 btHeightfieldTerrainShape::btHeightfieldTerrainShape
  23 (
  24 int heightStickWidth, int heightStickLength, const void* heightfieldData,
  25 btScalar heightScale, btScalar minHeight, btScalar maxHeight,int upAxis,
  26 PHY_ScalarType hdt, bool flipQuadEdges
  27 )
  28 {
  29         initialize(heightStickWidth, heightStickLength, heightfieldData,
  30                    heightScale, minHeight, maxHeight, upAxis, hdt,
  31                    flipQuadEdges);
  32 }
  33
  34
  35
  36 btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength,const void* heightfieldData,btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges)
  37 {
  38         // legacy constructor: support only float or unsigned char,
  39         //      and min height is zero
  40         PHY_ScalarType hdt = (useFloatData) ? PHY_FLOAT : PHY_UCHAR;
  41         btScalar minHeight = 0.0f;
  42
  43         // previously, height = uchar * maxHeight / 65535.
  44         // So to preserve legacy behavior, heightScale = maxHeight / 65535
  45         btScalar heightScale = maxHeight / 65535;
  46
  47         initialize(heightStickWidth, heightStickLength, heightfieldData,
  48                    heightScale, minHeight, maxHeight, upAxis, hdt,
  49                    flipQuadEdges);
  50 }
  51
  52
  53
  54 void btHeightfieldTerrainShape::initialize
  55 (
  56 int heightStickWidth, int heightStickLength, const void* heightfieldData,
  57 btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis,
  58 PHY_ScalarType hdt, bool flipQuadEdges
  59 )
  60 {
  61         // validation
  62         btAssert(heightStickWidth > 1 && "bad width");
  63         btAssert(heightStickLength > 1 && "bad length");
  64         btAssert(heightfieldData && "null heightfield data");
  65         // btAssert(heightScale) -- do we care?  Trust caller here
  66         btAssert(minHeight <= maxHeight && "bad min/max height");
  67         btAssert(upAxis >= 0 && upAxis < 3 &&
  68             "bad upAxis--should be in range [0,2]");
  69         btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT &&
  70             "Bad height data type enum");
  71
  72         // initialize member variables
  73         m_shapeType = TERRAIN_SHAPE_PROXYTYPE;
  74         m_heightStickWidth = heightStickWidth;
  75         m_heightStickLength = heightStickLength;
  76         m_minHeight = minHeight;
  77         m_maxHeight = maxHeight;
  78         m_width = (btScalar) (heightStickWidth - 1);
  79         m_length = (btScalar) (heightStickLength - 1);
  80         m_heightScale = heightScale;
  81         m_heightfieldDataUnknown = heightfieldData;
  82         m_heightDataType = hdt;
  83         m_flipQuadEdges = flipQuadEdges;
  84         m_useDiamondSubdivision = false;
  85         m_useZigzagSubdivision = false;
  86         m_upAxis = upAxis;
  87         m_localScaling.setValue(btScalar(1.), btScalar(1.), btScalar(1.));
  88
  89         // determine min/max axis-aligned bounding box (aabb) values
  90         switch (m_upAxis)
  91         {
  92         case 0:
  93                 {
  94                         m_localAabbMin.setValue(m_minHeight, 0, 0);
  95                         m_localAabbMax.setValue(m_maxHeight, m_width, m_length);
  96                         break;
  97                 }
  98         case 1:
  99                 {
 100                         m_localAabbMin.setValue(0, m_minHeight, 0);
 101                         m_localAabbMax.setValue(m_width, m_maxHeight, m_length);
 102                         break;
 103                 };
 104         case 2:
 105                 {
 106                         m_localAabbMin.setValue(0, 0, m_minHeight);
 107                         m_localAabbMax.setValue(m_width, m_length, m_maxHeight);
 108                         break;
 109                 }
 110         default:
 111                 {
 112                         //need to get valid m_upAxis
 113                         btAssert(0 && "Bad m_upAxis");
 114                 }
 115         }
 116
 117         // remember origin (defined as exact middle of aabb)
 118         m_localOrigin = btScalar(0.5) * (m_localAabbMin + m_localAabbMax);
 119 }
 120
 121
 122
 123 btHeightfieldTerrainShape::~btHeightfieldTerrainShape()
 124 {
 125 }
 126
 127
 128
 129 void btHeightfieldTerrainShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
 130 {
 131         btVector3 halfExtents = (m_localAabbMax-m_localAabbMin)* m_localScaling * btScalar(0.5);
 132
 133         btVector3 localOrigin(0, 0, 0);
 134         localOrigin[m_upAxis] = (m_minHeight + m_maxHeight) * btScalar(0.5);
 135         localOrigin *= m_localScaling;
 136
 137         btMatrix3x3 abs_b = t.getBasis().absolute();
 138         btVector3 center = t.getOrigin();
 139     btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
 140         extent += btVector3(getMargin(),getMargin(),getMargin());
 141
 142         aabbMin = center - extent;
 143         aabbMax = center + extent;
 144 }
 145
 146
 147 /// This returns the "raw" (user's initial) height, not the actual height.
 148 /// The actual height needs to be adjusted to be relative to the center
 149 ///   of the heightfield's AABB.
 150 btScalar
 151 btHeightfieldTerrainShape::getRawHeightFieldValue(int x,int y) const
 152 {
 153         btScalar val = 0.f;
 154         switch (m_heightDataType)
 155         {
 156         case PHY_FLOAT:
 157                 {
 158                         val = m_heightfieldDataFloat[(y*m_heightStickWidth)+x];
 159                         break;
 160                 }
 161
 162         case PHY_UCHAR:
 163                 {
 164                         unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y*m_heightStickWidth)+x];
 165                         val = heightFieldValue * m_heightScale;
 166                         break;
 167                 }
 168
 169         case PHY_SHORT:
 170                 {
 171                         short hfValue = m_heightfieldDataShort[(y * m_heightStickWidth) + x];
 172                         val = hfValue * m_heightScale;
 173                         break;
 174                 }
 175
 176         default:
 177                 {
 178                         btAssert(!"Bad m_heightDataType");
 179                 }
 180         }
 181
 182         return val;
 183 }
 184
 185
 186
 187
 188 /// this returns the vertex in bullet-local coordinates
 189 void    btHeightfieldTerrainShape::getVertex(int x,int y,btVector3& vertex) const
 190 {
 191         btAssert(x>=0);
 192         btAssert(y>=0);
 193         btAssert(x<m_heightStickWidth);
 194         btAssert(y<m_heightStickLength);
 195
 196         btScalar        height = getRawHeightFieldValue(x,y);
 197
 198         switch (m_upAxis)
 199         {
 200         case 0:
 201                 {
 202                 vertex.setValue(
 203                         height - m_localOrigin.getX(),
 204                         (-m_width/btScalar(2.0)) + x,
 205                         (-m_length/btScalar(2.0) ) + y
 206                         );
 207                         break;
 208                 }
 209         case 1:
 210                 {
 211                         vertex.setValue(
 212                         (-m_width/btScalar(2.0)) + x,
 213                         height - m_localOrigin.getY(),
 214                         (-m_length/btScalar(2.0)) + y
 215                         );
 216                         break;
 217                 };
 218         case 2:
 219                 {
 220                         vertex.setValue(
 221                         (-m_width/btScalar(2.0)) + x,
 222                         (-m_length/btScalar(2.0)) + y,
 223                         height - m_localOrigin.getZ()
 224                         );
 225                         break;
 226                 }
 227         default:
 228                 {
 229                         //need to get valid m_upAxis
 230                         btAssert(0);
 231                 }
 232         }
 233
 234         vertex*=m_localScaling;
 235 }
 236
 237
 238
 239 static inline int
 240 getQuantized
 241 (
 242 btScalar x
 243 )
 244 {
 245         if (x < 0.0) {
 246                 return (int) (x - 0.5);
 247         }
 248         return (int) (x + 0.5);
 249 }
 250
 251
 252
 253 /// given input vector, return quantized version
 254 /**
 255   This routine is basically determining the gridpoint indices for a given
 256   input vector, answering the question: "which gridpoint is closest to the
 257   provided point?".
 258
 259   "with clamp" means that we restrict the point to be in the heightfield's
 260   axis-aligned bounding box.
 261  */
 262 void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& point,int /*isMax*/) const
 263 {
 264         btVector3 clampedPoint(point);
 265         clampedPoint.setMax(m_localAabbMin);
 266         clampedPoint.setMin(m_localAabbMax);
 267
 268         out[0] = getQuantized(clampedPoint.getX());
 269         out[1] = getQuantized(clampedPoint.getY());
 270         out[2] = getQuantized(clampedPoint.getZ());
 271
 272 }
 273
 274
 275
 276 /// process all triangles within the provided axis-aligned bounding box
 277 /**
 278   basic algorithm:
 279     - convert input aabb to local coordinates (scale down and shift for local origin)
 280     - convert input aabb to a range of heightfield grid points (quantize)
 281     - iterate over all triangles in that subset of the grid
 282  */
 283 void    btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
 284 {
 285         // scale down the input aabb's so they are in local (non-scaled) coordinates
 286         btVector3       localAabbMin = aabbMin*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]);
 287         btVector3       localAabbMax = aabbMax*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]);
 288
 289         // account for local origin
 290         localAabbMin += m_localOrigin;
 291         localAabbMax += m_localOrigin;
 292
 293         //quantize the aabbMin and aabbMax, and adjust the start/end ranges
 294         int     quantizedAabbMin[3];
 295         int     quantizedAabbMax[3];
 296         quantizeWithClamp(quantizedAabbMin, localAabbMin,0);
 297         quantizeWithClamp(quantizedAabbMax, localAabbMax,1);
 298
 299         // expand the min/max quantized values
 300         // this is to catch the case where the input aabb falls between grid points!
 301         for (int i = 0; i < 3; ++i) {
 302                 quantizedAabbMin[i]--;
 303                 quantizedAabbMax[i]++;
 304         }
 305
 306         int startX=0;
 307         int endX=m_heightStickWidth-1;
 308         int startJ=0;
 309         int endJ=m_heightStickLength-1;
 310
 311         switch (m_upAxis)
 312         {
 313         case 0:
 314                 {
 315                         if (quantizedAabbMin[1]>startX)
 316                                 startX = quantizedAabbMin[1];
 317                         if (quantizedAabbMax[1]<endX)
 318                                 endX = quantizedAabbMax[1];
 319                         if (quantizedAabbMin[2]>startJ)
 320                                 startJ = quantizedAabbMin[2];
 321                         if (quantizedAabbMax[2]<endJ)
 322                                 endJ = quantizedAabbMax[2];
 323                         break;
 324                 }
 325         case 1:
 326                 {
 327                         if (quantizedAabbMin[0]>startX)
 328                                 startX = quantizedAabbMin[0];
 329                         if (quantizedAabbMax[0]<endX)
 330                                 endX = quantizedAabbMax[0];
 331                         if (quantizedAabbMin[2]>startJ)
 332                                 startJ = quantizedAabbMin[2];
 333                         if (quantizedAabbMax[2]<endJ)
 334                                 endJ = quantizedAabbMax[2];
 335                         break;
 336                 };
 337         case 2:
 338                 {
 339                         if (quantizedAabbMin[0]>startX)
 340                                 startX = quantizedAabbMin[0];
 341                         if (quantizedAabbMax[0]<endX)
 342                                 endX = quantizedAabbMax[0];
 343                         if (quantizedAabbMin[1]>startJ)
 344                                 startJ = quantizedAabbMin[1];
 345                         if (quantizedAabbMax[1]<endJ)
 346                                 endJ = quantizedAabbMax[1];
 347                         break;
 348                 }
 349         default:
 350                 {
 351                         //need to get valid m_upAxis
 352                         btAssert(0);
 353                 }
 354         }
 355
 356
 357
 358
 359         for(int j=startJ; j<endJ; j++)
 360         {
 361                 for(int x=startX; x<endX; x++)
 362                 {
 363                         btVector3 vertices[3];
 364                         if (m_flipQuadEdges || (m_useDiamondSubdivision && !((j+x) & 1))|| (m_useZigzagSubdivision  && !(j & 1)))
 365                         {
 366         //first triangle
 367         getVertex(x,j,vertices[0]);
 368         getVertex(x+1,j,vertices[1]);
 369         getVertex(x+1,j+1,vertices[2]);
 370         callback->processTriangle(vertices,x,j);
 371         //second triangle
 372         getVertex(x,j,vertices[0]);
 373         getVertex(x+1,j+1,vertices[1]);
 374         getVertex(x,j+1,vertices[2]);
 375         callback->processTriangle(vertices,x,j);
 376                         } else
 377                         {
 378         //first triangle
 379         getVertex(x,j,vertices[0]);
 380         getVertex(x,j+1,vertices[1]);
 381         getVertex(x+1,j,vertices[2]);
 382         callback->processTriangle(vertices,x,j);
 383         //second triangle
 384         getVertex(x+1,j,vertices[0]);
 385         getVertex(x,j+1,vertices[1]);
 386         getVertex(x+1,j+1,vertices[2]);
 387         callback->processTriangle(vertices,x,j);
 388                         }
 389                 }
 390         }
 391
 392
 393
 394 }
 395
 396 void    btHeightfieldTerrainShape::calculateLocalInertia(btScalar ,btVector3& inertia) const
 397 {
 398         //moving concave objects not supported
 399
 400         inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
 401 }
 402
 403 void    btHeightfieldTerrainShape::setLocalScaling(const btVector3& scaling)
 404 {
 405         m_localScaling = scaling;
 406 }
 407 const btVector3& btHeightfieldTerrainShape::getLocalScaling() const
 408 {
 409         return m_localScaling;
 410 }