1 | #include <stdio.h>
|
---|
2 | #include "PCASplit.h"
|
---|
3 |
|
---|
4 | #define WANT_STREAM // include.h will get stream fns
|
---|
5 | #define WANT_MATH // include.h will get math fns
|
---|
6 | // newmatap.h will get include.h
|
---|
7 | #include <newmatap.h> // need matrix applications
|
---|
8 | #include <newmatio.h> // need matrix output routines
|
---|
9 | #include <newmatrc.h>
|
---|
10 |
|
---|
11 | #ifdef use_namespace
|
---|
12 | using namespace NEWMAT; // access NEWMAT namespace
|
---|
13 | #endif
|
---|
14 |
|
---|
15 | namespace PCA {
|
---|
16 |
|
---|
17 | PCASplit::PCASplit()
|
---|
18 | : _maxLevel(4), _minDistance(0.5f), _distanceScale(0.2f), _finalMaxLevel(0), _indexCount(0)
|
---|
19 | {
|
---|
20 | _indices = new unsigned int[MAX_INDEX];
|
---|
21 | _memClusterChunk1 = new ClusterListNode[MAX_INDEX];
|
---|
22 | _memClusterChunk2 = new ClusterListNode[MAX_INDEX];
|
---|
23 | _curMemClusterChunk = _memClusterChunk1;
|
---|
24 | _memClusterChunkIndex = 0;
|
---|
25 | }
|
---|
26 |
|
---|
27 | PCASplit::~PCASplit()
|
---|
28 | {
|
---|
29 | delete [] _indices;
|
---|
30 | delete [] _memClusterChunk1;
|
---|
31 | delete [] _memClusterChunk2;
|
---|
32 | }
|
---|
33 |
|
---|
34 | void PCASplit::computeCentroid(Point* data, int count, Vector3& mean/*, float& minValue, float& maxValue*/)
|
---|
35 | {
|
---|
36 | float sumx= 0, sumy = 0, sumz = 0;
|
---|
37 | float size = 0;
|
---|
38 | float sumsize = 0;
|
---|
39 | for (int i = 0; i < count; ++i)
|
---|
40 | {
|
---|
41 | size = data[i].size;
|
---|
42 | sumx += data[i].position.x * size;
|
---|
43 | sumy += data[i].position.y * size;
|
---|
44 | sumz += data[i].position.z * size;
|
---|
45 | sumsize += size;
|
---|
46 | }
|
---|
47 |
|
---|
48 | sumsize = 1.0f / sumsize;
|
---|
49 | mean.set(sumx * sumsize, sumy * sumsize, sumz * sumsize);
|
---|
50 | }
|
---|
51 |
|
---|
52 | void PCASplit::computeCovariant(Point* data, int count, const Vector3& mean, float* m)
|
---|
53 | {
|
---|
54 | memset(m, 0, sizeof(float) * 9);
|
---|
55 |
|
---|
56 | for (int i = 0; i < count; ++i)
|
---|
57 | {
|
---|
58 | m[0] += (data[i].position.x - mean.x) * (data[i].position.x - mean.x);
|
---|
59 | m[1] += (data[i].position.x - mean.x) * (data[i].position.y - mean.y);
|
---|
60 | m[2] += (data[i].position.x - mean.x) * (data[i].position.z - mean.z);
|
---|
61 | m[4] += (data[i].position.y - mean.y) * (data[i].position.y - mean.y);
|
---|
62 | m[5] += (data[i].position.y - mean.y) * (data[i].position.z - mean.z);
|
---|
63 | m[8] += (data[i].position.z - mean.z) * (data[i].position.z - mean.z);
|
---|
64 | }
|
---|
65 |
|
---|
66 | float invCount = 1.0f / (count - 1);
|
---|
67 | m[0] *= invCount;
|
---|
68 | m[1] *= invCount;
|
---|
69 | m[2] *= invCount;
|
---|
70 | m[4] *= invCount;
|
---|
71 | m[5] *= invCount;
|
---|
72 | m[8] *= invCount;
|
---|
73 | m[3] = m[1];
|
---|
74 | m[6] = m[2];
|
---|
75 | m[7] = m[5];
|
---|
76 | }
|
---|
77 |
|
---|
78 | void PCASplit::computeDistortion(Point* data, int count, const Vector3& mean, float& distortion, float& finalSize)
|
---|
79 | {
|
---|
80 | distortion = 0.0f;
|
---|
81 | finalSize = 0.0f;
|
---|
82 |
|
---|
83 | float maxSize = 0.0f;
|
---|
84 | float distance;
|
---|
85 | for (int i = 0; i < count; ++i)
|
---|
86 | {
|
---|
87 | // sum
|
---|
88 | distance = mean.distanceSquare(data[i].position.x, data[i].position.y, data[i].position.z);
|
---|
89 | distortion += distance;
|
---|
90 |
|
---|
91 | if (data[i].size > maxSize)
|
---|
92 | {
|
---|
93 | maxSize = data[i].size;
|
---|
94 | }
|
---|
95 |
|
---|
96 | /*
|
---|
97 | finalSize += data[i].size * sqrt(distance);
|
---|
98 | */
|
---|
99 | if (distance > finalSize) finalSize = distance;;
|
---|
100 | }
|
---|
101 |
|
---|
102 | // equation 2
|
---|
103 | //finalSize = 0.5f * sqrt (finalSize) / (float) (count - 1) + maxSize;
|
---|
104 | finalSize = sqrt (finalSize) + maxSize;
|
---|
105 |
|
---|
106 | }
|
---|
107 |
|
---|
108 | void PCASplit::init()
|
---|
109 | {
|
---|
110 | _curClusterNode = 0;
|
---|
111 | _curClusterCount = 0;
|
---|
112 | }
|
---|
113 |
|
---|
114 | Cluster* PCASplit::createClusterBlock(ClusterListNode* clusterList, int count, int level)
|
---|
115 | {
|
---|
116 | static int cc = 0;
|
---|
117 | cc += count;
|
---|
118 | Cluster* clusterBlock = new Cluster[count];
|
---|
119 |
|
---|
120 | _clusterHeader->numOfClusters[level - 1] = count;
|
---|
121 | _clusterHeader->startPointerCluster[level - 1] = clusterBlock;
|
---|
122 |
|
---|
123 | printf("Cluster created %d [in level %d]:total %d\n", count, level, cc);
|
---|
124 | fflush(stdout);
|
---|
125 |
|
---|
126 | int i = 0;
|
---|
127 | ClusterListNode* clusterNode = clusterList;
|
---|
128 | while (clusterNode)
|
---|
129 | {
|
---|
130 | clusterBlock[i].centroid = clusterList->data->centroid_t;
|
---|
131 | clusterBlock[i].level = level;
|
---|
132 | clusterBlock[i].scale = clusterList->data->scale_t;
|
---|
133 |
|
---|
134 | clusterNode = clusterNode->next;
|
---|
135 | ++i;
|
---|
136 | }
|
---|
137 | if (count != i)
|
---|
138 | {
|
---|
139 | printf("ERROR\n");
|
---|
140 | }
|
---|
141 | return clusterBlock;
|
---|
142 | }
|
---|
143 |
|
---|
144 | ClusterAccel* PCASplit::doIt(Point* data, int count)
|
---|
145 | {
|
---|
146 | init();
|
---|
147 |
|
---|
148 | _clusterHeader = new ClusterAccel(_maxLevel);
|
---|
149 |
|
---|
150 | Cluster* root = new Cluster;
|
---|
151 | Cluster_t* cluster_t = new Cluster_t();
|
---|
152 | cluster_t->points_t = data;
|
---|
153 | cluster_t->numOfPoints_t = count;
|
---|
154 | root->level = 1;
|
---|
155 |
|
---|
156 | _clusterHeader->root = root;
|
---|
157 | _clusterHeader->numOfClusters[0] = 1;
|
---|
158 | _clusterHeader->startPointerCluster[0] = root;
|
---|
159 |
|
---|
160 | Vector3 mean;
|
---|
161 | float distortion, scale;
|
---|
162 |
|
---|
163 | computeCentroid(cluster_t->points_t, cluster_t->numOfPoints_t, mean);
|
---|
164 | computeDistortion(cluster_t->points_t, cluster_t->numOfPoints_t, mean, distortion, scale);
|
---|
165 |
|
---|
166 | cluster_t->centroid_t = mean;
|
---|
167 | cluster_t->scale_t = scale;
|
---|
168 |
|
---|
169 | float mindistance = _minDistance;
|
---|
170 | int level = 2;
|
---|
171 |
|
---|
172 | ClusterListNode* clustNodeList = &(_memClusterChunk2[0]);
|
---|
173 | clustNodeList->data = cluster_t;
|
---|
174 | clustNodeList->next = 0;
|
---|
175 |
|
---|
176 | _curRoot = root;
|
---|
177 | do {
|
---|
178 | analyze(clustNodeList, _curRoot, level, mindistance);
|
---|
179 |
|
---|
180 | mindistance *= _distanceScale;
|
---|
181 | ++level;
|
---|
182 |
|
---|
183 | // swap memory buffer & init
|
---|
184 | _curMemClusterChunk = (_curMemClusterChunk == _memClusterChunk1)?
|
---|
185 | _memClusterChunk2 : _memClusterChunk1;
|
---|
186 | _memClusterChunkIndex = 0;
|
---|
187 |
|
---|
188 | clustNodeList = _curClusterNode;
|
---|
189 |
|
---|
190 | } while (level <= _maxLevel);
|
---|
191 |
|
---|
192 | return _clusterHeader;
|
---|
193 | }
|
---|
194 |
|
---|
195 | void PCASplit::addLeafCluster(Cluster_t* cluster)
|
---|
196 | {
|
---|
197 | ClusterListNode* clusterNode = &_curMemClusterChunk[_memClusterChunkIndex++];
|
---|
198 | clusterNode->data = cluster;
|
---|
199 | clusterNode->next = _curClusterNode;
|
---|
200 | _curClusterNode = clusterNode;
|
---|
201 | ++_curClusterCount;
|
---|
202 | }
|
---|
203 |
|
---|
204 | void PCASplit::split(Point* data, int count, float limit)
|
---|
205 | {
|
---|
206 | Vector3 mean;
|
---|
207 | float m[9];
|
---|
208 |
|
---|
209 | computeCentroid(data, count, mean);
|
---|
210 | float scale, distortion;
|
---|
211 | computeDistortion(data, count, mean, distortion, scale);
|
---|
212 |
|
---|
213 | //if (distortion < limit)
|
---|
214 | if (scale < limit)
|
---|
215 | {
|
---|
216 | Cluster_t* cluster_t = new Cluster_t();
|
---|
217 | cluster_t->centroid_t = mean;
|
---|
218 | cluster_t->points_t = data;
|
---|
219 | cluster_t->numOfPoints_t = count;
|
---|
220 | cluster_t->scale_t = scale;
|
---|
221 | addLeafCluster(cluster_t);
|
---|
222 | return;
|
---|
223 | }
|
---|
224 |
|
---|
225 | computeCovariant(data, count, mean, m);
|
---|
226 |
|
---|
227 | SymmetricMatrix A(3);
|
---|
228 | for (int i = 1, index = 0; i <= 3; ++i)
|
---|
229 | for (int j = 1; j <= i; ++j)
|
---|
230 | {
|
---|
231 | A(i, j) = m[(i - 1) * 3 + j - 1];
|
---|
232 | }
|
---|
233 |
|
---|
234 |
|
---|
235 | Matrix U; DiagonalMatrix D;
|
---|
236 | eigenvalues(A,D,U);
|
---|
237 | Vector3 emax(U(1, 3), U(2, 3), U(3, 3));
|
---|
238 |
|
---|
239 | int index1Count = 0, index2Count = 0;
|
---|
240 |
|
---|
241 | int left = 0, right = count - 1;
|
---|
242 |
|
---|
243 | Point p;
|
---|
244 | for (;left < right;)
|
---|
245 | {
|
---|
246 | while (left < count && emax.dot(data[left].position - mean) >= 0.0f) ++left;
|
---|
247 | while (right >= 0 && emax.dot(data[right].position - mean) < 0.0f) --right;
|
---|
248 |
|
---|
249 | if (left > right) break;
|
---|
250 |
|
---|
251 | p = data[left];
|
---|
252 | data[left] = data[right];
|
---|
253 | data[right] = p;
|
---|
254 | ++left, --right;
|
---|
255 |
|
---|
256 | }
|
---|
257 |
|
---|
258 | if (left == 0 || right == count - 1)
|
---|
259 | {
|
---|
260 | printf("error\n");
|
---|
261 | exit(1);
|
---|
262 | }
|
---|
263 | else
|
---|
264 | {
|
---|
265 | split(data, left, limit);
|
---|
266 | split(data + left, count - left, limit);
|
---|
267 | }
|
---|
268 | }
|
---|
269 |
|
---|
270 | void PCASplit::analyze(ClusterListNode* clusterNode, Cluster* parent, int level, float limit)
|
---|
271 | {
|
---|
272 | if (level > _maxLevel) return;
|
---|
273 | else if (level > _finalMaxLevel) _finalMaxLevel = level;
|
---|
274 |
|
---|
275 | init();
|
---|
276 |
|
---|
277 | ClusterListNode* clNode = clusterNode;
|
---|
278 |
|
---|
279 | // initialize the indexCount of indices
|
---|
280 | _indexCount = 0;
|
---|
281 |
|
---|
282 | int i = 0;
|
---|
283 | while (clNode)
|
---|
284 | {
|
---|
285 | if (clNode->data)
|
---|
286 | {
|
---|
287 | split(clNode->data->points_t, clNode->data->numOfPoints_t, limit);
|
---|
288 | _indices[_indexCount++] = _curClusterCount;
|
---|
289 | }
|
---|
290 |
|
---|
291 | clNode = clNode->next;
|
---|
292 | }
|
---|
293 |
|
---|
294 | //Vector3 mean;
|
---|
295 | //computeCentroid(cluster->points, cluster->numOfPoints, mean);
|
---|
296 |
|
---|
297 | // the process values of split are in _curClusterNode and _curClusterCount
|
---|
298 | ClusterListNode* curClusterNode = _curClusterNode;
|
---|
299 | int curClusterNodeCount = _curClusterCount;
|
---|
300 |
|
---|
301 | if (curClusterNodeCount)
|
---|
302 | {
|
---|
303 |
|
---|
304 | // create and init centroid
|
---|
305 | Cluster* retClusterBlock = createClusterBlock(curClusterNode, curClusterNodeCount, level);
|
---|
306 |
|
---|
307 | _curRoot = retClusterBlock;
|
---|
308 |
|
---|
309 | if (level == _maxLevel)
|
---|
310 | {
|
---|
311 | ClusterListNode* node = curClusterNode;
|
---|
312 | if (_indexCount > 0)
|
---|
313 | {
|
---|
314 | // for parent
|
---|
315 | Point* points = new Point[curClusterNodeCount];
|
---|
316 |
|
---|
317 | parent[0].setChildren(retClusterBlock, _indices[0]);
|
---|
318 | parent[0].setPoints(points, _indices[0]);
|
---|
319 |
|
---|
320 | for (int i = 0, in = 0; i < curClusterNodeCount; ++i)
|
---|
321 | {
|
---|
322 | if (i >= _indices[in])
|
---|
323 | {
|
---|
324 | in++;
|
---|
325 | parent[in].setChildren(retClusterBlock + _indices[in - 1], _indices[in] - _indices[in - 1]);
|
---|
326 | parent[in].setPoints(points + _indices[in - 1], _indices[in] - _indices[in - 1]);
|
---|
327 | }
|
---|
328 |
|
---|
329 | retClusterBlock[i].scale = node->data->scale_t;
|
---|
330 | retClusterBlock[i].centroid = node->data->centroid_t;
|
---|
331 | retClusterBlock[i].points = node->data->points_t;
|
---|
332 | retClusterBlock[i].numOfPoints = node->data->numOfPoints_t;
|
---|
333 | retClusterBlock[i].color.set(
|
---|
334 | float(rand()) / RAND_MAX, float(rand()) / RAND_MAX, float(rand()) / RAND_MAX,
|
---|
335 | 0.2);
|
---|
336 |
|
---|
337 | printf("CLUSTER points : %d\n", node->data->numOfPoints_t);
|
---|
338 | fflush(stdout);
|
---|
339 |
|
---|
340 | points[i].position = node->data->centroid_t;
|
---|
341 | points[i].color.set(1.0f, 1.0f, 1.0f, 0.2f);
|
---|
342 | points[i].size = node->data->scale_t;
|
---|
343 |
|
---|
344 | node = node->next;
|
---|
345 |
|
---|
346 | //::glGenBuffers(1, &(cluster[i].vbo));
|
---|
347 | //glBindBuffer(GL_ARRAY_BUFFER, cluster[i].vbo);
|
---|
348 | // glBufferData(GL_ARRAY_BUFFER,
|
---|
349 | // sizeof(Point) * cluster[i].numOfPoints,
|
---|
350 | // cluster[i].points,
|
---|
351 | // GL_STATIC_DRAW);
|
---|
352 | //glBindBuffer(GL_ARRAY_BUFFER, 0);
|
---|
353 |
|
---|
354 | }
|
---|
355 |
|
---|
356 | /*
|
---|
357 | ::glGenBuffers(1, &(_clusterHeader->_vbo[level - 1]));
|
---|
358 | ::glBindBuffer(GL_ARRAY_BUFFER, _clusterHeader->_vbo[level - 1]);
|
---|
359 | ::glBufferData(GL_ARRAY_BUFFER,
|
---|
360 | sizeof(Point) * curClusterNodeCount,
|
---|
361 | points,
|
---|
362 | GL_STATIC_DRAW);
|
---|
363 | ::glBindBuffer(GL_ARRAY_BUFFER, 0);
|
---|
364 | */
|
---|
365 | }
|
---|
366 |
|
---|
367 |
|
---|
368 |
|
---|
369 | }
|
---|
370 | else
|
---|
371 | {
|
---|
372 | Point* points = new Point[curClusterNodeCount];
|
---|
373 | ClusterListNode* node = curClusterNode;
|
---|
374 |
|
---|
375 | if (_indexCount > 0)
|
---|
376 | {
|
---|
377 | parent[0].setPoints(points, _indices[0]);
|
---|
378 | parent[0].setChildren(retClusterBlock, _indices[0]);
|
---|
379 | for (int k = 1; k < _indexCount; ++k)
|
---|
380 | {
|
---|
381 | parent[k].setPoints(points + _indices[k - 1], _indices[k] - _indices[k - 1]);
|
---|
382 | parent[k].setChildren(retClusterBlock + _indices[k - 1], _indices[k] - _indices[k - 1]);
|
---|
383 | }
|
---|
384 |
|
---|
385 | // set points of sub-clusters
|
---|
386 | for (int i = 0; i < curClusterNodeCount; ++i)
|
---|
387 | {
|
---|
388 | points[i].position = node->data->centroid_t;
|
---|
389 | points[i].color.set(1.0f, 1.0f, 1.0f, 0.2f);
|
---|
390 | points[i].size = node->data->scale_t;
|
---|
391 | node = node->next;
|
---|
392 | }
|
---|
393 |
|
---|
394 | printf("points %d\n", curClusterNodeCount);
|
---|
395 | }
|
---|
396 | }
|
---|
397 | }
|
---|
398 | }
|
---|
399 |
|
---|
400 | }
|
---|
401 |
|
---|