← Back to branch summary

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

« back to all changes in this revision

Viewing changes to tests/bullet/Extras/ConvexDecomposition/bestfit.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/Extras/ConvexDecomposition/bestfit.cpp
 
1
#include "float_math.h"
 
2
#include <stdio.h>
 
3
#include <stdlib.h>
 
4
#include <string.h>
 
5
#include <assert.h>
 
6
#include <math.h>
 
7
 
 
8
/*----------------------------------------------------------------------
 
9
                Copyright (c) 2004 Open Dynamics Framework Group
 
10
                                        www.physicstools.org
 
11
                All rights reserved.
 
12
 
 
13
                Redistribution and use in source and binary forms, with or without modification, are permitted provided
 
14
                that the following conditions are met:
 
15
 
 
16
                Redistributions of source code must retain the above copyright notice, this list of conditions
 
17
                and the following disclaimer.
 
18
 
 
19
                Redistributions in binary form must reproduce the above copyright notice,
 
20
                this list of conditions and the following disclaimer in the documentation
 
21
                and/or other materials provided with the distribution.
 
22
 
 
23
                Neither the name of the Open Dynamics Framework Group nor the names of its contributors may
 
24
                be used to endorse or promote products derived from this software without specific prior written permission.
 
25
 
 
26
                THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES,
 
27
                INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 
28
                DISCLAIMED. IN NO EVENT SHALL THE INTEL OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 
29
                EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 
30
                LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
 
31
                IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
 
32
                THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
33
-----------------------------------------------------------------------*/
 
34
 
 
35
// http://codesuppository.blogspot.com
 
36
//
 
37
// mailto: jratcliff@infiniplex.net
 
38
//
 
39
// http://www.amillionpixels.us
 
40
//
 
41
// Geometric Tools, Inc.
 
42
// http://www.geometrictools.com
 
43
// Copyright (c) 1998-2006.  All Rights Reserved
 
44
//
 
45
// The Wild Magic Library (WM3) source code is supplied under the terms of
 
46
// the license agreement
 
47
//     http://www.geometrictools.com/License/WildMagic3License.pdf
 
48
// and may not be copied or disclosed except in accordance with the terms
 
49
// of that agreement.
 
50
 
 
51
#include "bestfit.h"
 
52
 
 
53
namespace BestFit
 
