diff options
Diffstat (limited to 'extern/bullet2/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp')
| -rw-r--r-- | extern/bullet2/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp | 411 |
1 files changed, 411 insertions, 0 deletions
diff --git a/extern/bullet2/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/extern/bullet2/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp new file mode 100644 index 00000000000..3a1e6f4a2b9 --- /dev/null +++ b/extern/bullet2/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp @@ -0,0 +1,411 @@ +/* +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 "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; +} |