source: cpp/frams/model/similarity/SVD/matrix_tools.cpp @ 389

Last change on this file since 389 was 389, checked in by Maciej Komosinski, 9 years ago

Code formatting

  • Property svn:eol-style set to native
File size: 4.8 KB
Line 
1// This file is a part of Framsticks SDK.  http://www.framsticks.com/
2// Copyright (C) 1999-2015  Maciej Komosinski and Szymon Ulatowski.
3// See LICENSE.txt for details.
4
5
6#include "matrix_tools.h"
7#include "lapack.h"
8#include <cstdlib>
9#include <cmath>
10#include <cstdio>
11#include <stdlib.h> //malloc(), embarcadero
12#include <math.h> //sqrt(), embarcadero
13
14
15double *Create(int nSize)
16{
17        double *matrix = (double *)malloc(nSize * sizeof(double));
18
19        for (int i = 0; i < nSize; i++)
20        {
21                matrix[i] = 0;
22        }
23
24        return matrix;
25}
26
27double *Multiply(double *&a, double *&b, int nrow, int ncol, double ncol2, double *&toDel, int delSize)
28{
29        double *c = Create(nrow * ncol2);
30        int i = 0, j = 0, k = 0;
31
32        for (i = 0; i < nrow; i++)
33        {
34                for (j = 0; j < ncol2; j++)
35                {
36                        for (k = 0; k < ncol; k++)
37                                c[i * nrow + j] += a[i * nrow + k] * b[k * ncol + j];
38                }
39        }
40
41        if (delSize != 0)
42                free(toDel);
43        return c;
44}
45
46double *Power(double *&array, int nrow, int ncol, double pow, double *&toDel, int delSize)
47{
48        double *m_Power = Create(nrow * ncol);
49        if (pow == 2)
50        {
51                for (int i = 0; i < nrow; i++)
52                {
53                        for (int j = 0; j < ncol; j++)
54                        {
55                                m_Power[i * nrow + j] = array[i * nrow + j] * array[i * nrow + j];
56                        }
57
58                }
59        }
60        else
61        {
62                for (int i = 0; i < nrow; i++)
63                {
64                        for (int j = 0; j < ncol; j++)
65                        {
66                                m_Power[i * nrow + j] = sqrt(array[i * nrow + j]);
67                        }
68
69                }
70        }
71
72        if (delSize != 0)
73                free(toDel);
74
75        return m_Power;
76}
77
78void Print(double *&mat, int nelems)
79{
80        for (int i = 0; i < nelems; i++)
81                printf("%6.2f ", mat[i]);
82        printf("\n");
83
84}
85
86double *Transpose(double *&A, int arow, int acol)
87{
88        double *result = Create(acol * arow);
89
90        for (int i = 0; i < acol; i++)
91                for (int j = 0; j < arow; j++)
92                {
93                result[i * arow + j] = A[j * acol + i];
94                }
95
96        return result;
97
98}
99
100/** Computes the SVD of the nSize x nSize distance matrix
101                @param vdEigenvalues [OUT] Vector of doubles. On return holds the eigenvalues of the
102                decomposed distance matrix (or rather, to be strict, of the matrix of scalar products
103                created from the matrix of distances). The vector is assumed to be empty before the function call and
104                all variance percentages are pushed at the end of it.
105                @param nSize size of the matrix of distances.
106                @param pDistances [IN] matrix of distances between parts.
107                @param Coordinates [OUT] array of three dimensional coordinates obtained from SVD of pDistances matrix.
108                */
109void MatrixTools::SVD(std::vector<double> &vdEigenvalues, int nSize, double *pDistances, Pt3D *&Coordinates)
110{
111        // compute squares of elements of this array
112        // compute the matrix B that is the parameter of SVD
113        double *B = Create(nSize * nSize);
114        {
115                // use additional scope to delete temporary matrices
116                double *Ones, *Eye, *Z, *D;
117
118                D = Create(nSize * nSize);
119                D = Power(pDistances, nSize, nSize, 2.0, D, nSize);
120
121                Ones = Create(nSize * nSize);
122                for (int i = 0; i < nSize; i++)
123                        for (int j = 0; j < nSize; j++)
124                        {
125                        Ones[i * nSize + j] = 1;
126                        }
127
128                Eye = Create(nSize * nSize);
129                for (int i = 0; i < nSize; i++)
130                {
131                        for (int j = 0; j < nSize; j++)
132                        {
133                                if (i == j)
134                                {
135                                        Eye[i * nSize + j] = 1;
136                                }
137                                else
138                                {
139                                        Eye[i * nSize + j] = 0;
140                                }
141                        }
142                }
143
144                Z = Create(nSize * nSize);
145                for (int i = 0; i < nSize; i++)
146                {
147                        for (int j = 0; j < nSize; j++)
148                        {
149                                Z[i * nSize + j] = 1.0 / ((double)nSize) * Ones[i * nSize + j];
150                        }
151                }
152
153                for (int i = 0; i < nSize; i++)
154                {
155                        for (int j = 0; j < nSize; j++)
156                        {
157                                Z[i * nSize + j] = Eye[i * nSize + j] - Z[i * nSize + j];
158                        }
159                }
160
161                for (int i = 0; i < nSize; i++)
162                {
163                        for (int j = 0; j < nSize; j++)
164                        {
165                                B[i * nSize + j] = Z[i * nSize + j] * -0.5;
166                        }
167                }
168
169                B = Multiply(B, D, nSize, nSize, nSize, B, nSize);
170                B = Multiply(B, Z, nSize, nSize, nSize, B, nSize);
171
172                free(Ones);
173                free(Eye);
174                free(Z);
175                free(D);
176        }
177
178        double *Eigenvalues = Create(nSize);
179        double *S = Create(nSize * nSize);
180
181        // call SVD function
182        double *Vt = Create(nSize * nSize);
183        size_t astep = nSize * sizeof(double);
184        Lapack::JacobiSVD(B, astep, Eigenvalues, Vt, astep, nSize, nSize, nSize);
185
186        double *W = Transpose(Vt, nSize, nSize);
187
188        free(B);
189        free(Vt);
190
191        for (int i = 0; i < nSize; i++)
192                for (int j = 0; j < nSize; j++)
193                {
194                if (i == j)
195                        S[i * nSize + j] = Eigenvalues[i];
196                else
197                        S[i * nSize + j] = 0;
198                }
199
200        // compute coordinates of points
201        double *sqS, *dCoordinates;
202        sqS = Power(S, nSize, nSize, 0.5, S, nSize);
203        dCoordinates = Multiply(W, sqS, nSize, nSize, nSize, W, nSize);
204        free(sqS);
205
206        for (int i = 0; i < nSize; i++)
207        {
208                // set coordinate from the SVD solution
209                Coordinates[i].x = dCoordinates[i * nSize];
210                Coordinates[i].y = dCoordinates[i * nSize + 1];
211                if (nSize > 2)
212                        Coordinates[i].z = dCoordinates[i * nSize + 2];
213                else
214                        Coordinates[i].z = 0;
215        }
216
217        // store the eigenvalues in the output vector
218        for (int i = 0; i < nSize; i++)
219        {
220                double dElement = Eigenvalues[i];
221                vdEigenvalues.push_back(dElement);
222        }
223
224        free(Eigenvalues);
225        free(dCoordinates);
226}
Note: See TracBrowser for help on using the repository browser.