← Back to branch summary

~ubuntu-branches/ubuntu/vivid/emscripten/vivid

« back to all changes in this revision

Viewing changes to tests/bullet/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp

  • Committer: Package Import Robot
  • Author(s): Sylvestre Ledru
  • Date: 2013-05-02 13:11:51 UTC
  • Revision ID: package-import@ubuntu.com-20130502131151-q8dvteqr1ef2x7xz
Tags: upstream-1.4.1~20130504~adb56cb
Import upstream version 1.4.1~20130504~adb56cb

Show diffs side-by-side

added added

removed removed

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