54
{
 
55
 
 
56
class Vec3
 
57
{
 
58
public:
 
59
  Vec3(void) { };
 
60
  Vec3(float _x,float _y,float _z) { x = _x; y = _y; z = _z; };
 
61
 
 
62
 
 
63
  float dot(const Vec3 &v)
 
64
  {
 
65
    return x*v.x + y*v.y + z*v.z; // the dot product
 
66
  }
 
67
 
 
68
  float x;
 
69
  float y;
 
70
  float z;
 
71
};
 
72
 
 
73
 
 
74
class Eigen
 
75
{
 
76
public:
 
77
 
 
78
 
 
79
  void DecrSortEigenStuff(void)
 
80
  {
 
81
    Tridiagonal(); //diagonalize the matrix.
 
82
    QLAlgorithm(); //
 
83
    DecreasingSort();
 
84
    GuaranteeRotation();
 
85
  }
 
86
 
 
87
  void Tridiagonal(void)
 
88
  {
 
89
    float fM00 = mElement[0][0];
 
90
    float fM01 = mElement[0][1];
 
91
    float fM02 = mElement[0][2];
 
92
    float fM11 = mElement[1][1];
 
93
    float fM12 = mElement[1][2];
 
94
    float fM22 = mElement[2][2];
 
95
 
 
96
    m_afDiag[0] = fM00;
 
97
    m_afSubd[2] = 0;
 
98
    if (fM02 != (float)0.0)
 
99
    {
 
100
      float fLength = sqrtf(fM01*fM01+fM02*fM02);
 
101
      float fInvLength = ((float)1.0)/fLength;
 
102
      fM01 *= fInvLength;
 
103
      fM02 *= fInvLength;
 
104
      float fQ = ((float)2.0)*fM01*fM12+fM02*(fM22-fM11);
 
105
      m_afDiag[1] = fM11+fM02*fQ;
 
106
      m_afDiag[2] = fM22-fM02*fQ;
 
107
      m_afSubd[0] = fLength;
 
108
      m_afSubd[1] = fM12-fM01*fQ;
 
109
      mElement[0][0] = (float)1.0;
 
110
      mElement[0][1] = (float)0.0;
 
111
      mElement[0][2] = (float)0.0;
 
112
      mElement[1][0] = (float)0.0;
 
113
      mElement[1][1] = fM01;
 
114
      mElement[1][2] = fM02;
 
115
      mElement[2][0] = (float)0.0;
 
116
      mElement[2][1] = fM02;
 
117
      mElement[2][2] = -fM01;
 
118
      m_bIsRotation = false;
 
119
    }
 
120
    else
 
121
    {
 
122
      m_afDiag[1] = fM11;
 
123
      m_afDiag[2] = fM22;
 
124
      m_afSubd[0] = fM01;
 
125
      m_afSubd[1] = fM12;
 
126
      mElement[0][0] = (float)1.0;
 
127
      mElement[0][1] = (float)0.0;
 
128
      mElement[0][2] = (float)0.0;
 
129
      mElement[1][0] = (float)0.0;
 
130
      mElement[1][1] = (float)1.0;
 
131
      mElement[1][2] = (float)0.0;
 
132
      mElement[2][0] = (float)0.0;
 
133
      mElement[2][1] = (float)0.0;
 
134
      mElement[2][2] = (float)1.0;
 
135
      m_bIsRotation = true;
 
136
    }
 
137
  }
 
138
 
 
139
  bool QLAlgorithm(void)
 
140
  {
 
141
    const int iMaxIter = 32;
 
142
 
 
143
    for (int i0 = 0; i0 <3; i0++)
 
144
    {
 
145
      int i1;
 
146
      for (i1 = 0; i1 < iMaxIter; i1++)
 
147
      {
 
148
        int i2;
 
149
        for (i2 = i0; i2 <= (3-2); i2++)
 
150
        {
 
151
          float fTmp = fabsf(m_afDiag[i2]) + fabsf(m_afDiag[i2+1]);
 
152
          if ( fabsf(m_afSubd[i2]) + fTmp == fTmp )
 
153
            break;
 
154
        }
 
155
        if (i2 == i0)
 
156
        {
 
157
          break;
 
158
        }
 
159
 
 
160
        float fG = (m_afDiag[i0+1] - m_afDiag[i0])/(((float)2.0) * m_afSubd[i0]);
 
161
        float fR = sqrtf(fG*fG+(float)1.0);
 
162
        if (fG < (float)0.0)
 
163
        {
 
164
          fG = m_afDiag[i2]-m_afDiag[i0]+m_afSubd[i0]/(fG-fR);
 
165
        }
 
166
        else
 
167
        {
 
168
          fG = m_afDiag[i2]-m_afDiag[i0]+m_afSubd[i0]/(fG+fR);
 
169
        }
 
170
        float fSin = (float)1.0, fCos = (float)1.0, fP = (float)0.0;
 
171
        for (int i3 = i2-1; i3 >= i0; i3--)
 
172
        {
 
173
          float fF = fSin*m_afSubd[i3];
 
174
          float fB = fCos*m_afSubd[i3];
 
175
          if (fabsf(fF) >= fabsf(fG))
 
176
          {
 
177
            fCos = fG/fF;
 
178
            fR = sqrtf(fCos*fCos+(float)1.0);
 
179
            m_afSubd[i3+1] = fF*fR;
 
180
            fSin = ((float)1.0)/fR;
 
181
            fCos *= fSin;
 
182
          }
 
183
          else
 
184
          {
 
185
            fSin = fF/fG;
 
186
            fR = sqrtf(fSin*fSin+(float)1.0);
 
187
            m_afSubd[i3+1] = fG*fR;
 
188
            fCos = ((float)1.0)/fR;
 
189
            fSin *= fCos;
 
190
          }
 
191
          fG = m_afDiag[i3+1]-fP;
 
192
          fR = (m_afDiag[i3]-fG)*fSin+((float)2.0)*fB*fCos;
 
193
          fP = fSin*fR;
 
194
          m_afDiag[i3+1] = fG+fP;
 
195
          fG = fCos*fR-fB;
 
196
          for (int i4 = 0; i4 < 3; i4++)
 
197
          {
 
198
            fF = mElement[i4][i3+1];
 
199
            mElement[i4][i3+1] = fSin*mElement[i4][i3]+fCos*fF;
 
200
            mElement[i4][i3] = fCos*mElement[i4][i3]-fSin*fF;
 
201
          }
 
202
        }
 
203
        m_afDiag[i0] -= fP;
 
204
        m_afSubd[i0] = fG;
 
205
        m_afSubd[i2] = (float)0.0;
 
206
      }
 
207
      if (i1 == iMaxIter)
 
208
      {
 
209
        return false;
 
210
      }
 
211
    }
 
212
    return true;
 
213
  }
 
214
 
 
215
  void DecreasingSort(void)
 
216
  {
 
217
    //sort eigenvalues in decreasing order, e[0] >= ... >= e[iSize-1]
 
218
    for (int i0 = 0, i1; i0 <= 3-2; i0++)
 
219
    {
 
220
      // locate maximum eigenvalue
 
221
      i1 = i0;
 
222
      float fMax = m_afDiag[i1];
 
223
      int i2;
 
224
      for (i2 = i0+1; i2 < 3; i2++)
 
225
      {
 
226
        if (m_afDiag[i2] > fMax)
 
227
        {
 
228
          i1 = i2;
 
229
          fMax = m_afDiag[i1];
 
230
        }
 
231
      }
 
232
 
 
233
      if (i1 != i0)
 
234
      {
 
235
        // swap eigenvalues
 
236
        m_afDiag[i1] = m_afDiag[i0];
 
237
        m_afDiag[i0] = fMax;
 
238
        // swap eigenvectors
 
239
        for (i2 = 0; i2 < 3; i2++)
 
240
        {
 
241
          float fTmp = mElement[i2][i0];
 
242
          mElement[i2][i0] = mElement[i2][i1];
 
243
          mElement[i2][i1] = fTmp;
 
244
          m_bIsRotation = !m_bIsRotation;
 
245
        }
 
246
      }
 
247
    }
 
248
  }
 
249
 
 
250
 
 
251
  void GuaranteeRotation(void)
 
252
  {
 
253
    if (!m_bIsRotation)
 
254
    {
 
255
      // change sign on the first column
 
256
      for (int iRow = 0; iRow <3; iRow++)
 
257
      {
 
258
        mElement[iRow][0] = -mElement[iRow][0];
 
259
      }
 
260
    }
 
261
  }
 
262
 
 
263
  float mElement[3][3];
 
264
  float m_afDiag[3];
 
265
  float m_afSubd[3];
 
266
  bool m_bIsRotation;
 
267
};
 
268
 
 
269
}
 
