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

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

#includes needed for Embarcadero

  • Property svn:eol-style set to native
File size: 5.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 <alloc.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.