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);// && "bad upAxis--should be in range [0,2]"); 68 btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT);// && "Bad height data type enum"); 69 70 // initialize member variables 71 m_shapeType = TERRAIN_SHAPE_PROXYTYPE; 72 m_heightStickWidth = heightStickWidth; 73 m_heightStickLength = heightStickLength; 74 m_minHeight = minHeight; 75 m_maxHeight = maxHeight; 76 m_width = (btScalar) (heightStickWidth - 1); 77 m_length = (btScalar) (heightStickLength - 1); 78 m_heightScale = heightScale; 79 m_heightfieldDataUnknown = heightfieldData; 80 m_heightDataType = hdt; 81 m_flipQuadEdges = flipQuadEdges; 82 m_useDiamondSubdivision = false; 83 m_useZigzagSubdivision = false; 84 m_upAxis = upAxis; 85 m_localScaling.setValue(btScalar(1.), btScalar(1.), btScalar(1.)); 86 87 // determine min/max axis-aligned bounding box (aabb) values 88 switch (m_upAxis) 89 { 90 case 0: 91 { 92 m_localAabbMin.setValue(m_minHeight, 0, 0); 93 m_localAabbMax.setValue(m_maxHeight, m_width, m_length); 94 break; 95 } 96 case 1: 97 { 98 m_localAabbMin.setValue(0, m_minHeight, 0); 99 m_localAabbMax.setValue(m_width, m_maxHeight, m_length); 100 break; 101 }; 102 case 2: 103 { 104 m_localAabbMin.setValue(0, 0, m_minHeight); 105 m_localAabbMax.setValue(m_width, m_length, m_maxHeight); 106 break; 107 } 108 default: 109 { 110 //need to get valid m_upAxis 111 btAssert(0);// && "Bad m_upAxis"); 112 } 113 } 114 115 // remember origin (defined as exact middle of aabb) 116 m_localOrigin = btScalar(0.5) * (m_localAabbMin + m_localAabbMax); 117 } 118 119 120 121 btHeightfieldTerrainShape::~btHeightfieldTerrainShape() 122 { 123 } 124 125 126 127 void btHeightfieldTerrainShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const 128 { 129 btVector3 halfExtents = (m_localAabbMax-m_localAabbMin)* m_localScaling * btScalar(0.5); 130 131 btVector3 localOrigin(0, 0, 0); 132 localOrigin[m_upAxis] = (m_minHeight + m_maxHeight) * btScalar(0.5); 133 localOrigin *= m_localScaling; 134 135 btMatrix3x3 abs_b = t.getBasis().absolute(); 136 btVector3 center = t.getOrigin(); 137 btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); 138 extent += btVector3(getMargin(),getMargin(),getMargin()); 139 140 aabbMin = center - extent; 141 aabbMax = center + extent; 142 } 143 144 145 /// This returns the "raw" (user's initial) height, not the actual height. 146 /// The actual height needs to be adjusted to be relative to the center 147 /// of the heightfield's AABB. 148 btScalar 149 btHeightfieldTerrainShape::getRawHeightFieldValue(int x,int y) const 150 { 151 btScalar val = 0.f; 152 switch (m_heightDataType) 153 { 154 case PHY_FLOAT: 155 { 156 val = m_heightfieldDataFloat[(y*m_heightStickWidth)+x]; 157 break; 158 } 159 160 case PHY_UCHAR: 161 { 162 unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y*m_heightStickWidth)+x]; 163 val = heightFieldValue * m_heightScale; 164 break; 165 } 166 167 case PHY_SHORT: 168 { 169 short hfValue = m_heightfieldDataShort[(y * m_heightStickWidth) + x]; 170 val = hfValue * m_heightScale; 171 break; 172 } 173 174 default: 175 { 176 btAssert(!"Bad m_heightDataType"); 177 } 178 } 179 180 return val; 181 } 182 183 184 185 186 /// this returns the vertex in bullet-local coordinates 187 void btHeightfieldTerrainShape::getVertex(int x,int y,btVector3& vertex) const 188 { 189 btAssert(x>=0); 190 btAssert(y>=0); 191 btAssert(x<m_heightStickWidth); 192 btAssert(y<m_heightStickLength); 193 194 btScalar height = getRawHeightFieldValue(x,y); 195 196 switch (m_upAxis) 197 { 198 case 0: 199 { 200 vertex.setValue( 201 height - m_localOrigin.getX(), 202 (-m_width/btScalar(2.0)) + x, 203 (-m_length/btScalar(2.0) ) + y 204 ); 205 break; 206 } 207 case 1: 208 { 209 vertex.setValue( 210 (-m_width/btScalar(2.0)) + x, 211 height - m_localOrigin.getY(), 212 (-m_length/btScalar(2.0)) + y 213 ); 214 break; 215 }; 216 case 2: 217 { 218 vertex.setValue( 219 (-m_width/btScalar(2.0)) + x, 220 (-m_length/btScalar(2.0)) + y, 221 height - m_localOrigin.getZ() 222 ); 223 break; 224 } 225 default: 226 { 227 //need to get valid m_upAxis 228 btAssert(0); 229 } 230 } 231 232 vertex*=m_localScaling; 233 } 234 235 236 237 static inline int 238 getQuantized 239 ( 240 btScalar x 241 ) 242 { 243 if (x < 0.0) { 244 return (int) (x - 0.5); 245 } 246 return (int) (x + 0.5); 247 } 248 249 250 251 /// given input vector, return quantized version 252 /** 253 This routine is basically determining the gridpoint indices for a given 254 input vector, answering the question: "which gridpoint is closest to the 255 provided point?". 256 257 "with clamp" means that we restrict the point to be in the heightfield's 258 axis-aligned bounding box. 259 */ 260 void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& point,int /*isMax*/) const 261 { 262 btVector3 clampedPoint(point); 263 clampedPoint.setMax(m_localAabbMin); 264 clampedPoint.setMin(m_localAabbMax); 265 266 out[0] = getQuantized(clampedPoint.getX()); 267 out[1] = getQuantized(clampedPoint.getY()); 268 out[2] = getQuantized(clampedPoint.getZ()); 269 270 } 271 272 273 274 /// process all triangles within the provided axis-aligned bounding box 275 /** 276 basic algorithm: 277 - convert input aabb to local coordinates (scale down and shift for local origin) 278 - convert input aabb to a range of heightfield grid points (quantize) 279 - iterate over all triangles in that subset of the grid 280 */ 281 void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const 282 { 283 // scale down the input aabb's so they are in local (non-scaled) coordinates 284 btVector3 localAabbMin = aabbMin*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]); 285 btVector3 localAabbMax = aabbMax*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]); 286 287 // account for local origin 288 localAabbMin += m_localOrigin; 289 localAabbMax += m_localOrigin; 290 291 //quantize the aabbMin and aabbMax, and adjust the start/end ranges 292 int quantizedAabbMin[3]; 293 int quantizedAabbMax[3]; 294 quantizeWithClamp(quantizedAabbMin, localAabbMin,0); 295 quantizeWithClamp(quantizedAabbMax, localAabbMax,1); 296 297 // expand the min/max quantized values 298 // this is to catch the case where the input aabb falls between grid points! 299 for (int i = 0; i < 3; ++i) { 300 quantizedAabbMin[i]--; 301 quantizedAabbMax[i]++; 302 } 303 304 int startX=0; 305 int endX=m_heightStickWidth-1; 306 int startJ=0; 307 int endJ=m_heightStickLength-1; 308 309 switch (m_upAxis) 310 { 311 case 0: 312 { 313 if (quantizedAabbMin[1]>startX) 314 startX = quantizedAabbMin[1]; 315 if (quantizedAabbMax[1]<endX) 316 endX = quantizedAabbMax[1]; 317 if (quantizedAabbMin[2]>startJ) 318 startJ = quantizedAabbMin[2]; 319 if (quantizedAabbMax[2]<endJ) 320 endJ = quantizedAabbMax[2]; 321 break; 322 } 323 case 1: 324 { 325 if (quantizedAabbMin[0]>startX) 326 startX = quantizedAabbMin[0]; 327 if (quantizedAabbMax[0]<endX) 328 endX = quantizedAabbMax[0]; 329 if (quantizedAabbMin[2]>startJ) 330 startJ = quantizedAabbMin[2]; 331 if (quantizedAabbMax[2]<endJ) 332 endJ = quantizedAabbMax[2]; 333 break; 334 }; 335 case 2: 336 { 337 if (quantizedAabbMin[0]>startX) 338 startX = quantizedAabbMin[0]; 339 if (quantizedAabbMax[0]<endX) 340 endX = quantizedAabbMax[0]; 341 if (quantizedAabbMin[1]>startJ) 342 startJ = quantizedAabbMin[1]; 343 if (quantizedAabbMax[1]<endJ) 344 endJ = quantizedAabbMax[1]; 345 break; 346 } 347 default: 348 { 349 //need to get valid m_upAxis 350 btAssert(0); 351 } 352 } 353 354 355 356 357 for(int j=startJ; j<endJ; j++) 358 { 359 for(int x=startX; x<endX; x++) 360 { 361 btVector3 vertices[3]; 362 if (m_flipQuadEdges || (m_useDiamondSubdivision && !((j+x) & 1))|| (m_useZigzagSubdivision && !(j & 1))) 363 { 364 //first triangle 365 getVertex(x,j,vertices[0]); 366 getVertex(x, j + 1, vertices[1]); 367 getVertex(x + 1, j + 1, vertices[2]); 368 callback->processTriangle(vertices,x,j); 369 //second triangle 370 // getVertex(x,j,vertices[0]);//already got this vertex before, thanks to Danny Chapman 371 getVertex(x+1,j+1,vertices[1]); 372 getVertex(x + 1, j, vertices[2]); 373 callback->processTriangle(vertices, x, j); 374 375 } else 376 { 377 //first triangle 378 getVertex(x,j,vertices[0]); 379 getVertex(x,j+1,vertices[1]); 380 getVertex(x+1,j,vertices[2]); 381 callback->processTriangle(vertices,x,j); 382 //second triangle 383 getVertex(x+1,j,vertices[0]); 384 //getVertex(x,j+1,vertices[1]); 385 getVertex(x+1,j+1,vertices[2]); 386 callback->processTriangle(vertices,x,j); 387 } 388 } 389 } 390 391 392 393 } 394 395 void btHeightfieldTerrainShape::calculateLocalInertia(btScalar ,btVector3& inertia) const 396 { 397 //moving concave objects not supported 398 399 inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); 400 } 401 402 void btHeightfieldTerrainShape::setLocalScaling(const btVector3& scaling) 403 { 404 m_localScaling = scaling; 405 } 406 const btVector3& btHeightfieldTerrainShape::getLocalScaling() const 407 { 408 return m_localScaling; 409 } 410
