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