| /* |
| Bullet Continuous Collision Detection and Physics Library |
| Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org |
| |
| This software is provided 'as-is', without any express or implied warranty. |
| In no event will the authors be held liable for any damages arising from the use of this software. |
| Permission is granted to anyone to use this software for any purpose, |
| including commercial applications, and to alter it and redistribute it freely, |
| subject to the following restrictions: |
| |
| 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. |
| 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. |
| 3. This notice may not be removed or altered from any source distribution. |
| */ |
| |
| #include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h" |
| |
| #include "LinearMath/btTransformUtil.h" |
| |
| |
| |
| btHeightfieldTerrainShape::btHeightfieldTerrainShape |
| ( |
| int heightStickWidth, int heightStickLength, 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); |
| } |
| |
| |
| |
| btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength,void* heightfieldData,btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges) |
| { |
| // legacy constructor: support only float or unsigned char, |
| // and min height is zero |
| PHY_ScalarType hdt = (useFloatData) ? PHY_FLOAT : PHY_UCHAR; |
| btScalar minHeight = 0.0; |
| |
| // previously, height = uchar * maxHeight / 65535. |
| // So to preserve legacy behavior, heightScale = maxHeight / 65535 |
| btScalar heightScale = maxHeight / 65535; |
| |
| initialize(heightStickWidth, heightStickLength, heightfieldData, |
| heightScale, minHeight, maxHeight, upAxis, hdt, |
| flipQuadEdges); |
| } |
| |
| |
| |
| void btHeightfieldTerrainShape::initialize |
| ( |
| int heightStickWidth, int heightStickLength, 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(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"); |
| |
| // initialize member variables |
| m_shapeType = TERRAIN_SHAPE_PROXYTYPE; |
| m_heightStickWidth = heightStickWidth; |
| m_heightStickLength = heightStickLength; |
| m_minHeight = minHeight; |
| m_maxHeight = maxHeight; |
| m_width = (btScalar) (heightStickWidth - 1); |
| m_length = (btScalar) (heightStickLength - 1); |
| m_heightScale = heightScale; |
| m_heightfieldDataUnknown = heightfieldData; |
| m_heightDataType = hdt; |
| m_flipQuadEdges = flipQuadEdges; |
| m_useDiamondSubdivision = false; |
| m_upAxis = upAxis; |
| m_localScaling.setValue(btScalar(1.), btScalar(1.), btScalar(1.)); |
| |
| // determine min/max axis-aligned bounding box (aabb) values |
| switch (m_upAxis) |
| { |
| case 0: |
| { |
| m_localAabbMin.setValue(m_minHeight, 0, 0); |
| m_localAabbMax.setValue(m_maxHeight, m_width, m_length); |
| break; |
| } |
| case 1: |
| { |
| m_localAabbMin.setValue(0, m_minHeight, 0); |
| m_localAabbMax.setValue(m_width, m_maxHeight, m_length); |
| break; |
| }; |
| case 2: |
| { |
| m_localAabbMin.setValue(0, 0, m_minHeight); |
| m_localAabbMax.setValue(m_width, m_length, m_maxHeight); |
| break; |
| } |
| default: |
| { |
| //need to get valid m_upAxis |
| btAssert(0 && "Bad m_upAxis"); |
| } |
| } |
| |
| // remember origin (defined as exact middle of aabb) |
| m_localOrigin = btScalar(0.5) * (m_localAabbMin + m_localAabbMax); |
| } |
| |
| |
| |
| btHeightfieldTerrainShape::~btHeightfieldTerrainShape() |
| { |
| } |
| |
| |
| |
| void btHeightfieldTerrainShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const |
| { |
| 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(); |
| btVector3 center = t.getOrigin(); |
| btVector3 extent = btVector3(abs_b[0].dot(halfExtents), |
| abs_b[1].dot(halfExtents), |
| abs_b[2].dot(halfExtents)); |
| 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 |
| { |
| btScalar val = 0.f; |
| switch (m_heightDataType) |
| { |
| case PHY_FLOAT: |
| { |
| val = m_heightfieldDataFloat[(y*m_heightStickWidth)+x]; |
| break; |
| } |
| |
| case PHY_UCHAR: |
| { |
| unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y*m_heightStickWidth)+x]; |
| val = heightFieldValue * m_heightScale; |
| break; |
| } |
| |
| case PHY_SHORT: |
| { |
| short hfValue = m_heightfieldDataShort[(y * m_heightStickWidth) + x]; |
| val = hfValue * m_heightScale; |
| break; |
| } |
| |
| default: |
| { |
| btAssert(!"Bad m_heightDataType"); |
| } |
| } |
| |
| return val; |
| } |
| |
| |
| |
| |
| /// this returns the vertex in bullet-local coordinates |
| void btHeightfieldTerrainShape::getVertex(int x,int y,btVector3& vertex) const |
| { |
| btAssert(x>=0); |
| btAssert(y>=0); |
| btAssert(x<m_heightStickWidth); |
| btAssert(y<m_heightStickLength); |
| |
| btScalar height = getRawHeightFieldValue(x,y); |
| |
| switch (m_upAxis) |
| { |
| case 0: |
| { |
| vertex.setValue( |
| height - m_localOrigin.getX(), |
| (-m_width/btScalar(2.0)) + x, |
| (-m_length/btScalar(2.0) ) + y |
| ); |
| break; |
| } |
| case 1: |
| { |
| vertex.setValue( |
| (-m_width/btScalar(2.0)) + x, |
| height - m_localOrigin.getY(), |
| (-m_length/btScalar(2.0)) + y |
| ); |
| break; |
| }; |
| case 2: |
| { |
| vertex.setValue( |
| (-m_width/btScalar(2.0)) + x, |
| (-m_length/btScalar(2.0)) + y, |
| height - m_localOrigin.getZ() |
| ); |
| break; |
| } |
| default: |
| { |
| //need to get valid m_upAxis |
| btAssert(0); |
| } |
| } |
| |
| vertex*=m_localScaling; |
| } |
| |
| |
| |
| static inline int |
| getQuantized |
| ( |
| btScalar x |
| ) |
| { |
| if (x < 0.0) { |
| 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 |
| input vector, answering the question: "which gridpoint is closest to the |
| provided point?". |
| |
| "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 |
| { |
| btVector3 clampedPoint(point); |
| clampedPoint.setMax(m_localAabbMin); |
| clampedPoint.setMin(m_localAabbMax); |
| |
| 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: |
| - convert input aabb to local coordinates (scale down and shift for local origin) |
| - 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 |
| { |
| // 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]); |
| |
| // 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); |
| |
| // 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) { |
| quantizedAabbMin[i]--; |
| quantizedAabbMax[i]++; |
| } |
| |
| int startX=0; |
| int endX=m_heightStickWidth-1; |
| int startJ=0; |
| int endJ=m_heightStickLength-1; |
| |
| switch (m_upAxis) |
| { |
| case 0: |
| { |
| if (quantizedAabbMin[1]>startX) |
| startX = quantizedAabbMin[1]; |
| if (quantizedAabbMax[1]<endX) |
| endX = quantizedAabbMax[1]; |
| if (quantizedAabbMin[2]>startJ) |
| startJ = quantizedAabbMin[2]; |
| if (quantizedAabbMax[2]<endJ) |
| endJ = quantizedAabbMax[2]; |
| break; |
| } |
| case 1: |
| { |
| if (quantizedAabbMin[0]>startX) |
| startX = quantizedAabbMin[0]; |
| if (quantizedAabbMax[0]<endX) |
| endX = quantizedAabbMax[0]; |
| if (quantizedAabbMin[2]>startJ) |
| startJ = quantizedAabbMin[2]; |
| if (quantizedAabbMax[2]<endJ) |
| endJ = quantizedAabbMax[2]; |
| break; |
| }; |
| case 2: |
| { |
| if (quantizedAabbMin[0]>startX) |
| startX = quantizedAabbMin[0]; |
| if (quantizedAabbMax[0]<endX) |
| endX = quantizedAabbMax[0]; |
| if (quantizedAabbMin[1]>startJ) |
| startJ = quantizedAabbMin[1]; |
| if (quantizedAabbMax[1]<endJ) |
| endJ = quantizedAabbMax[1]; |
| break; |
| } |
| default: |
| { |
| //need to get valid m_upAxis |
| btAssert(0); |
| } |
| } |
| |
| |
| |
| |
| for(int j=startJ; j<endJ; j++) |
| { |
| for(int x=startX; x<endX; x++) |
| { |
| btVector3 vertices[3]; |
| if (m_flipQuadEdges || (m_useDiamondSubdivision && !((j+x) & 1))) |
| { |
| //first triangle |
| getVertex(x,j,vertices[0]); |
| getVertex(x+1,j,vertices[1]); |
| getVertex(x+1,j+1,vertices[2]); |
| callback->processTriangle(vertices,x,j); |
| //second triangle |
| getVertex(x,j,vertices[0]); |
| getVertex(x+1,j+1,vertices[1]); |
| getVertex(x,j+1,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); |
| } |
| } |
| } |
| |
| |
| |
| } |
| |
| void btHeightfieldTerrainShape::calculateLocalInertia(btScalar ,btVector3& inertia) const |
| { |
| //moving concave objects not supported |
| |
| inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); |
| } |
| |
| void btHeightfieldTerrainShape::setLocalScaling(const btVector3& scaling) |
| { |
| m_localScaling = scaling; |
| } |
| const btVector3& btHeightfieldTerrainShape::getLocalScaling() const |
| { |
| return m_localScaling; |
| } |