270
 
 
271
 
 
272
using namespace BestFit;
 
273
 
 
274
 
 
275
bool getBestFitPlane(unsigned int vcount,
 
276
                     const float *points,
 
277
                     unsigned int vstride,
 
278
                     const float *weights,
 
279
                     unsigned int wstride,
 
280
                     float *plane)
 
281
{
 
282
  bool ret = false;
 
283
 
 
284
  Vec3 kOrigin(0,0,0);
 
285
 
 
286
  float wtotal = 0;
 
287
 
 
288
  if ( 1 )
 
289
  {
 
290
    const char *source  = (const char *) points;
 
291
    const char *wsource = (const char *) weights;
 
292
 
 
293
    for (unsigned int i=0; i<vcount; i++)
 
294
    {
 
295
 
 
296
      const float *p = (const float *) source;
 
297
 
 
298
      float w = 1;
 
299
 
 
300
      if ( wsource )
 
301
      {
 
302
        const float *ws = (const float *) wsource;
 
303
        w = *ws; //
 
304
        wsource+=wstride;
 
305
      }
 
306
 
 
307
      kOrigin.x+=p[0]*w;
 
308
      kOrigin.y+=p[1]*w;
 
309
      kOrigin.z+=p[2]*w;
 
310
 
 
311
      wtotal+=w;
 
312
 
 
313
      source+=vstride;
 
314
    }
 
315
  }
 
316
 
 
317
  float recip = 1.0f / wtotal; // reciprocol of total weighting
 
318
 
 
319
  kOrigin.x*=recip;
 
320
  kOrigin.y*=recip;
 
321
  kOrigin.z*=recip;
 
322
 
 
323
 
 
324
  float fSumXX=0;
 
325
  float fSumXY=0;
 
326
  float fSumXZ=0;
 
327
 
 
328
  float fSumYY=0;
 
329
  float fSumYZ=0;
 
330
  float fSumZZ=0;
 
331
 
 
332
 
 
333
  if ( 1 )
 
334
  {
 
335
    const char *source  = (const char *) points;
 
336
    const char *wsource = (const char *) weights;
 
337
 
 
338
    for (unsigned int i=0; i<vcount; i++)
 
339
    {
 
340
 
 
341
      const float *p = (const float *) source;
 
342
 
 
343
      float w = 1;
 
344
 
 
345
      if ( wsource )
 
346
      {
 
347
        const float *ws = (const float *) wsource;
 
348
        w = *ws; //
 
349
        wsource+=wstride;
 
350
      }
 
351
 
 
352
      Vec3 kDiff;
 
353
 
 
354
      kDiff.x = w*(p[0] - kOrigin.x); // apply vertex weighting!
 
355
      kDiff.y = w*(p[1] - kOrigin.y);
 
356
      kDiff.z = w*(p[2] - kOrigin.z);
 
357
 
 
358
      fSumXX+= kDiff.x * kDiff.x; // sume of the squares of the differences.
 
359
      fSumXY+= kDiff.x * kDiff.y; // sume of the squares of the differences.
 
360
      fSumXZ+= kDiff.x * kDiff.z; // sume of the squares of the differences.
 
361
 
 
362
      fSumYY+= kDiff.y * kDiff.y;
 
363
      fSumYZ+= kDiff.y * kDiff.z;
 
364
      fSumZZ+= kDiff.z * kDiff.z;
 
365
 
 
366
 
 
367
      source+=vstride;
 
368
    }
 
369
  }
 
370
 
 
371
  fSumXX *= recip;
 
372
  fSumXY *= recip;
 
373
  fSumXZ *= recip;
 
374
  fSumYY *= recip;
 
375
  fSumYZ *= recip;
 
376
  fSumZZ *= recip;
 
377
 
 
378
  // setup the eigensolver
 
379
  Eigen kES;
 
380
 
 
381
  kES.mElement[0][0] = fSumXX;
 
382
  kES.mElement[0][1] = fSumXY;
 
383
  kES.mElement[0][2] = fSumXZ;
 
384
 
 
385
  kES.mElement[1][0] = fSumXY;
 
386
  kES.mElement[1][1] = fSumYY;
 
387
  kES.mElement[1][2] = fSumYZ;
 
388
 
 
389
  kES.mElement[2][0] = fSumXZ;
 
390
  kES.mElement[2][1] = fSumYZ;
 
391
  kES.mElement[2][2] = fSumZZ;
 
392
 
 
393
  // compute eigenstuff, smallest eigenvalue is in last position
 
394
  kES.DecrSortEigenStuff();
 
395
 
 
396
  Vec3 kNormal;
 
397
 
 
398
  kNormal.x = kES.mElement[0][2];
 
399
  kNormal.y = kES.mElement[1][2];
 
400
  kNormal.z = kES.mElement[2][2];
 
401
 
 
402
  // the minimum energy
 
403
  plane[0] = kNormal.x;
 
404
  plane[1] = kNormal.y;
 
405
  plane[2] = kNormal.z;
 
406
 
 
407
  plane[3] = 0 - kNormal.dot(kOrigin);
 
408
 
 
409
  return ret;
 
410
}
 
