Home | History | Annotate | Download | only in CollisionShapes
      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