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

Last change on this file since 357 was 357, checked in by Maciej Komosinski, 6 years ago

Set svn:eol-style to native

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