diff options
Diffstat (limited to 'thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp')
| -rw-r--r-- | thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp | 286 |
1 files changed, 126 insertions, 160 deletions
diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp index 441a89c6bb..c85ce2498e 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp @@ -17,23 +17,17 @@ subject to the following restrictions: #include "LinearMath/btTransformUtil.h" - - -btHeightfieldTerrainShape::btHeightfieldTerrainShape -( -int heightStickWidth, int heightStickLength, const void* heightfieldData, -btScalar heightScale, btScalar minHeight, btScalar maxHeight,int upAxis, -PHY_ScalarType hdt, bool flipQuadEdges -) +btHeightfieldTerrainShape::btHeightfieldTerrainShape( + int heightStickWidth, int heightStickLength, const void* heightfieldData, + btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis, + PHY_ScalarType hdt, bool flipQuadEdges) { initialize(heightStickWidth, heightStickLength, heightfieldData, - heightScale, minHeight, maxHeight, upAxis, hdt, - flipQuadEdges); + heightScale, minHeight, maxHeight, upAxis, hdt, + flipQuadEdges); } - - -btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength,const void* heightfieldData,btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges) +btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength, const void* heightfieldData, btScalar maxHeight, int upAxis, bool useFloatData, bool flipQuadEdges) { // legacy constructor: support only float or unsigned char, // and min height is zero @@ -45,27 +39,23 @@ btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int h btScalar heightScale = maxHeight / 65535; initialize(heightStickWidth, heightStickLength, heightfieldData, - heightScale, minHeight, maxHeight, upAxis, hdt, - flipQuadEdges); + heightScale, minHeight, maxHeight, upAxis, hdt, + flipQuadEdges); } - - -void btHeightfieldTerrainShape::initialize -( -int heightStickWidth, int heightStickLength, const void* heightfieldData, -btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis, -PHY_ScalarType hdt, bool flipQuadEdges -) +void btHeightfieldTerrainShape::initialize( + int heightStickWidth, int heightStickLength, const void* heightfieldData, + btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis, + PHY_ScalarType hdt, bool flipQuadEdges) { // validation - btAssert(heightStickWidth > 1);// && "bad width"); - btAssert(heightStickLength > 1);// && "bad length"); - btAssert(heightfieldData);// && "null heightfield data"); + btAssert(heightStickWidth > 1); // && "bad width"); + btAssert(heightStickLength > 1); // && "bad length"); + btAssert(heightfieldData); // && "null heightfield data"); // btAssert(heightScale) -- do we care? Trust caller here - btAssert(minHeight <= maxHeight);// && "bad min/max height"); - btAssert(upAxis >= 0 && upAxis < 3);// && "bad upAxis--should be in range [0,2]"); - btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT);// && "Bad height data type enum"); + btAssert(minHeight <= maxHeight); // && "bad min/max height"); + btAssert(upAxis >= 0 && upAxis < 3); // && "bad upAxis--should be in range [0,2]"); + btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT); // && "Bad height data type enum"); // initialize member variables m_shapeType = TERRAIN_SHAPE_PROXYTYPE; @@ -73,8 +63,8 @@ PHY_ScalarType hdt, bool flipQuadEdges m_heightStickLength = heightStickLength; m_minHeight = minHeight; m_maxHeight = maxHeight; - m_width = (btScalar) (heightStickWidth - 1); - m_length = (btScalar) (heightStickLength - 1); + m_width = (btScalar)(heightStickWidth - 1); + m_length = (btScalar)(heightStickLength - 1); m_heightScale = heightScale; m_heightfieldDataUnknown = heightfieldData; m_heightDataType = hdt; @@ -87,28 +77,28 @@ PHY_ScalarType hdt, bool flipQuadEdges // determine min/max axis-aligned bounding box (aabb) values switch (m_upAxis) { - case 0: + case 0: { m_localAabbMin.setValue(m_minHeight, 0, 0); m_localAabbMax.setValue(m_maxHeight, m_width, m_length); break; } - case 1: + case 1: { m_localAabbMin.setValue(0, m_minHeight, 0); m_localAabbMax.setValue(m_width, m_maxHeight, m_length); break; }; - case 2: + case 2: { m_localAabbMin.setValue(0, 0, m_minHeight); m_localAabbMax.setValue(m_width, m_length, m_maxHeight); break; } - default: + default: { //need to get valid m_upAxis - btAssert(0);// && "Bad m_upAxis"); + btAssert(0); // && "Bad m_upAxis"); } } @@ -116,62 +106,57 @@ PHY_ScalarType hdt, bool flipQuadEdges m_localOrigin = btScalar(0.5) * (m_localAabbMin + m_localAabbMax); } - - btHeightfieldTerrainShape::~btHeightfieldTerrainShape() { } - - -void btHeightfieldTerrainShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +void btHeightfieldTerrainShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { - btVector3 halfExtents = (m_localAabbMax-m_localAabbMin)* m_localScaling * btScalar(0.5); + btVector3 halfExtents = (m_localAabbMax - m_localAabbMin) * m_localScaling * btScalar(0.5); btVector3 localOrigin(0, 0, 0); localOrigin[m_upAxis] = (m_minHeight + m_maxHeight) * btScalar(0.5); localOrigin *= m_localScaling; - btMatrix3x3 abs_b = t.getBasis().absolute(); + btMatrix3x3 abs_b = t.getBasis().absolute(); btVector3 center = t.getOrigin(); - btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); - extent += btVector3(getMargin(),getMargin(),getMargin()); + btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); + extent += btVector3(getMargin(), getMargin(), getMargin()); aabbMin = center - extent; aabbMax = center + extent; } - /// This returns the "raw" (user's initial) height, not the actual height. /// The actual height needs to be adjusted to be relative to the center /// of the heightfield's AABB. btScalar -btHeightfieldTerrainShape::getRawHeightFieldValue(int x,int y) const +btHeightfieldTerrainShape::getRawHeightFieldValue(int x, int y) const { btScalar val = 0.f; switch (m_heightDataType) { - case PHY_FLOAT: + case PHY_FLOAT: { - val = m_heightfieldDataFloat[(y*m_heightStickWidth)+x]; + val = m_heightfieldDataFloat[(y * m_heightStickWidth) + x]; break; } - case PHY_UCHAR: + case PHY_UCHAR: { - unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y*m_heightStickWidth)+x]; + unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y * m_heightStickWidth) + x]; val = heightFieldValue * m_heightScale; break; } - case PHY_SHORT: + case PHY_SHORT: { short hfValue = m_heightfieldDataShort[(y * m_heightStickWidth) + x]; val = hfValue * m_heightScale; break; } - default: + default: { btAssert(!"Bad m_heightDataType"); } @@ -180,74 +165,63 @@ btHeightfieldTerrainShape::getRawHeightFieldValue(int x,int y) const return val; } - - - /// this returns the vertex in bullet-local coordinates -void btHeightfieldTerrainShape::getVertex(int x,int y,btVector3& vertex) const +void btHeightfieldTerrainShape::getVertex(int x, int y, btVector3& vertex) const { - btAssert(x>=0); - btAssert(y>=0); - btAssert(x<m_heightStickWidth); - btAssert(y<m_heightStickLength); + btAssert(x >= 0); + btAssert(y >= 0); + btAssert(x < m_heightStickWidth); + btAssert(y < m_heightStickLength); - btScalar height = getRawHeightFieldValue(x,y); + btScalar height = getRawHeightFieldValue(x, y); switch (m_upAxis) { - case 0: + case 0: { - vertex.setValue( - height - m_localOrigin.getX(), - (-m_width/btScalar(2.0)) + x, - (-m_length/btScalar(2.0) ) + y - ); + vertex.setValue( + height - m_localOrigin.getX(), + (-m_width / btScalar(2.0)) + x, + (-m_length / btScalar(2.0)) + y); break; } - case 1: + case 1: { vertex.setValue( - (-m_width/btScalar(2.0)) + x, - height - m_localOrigin.getY(), - (-m_length/btScalar(2.0)) + y - ); + (-m_width / btScalar(2.0)) + x, + height - m_localOrigin.getY(), + (-m_length / btScalar(2.0)) + y); break; }; - case 2: + case 2: { vertex.setValue( - (-m_width/btScalar(2.0)) + x, - (-m_length/btScalar(2.0)) + y, - height - m_localOrigin.getZ() - ); + (-m_width / btScalar(2.0)) + x, + (-m_length / btScalar(2.0)) + y, + height - m_localOrigin.getZ()); break; } - default: + default: { //need to get valid m_upAxis btAssert(0); } } - vertex*=m_localScaling; + vertex *= m_localScaling; } - - static inline int -getQuantized -( -btScalar x -) +getQuantized( + btScalar x) { - if (x < 0.0) { - return (int) (x - 0.5); + if (x < 0.0) + { + return (int)(x - 0.5); } - return (int) (x + 0.5); + return (int)(x + 0.5); } - - /// given input vector, return quantized version /** This routine is basically determining the gridpoint indices for a given @@ -257,7 +231,7 @@ btScalar x "with clamp" means that we restrict the point to be in the heightfield's axis-aligned bounding box. */ -void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& point,int /*isMax*/) const +void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& point, int /*isMax*/) const { btVector3 clampedPoint(point); clampedPoint.setMax(m_localAabbMin); @@ -266,11 +240,8 @@ void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& poi out[0] = getQuantized(clampedPoint.getX()); out[1] = getQuantized(clampedPoint.getY()); out[2] = getQuantized(clampedPoint.getZ()); - } - - /// process all triangles within the provided axis-aligned bounding box /** basic algorithm: @@ -278,128 +249,123 @@ void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& poi - convert input aabb to a range of heightfield grid points (quantize) - iterate over all triangles in that subset of the grid */ -void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const { // scale down the input aabb's so they are in local (non-scaled) coordinates - btVector3 localAabbMin = aabbMin*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]); - btVector3 localAabbMax = aabbMax*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]); + btVector3 localAabbMin = aabbMin * btVector3(1.f / m_localScaling[0], 1.f / m_localScaling[1], 1.f / m_localScaling[2]); + btVector3 localAabbMax = aabbMax * btVector3(1.f / m_localScaling[0], 1.f / m_localScaling[1], 1.f / m_localScaling[2]); // account for local origin localAabbMin += m_localOrigin; localAabbMax += m_localOrigin; //quantize the aabbMin and aabbMax, and adjust the start/end ranges - int quantizedAabbMin[3]; - int quantizedAabbMax[3]; - quantizeWithClamp(quantizedAabbMin, localAabbMin,0); - quantizeWithClamp(quantizedAabbMax, localAabbMax,1); - + int quantizedAabbMin[3]; + int quantizedAabbMax[3]; + quantizeWithClamp(quantizedAabbMin, localAabbMin, 0); + quantizeWithClamp(quantizedAabbMax, localAabbMax, 1); + // expand the min/max quantized values // this is to catch the case where the input aabb falls between grid points! - for (int i = 0; i < 3; ++i) { + for (int i = 0; i < 3; ++i) + { quantizedAabbMin[i]--; quantizedAabbMax[i]++; - } + } - int startX=0; - int endX=m_heightStickWidth-1; - int startJ=0; - int endJ=m_heightStickLength-1; + int startX = 0; + int endX = m_heightStickWidth - 1; + int startJ = 0; + int endJ = m_heightStickLength - 1; switch (m_upAxis) { - case 0: + case 0: { - if (quantizedAabbMin[1]>startX) + if (quantizedAabbMin[1] > startX) startX = quantizedAabbMin[1]; - if (quantizedAabbMax[1]<endX) + if (quantizedAabbMax[1] < endX) endX = quantizedAabbMax[1]; - if (quantizedAabbMin[2]>startJ) + if (quantizedAabbMin[2] > startJ) startJ = quantizedAabbMin[2]; - if (quantizedAabbMax[2]<endJ) + if (quantizedAabbMax[2] < endJ) endJ = quantizedAabbMax[2]; break; } - case 1: + case 1: { - if (quantizedAabbMin[0]>startX) + if (quantizedAabbMin[0] > startX) startX = quantizedAabbMin[0]; - if (quantizedAabbMax[0]<endX) + if (quantizedAabbMax[0] < endX) endX = quantizedAabbMax[0]; - if (quantizedAabbMin[2]>startJ) + if (quantizedAabbMin[2] > startJ) startJ = quantizedAabbMin[2]; - if (quantizedAabbMax[2]<endJ) + if (quantizedAabbMax[2] < endJ) endJ = quantizedAabbMax[2]; break; }; - case 2: + case 2: { - if (quantizedAabbMin[0]>startX) + if (quantizedAabbMin[0] > startX) startX = quantizedAabbMin[0]; - if (quantizedAabbMax[0]<endX) + if (quantizedAabbMax[0] < endX) endX = quantizedAabbMax[0]; - if (quantizedAabbMin[1]>startJ) + if (quantizedAabbMin[1] > startJ) startJ = quantizedAabbMin[1]; - if (quantizedAabbMax[1]<endJ) + if (quantizedAabbMax[1] < endJ) endJ = quantizedAabbMax[1]; break; } - default: + default: { //need to get valid m_upAxis btAssert(0); } } - - - - for(int j=startJ; j<endJ; j++) + for (int j = startJ; j < endJ; j++) { - for(int x=startX; x<endX; x++) + for (int x = startX; x < endX; x++) { btVector3 vertices[3]; - if (m_flipQuadEdges || (m_useDiamondSubdivision && !((j+x) & 1))|| (m_useZigzagSubdivision && !(j & 1))) + if (m_flipQuadEdges || (m_useDiamondSubdivision && !((j + x) & 1)) || (m_useZigzagSubdivision && !(j & 1))) { - //first triangle - getVertex(x,j,vertices[0]); - getVertex(x, j + 1, vertices[1]); - getVertex(x + 1, j + 1, vertices[2]); - callback->processTriangle(vertices,x,j); - //second triangle - // getVertex(x,j,vertices[0]);//already got this vertex before, thanks to Danny Chapman - getVertex(x+1,j+1,vertices[1]); - getVertex(x + 1, j, vertices[2]); - callback->processTriangle(vertices, x, j); - - } else + //first triangle + getVertex(x, j, vertices[0]); + getVertex(x, j + 1, vertices[1]); + getVertex(x + 1, j + 1, vertices[2]); + callback->processTriangle(vertices, x, j); + //second triangle + // getVertex(x,j,vertices[0]);//already got this vertex before, thanks to Danny Chapman + getVertex(x + 1, j + 1, vertices[1]); + getVertex(x + 1, j, vertices[2]); + callback->processTriangle(vertices, x, j); + } + else { - //first triangle - getVertex(x,j,vertices[0]); - getVertex(x,j+1,vertices[1]); - getVertex(x+1,j,vertices[2]); - callback->processTriangle(vertices,x,j); - //second triangle - getVertex(x+1,j,vertices[0]); - //getVertex(x,j+1,vertices[1]); - getVertex(x+1,j+1,vertices[2]); - callback->processTriangle(vertices,x,j); + //first triangle + getVertex(x, j, vertices[0]); + getVertex(x, j + 1, vertices[1]); + getVertex(x + 1, j, vertices[2]); + callback->processTriangle(vertices, x, j); + //second triangle + getVertex(x + 1, j, vertices[0]); + //getVertex(x,j+1,vertices[1]); + getVertex(x + 1, j + 1, vertices[2]); + callback->processTriangle(vertices, x, j); } } } - - - } -void btHeightfieldTerrainShape::calculateLocalInertia(btScalar ,btVector3& inertia) const +void btHeightfieldTerrainShape::calculateLocalInertia(btScalar, btVector3& inertia) const { //moving concave objects not supported - - inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + + inertia.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); } -void btHeightfieldTerrainShape::setLocalScaling(const btVector3& scaling) +void btHeightfieldTerrainShape::setLocalScaling(const btVector3& scaling) { m_localScaling = scaling; } |