411
 
 
412
 
 
413
 
 
414
float getBoundingRegion(unsigned int vcount,const float *points,unsigned int pstride,float *bmin,float *bmax) // returns the diagonal distance
 
415
{
 
416
 
 
417
  const unsigned char *source = (const unsigned char *) points;
 
418
 
 
419
        bmin[0] = points[0];
 
420
        bmin[1] = points[1];
 
421
        bmin[2] = points[2];
 
422
 
 
423
        bmax[0] = points[0];
 
424
        bmax[1] = points[1];
 
425
        bmax[2] = points[2];
 
426
 
 
427
 
 
428
  for (unsigned int i=1; i<vcount; i++)
 
429
  {
 
430
        source+=pstride;
 
431
        const float *p = (const float *) source;
 
432
 
 
433
        if ( p[0] < bmin[0] ) bmin[0] = p[0];
 
434
        if ( p[1] < bmin[1] ) bmin[1] = p[1];
 
435
        if ( p[2] < bmin[2] ) bmin[2] = p[2];
 
436
 
 
437
                if ( p[0] > bmax[0] ) bmax[0] = p[0];
 
438
                if ( p[1] > bmax[1] ) bmax[1] = p[1];
 
439
                if ( p[2] > bmax[2] ) bmax[2] = p[2];
 
440
 
 
441
  }
 
442
 
 
443
  float dx = bmax[0] - bmin[0];
 
444
  float dy = bmax[1] - bmin[1];
 
445
  float dz = bmax[2] - bmin[2];
 
446
 
 
447
        return sqrtf( dx*dx + dy*dy + dz*dz );
 
448
 
 
449
}
 
450
 
 
451
 
 
452
bool  overlapAABB(const float *bmin1,const float *bmax1,const float *bmin2,const float *bmax2) // return true if the two AABB's overlap.
 
453
{
 
454
  if ( bmax2[0] < bmin1[0] ) return false; // if the maximum is less than our minimum on any axis
 
455
  if ( bmax2[1] < bmin1[1] ) return false;
 
456
  if ( bmax2[2] < bmin1[2] ) return false;
 
457
 
 
458
  if ( bmin2[0] > bmax1[0] ) return false; // if the minimum is greater than our maximum on any axis
 
459
  if ( bmin2[1] > bmax1[1] ) return false; // if the minimum is greater than our maximum on any axis
 
460
  if ( bmin2[2] > bmax1[2] ) return false; // if the minimum is greater than our maximum on any axis
 
461
 
 
462
 
 
463
  return true; // the extents overlap
 
464
}
 
465
 
 
466