1 #include "float_math.h"
2
3 #include <stdio.h>
4 #include <stdlib.h>
5 #include <string.h>
6 #include <math.h>
7 #include <assert.h>
8
9 /*----------------------------------------------------------------------
10 Copyright (c) 2004 Open Dynamics Framework Group
11 www.physicstools.org
12 All rights reserved.
13
14 Redistribution and use in source and binary forms, with or without modification, are permitted provided
15 that the following conditions are met:
16
17 Redistributions of source code must retain the above copyright notice, this list of conditions
18 and the following disclaimer.
19
20 Redistributions in binary form must reproduce the above copyright notice,
21 this list of conditions and the following disclaimer in the documentation
22 and/or other materials provided with the distribution.
23
24 Neither the name of the Open Dynamics Framework Group nor the names of its contributors may
25 be used to endorse or promote products derived from this software without specific prior written permission.
26
27 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES,
28 INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
29 DISCLAIMED. IN NO EVENT SHALL THE INTEL OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
30 EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
32 IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
33 THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
34 -----------------------------------------------------------------------*/
35
36 // http://codesuppository.blogspot.com
37 //
38 // mailto: jratcliff@infiniplex.net
39 //
40 // http://www.amillionpixels.us
41 //
42
43 #include "bestfitobb.h"
44 #include "float_math.h"
45
46 // computes the OBB for this set of points relative to this transform matrix.
computeOBB(unsigned int vcount,const float * points,unsigned int pstride,float * sides,const float * matrix)47 void computeOBB(unsigned int vcount, const float *points, unsigned int pstride, float *sides, const float *matrix)
48 {
49 const char *src = (const char *)points;
50
51 float bmin[3] = {1e9, 1e9, 1e9};
52 float bmax[3] = {-1e9, -1e9, -1e9};
53
54 for (unsigned int i = 0; i < vcount; i++)
55 {
56 const float *p = (const float *)src;
57 float t[3];
58
59 fm_inverseRT(matrix, p, t); // inverse rotate translate
60
61 if (t[0] < bmin[0]) bmin[0] = t[0];
62 if (t[1] < bmin[1]) bmin[1] = t[1];
63 if (t[2] < bmin[2]) bmin[2] = t[2];
64
65 if (t[0] > bmax[0]) bmax[0] = t[0];
66 if (t[1] > bmax[1]) bmax[1] = t[1];
67 if (t[2] > bmax[2]) bmax[2] = t[2];
68
69 src += pstride;
70 }
71
72 sides[0] = bmax[0];
73 sides[1] = bmax[1];
74 sides[2] = bmax[2];
75
76 if (fabsf(bmin[0]) > sides[0]) sides[0] = fabsf(bmin[0]);
77 if (fabsf(bmin[1]) > sides[1]) sides[1] = fabsf(bmin[1]);
78 if (fabsf(bmin[2]) > sides[2]) sides[2] = fabsf(bmin[2]);
79
80 sides[0] *= 2.0f;
81 sides[1] *= 2.0f;
82 sides[2] *= 2.0f;
83 }
84
computeBestFitOBB(unsigned int vcount,const float * points,unsigned int pstride,float * sides,float * matrix)85 void computeBestFitOBB(unsigned int vcount, const float *points, unsigned int pstride, float *sides, float *matrix)
86 {
87 float bmin[3];
88 float bmax[3];
89
90 fm_getAABB(vcount, points, pstride, bmin, bmax);
91
92 float center[3];
93
94 center[0] = (bmax[0] - bmin[0]) * 0.5f + bmin[0];
95 center[1] = (bmax[1] - bmin[1]) * 0.5f + bmin[1];
96 center[2] = (bmax[2] - bmin[2]) * 0.5f + bmin[2];
97
98 float ax = 0;
99 float ay = 0;
100 float az = 0;
101
102 float sweep = 45.0f; // 180 degree sweep on all three axes.
103 float steps = 8.0f; // 16 steps on each axis.
104
105 float bestVolume = 1e9;
106 float angle[3] = {0.f, 0.f, 0.f};
107
108 while (sweep >= 1)
109 {
110 bool found = false;
111
112 float stepsize = sweep / steps;
113
114 for (float x = ax - sweep; x <= ax + sweep; x += stepsize)
115 {
116 for (float y = ay - sweep; y <= ay + sweep; y += stepsize)
117 {
118 for (float z = az - sweep; z <= az + sweep; z += stepsize)
119 {
120 float pmatrix[16];
121
122 fm_eulerMatrix(x * FM_DEG_TO_RAD, y * FM_DEG_TO_RAD, z * FM_DEG_TO_RAD, pmatrix);
123
124 pmatrix[3 * 4 + 0] = center[0];
125 pmatrix[3 * 4 + 1] = center[1];
126 pmatrix[3 * 4 + 2] = center[2];
127
128 float psides[3];
129
130 computeOBB(vcount, points, pstride, psides, pmatrix);
131
132 float volume = psides[0] * psides[1] * psides[2]; // the volume of the cube
133
134 if (volume <= bestVolume)
135 {
136 bestVolume = volume;
137
138 sides[0] = psides[0];
139 sides[1] = psides[1];
140 sides[2] = psides[2];
141
142 angle[0] = ax;
143 angle[1] = ay;
144 angle[2] = az;
145
146 memcpy(matrix, pmatrix, sizeof(float) * 16);
147 found = true; // yes, we found an improvement.
148 }
149 }
150 }
151 }
152
153 if (found)
154 {
155 ax = angle[0];
156 ay = angle[1];
157 az = angle[2];
158
159 sweep *= 0.5f; // sweep 1/2 the distance as the last time.
160 }
161 else
162 {
163 break; // no improvement, so just
164 }
165 }
166 }
167