double logRV(size_t dims,gsl_vector_uint *x,gsl_vector_uint *sums,unsigned int NN) {
unsigned int n=0;
double r=0;
size_t i;
for(i=0;i<dims;i++) {
n+=gsl_vector_uint_get(x,i);
}
// x*(log(Na)-log(x))+y*(log(Nb)-log(y))+n*(log(n)-log(NN));
for(i=0;i<dims;i++) {
unsigned int xi=gsl_vector_uint_get(x,i);
if (xi != 0) {
r+=xi*(log(ELT(sums,i))-log(xi));
} else {
r+=SMALL_P*(log(ELT(sums,i))-log(SMALL_P));
}
}
if (n==0)
return r;
else
return r+n*(log(n)-log(NN));
}
void fbst_loginorm(gsl_rng *r,
gsl_vector_uint *x,
gsl_vector_uint *sums,
double alpha,
double beta,
double *_ev,double *_err,
FBSTConfig *config) {
size_t i,j;
double mean;
double var;
double a,b;
assert(x->size == sums->size);
gsl_vector *means=gsl_vector_alloc(x->size);
gsl_vector *sd=gsl_vector_alloc(x->size);
for(i=0;i<x->size;i++) {
a=ELT(x,i)+alpha;
b=ELT(sums,i)+beta-a;
mean=gsl_sf_psi(a)-gsl_sf_psi(b);
var=gsl_sf_psi_1(a)+gsl_sf_psi_1(b);
printf("x%i=N(%lg %lg)\n",i,mean,var);
gsl_vector_set(means,i,mean);
gsl_vector_set(sd,i,var);
}
fbst_normal(r,means,sd,_ev,_err,config);
gsl_vector_free(means);
gsl_vector_free(sd);
}
jd_var *jd_array_insert(jd_array *jda, int idx, size_t count) {
unsigned ix = check_idx_open(jda, idx);
size_t sz = jd_array_count(jda);
size_t need = count * sizeof(jd_var);
jd_string_space(&jda->s, need);
memmove(ELT(jda, ix + count), ELT(jda, ix), (sz - ix) * sizeof(jd_var));
memset(ELT(jda, ix), 0, count * sizeof(jd_var));
jda->s.used += need;
return ELT(jda, ix);
}
void ComputeStiffnessMatrixNullspace::RemoveNullspaceComponent(int n, int nullspaceDimension, const double * nullspaceOrthonormalBasis, double * x)
{
for(int i=0; i<nullspaceDimension; i++)
{
double dotp=0.0;
for(int j=0; j<3*n; j++)
dotp += x[j] * nullspaceOrthonormalBasis[ELT(3*n, j, i)];
for(int j=0; j<3*n; j++)
x[j] -= dotp * nullspaceOrthonormalBasis[ELT(3*n, j, i)];
}
}
void MyFrame::InterpolateMatrix(wxString dataDescription, ModalMatrix * inputModalMatrix)
{
wxFileDialog *dlg = new wxFileDialog(this, _T("Interpolate ") + dataDescription + _T(" to triangle mesh"), uiState.currentWorkingDirectory, _T(""), _T("Modal Matrix Files(*.U)|*.U|All files(*.*)|*.*"), wxFD_SAVE /*| wxHIDE_READONLY*/, wxDefaultPosition);
if ( dlg->ShowModal() == wxID_OK )
{
wxString outputFilename( dlg->GetPath() );
SaveCurrentWorkingDirectory(outputFilename);
if( !outputFilename.empty() )
{
SetCursor(*wxHOURGLASS_CURSOR);
if (!precomputationState.interpolationDataAvailable)
BuildInterpolant();
// interpolate
printf("Interpolating data...\n");
double * inputMatrix = inputModalMatrix->GetMatrix();
int nTarget = (int)(precomputationState.renderingMesh->getNumVertices());
double * outputMatrix = (double*) malloc (sizeof(double) * 3 * nTarget * inputModalMatrix->Getr());
for(int i=0; i<inputModalMatrix->Getr(); i++)
{
precomputationState.simulationMesh->interpolate(
&inputMatrix[ELT(3*precomputationState.simulationMesh->getNumVertices(),0,i)],
&outputMatrix[ELT(3*nTarget,0,i)],
nTarget, precomputationState.simulationMesh->getNumElementVertices(),
precomputationState.interpolationData_vertices,
precomputationState.interpolationData_weights);
}
// save file to disk
const char * filename = outputFilename.mb_str();
printf("Saving output to %s.\n", (char*)filename);
int code = WriteMatrixToDisk((char*)filename,
3 * nTarget,
inputModalMatrix->Getr(),
outputMatrix);
free(outputMatrix);
SetCursor(*wxSTANDARD_CURSOR);
if (code != 0)
{
this->errMsg( _T("Saving error"),
_T("Unable to save interpolated data to ") + outputFilename );
dlg->Destroy();
return;
}
}
}
dlg->Destroy();
}
void ReducedSubspaceStVKForceModel::GetTangentStiffnessMatrixHelper(
double * tangentStiffnessMatrix)
{
// evaluate stiffness matrix
stVKStiffnessMatrix->ComputeStiffnessMatrix(u, sparseMatrix);
// project matrix
for(int i=0; i<r; i++)
sparseMatrix->MultiplyVector(&U[ELT(3*n,0,i)], &bufferMatrix[ELT(3*n,0,i)]);
modalMatrix->ProjectMatrix(r, bufferMatrix, tangentStiffnessMatrix);
}
void ReducedMassSpringSystemForceModel::GetTangentStiffnessMatrixHelper(
double * tangentStiffnessMatrix)
{
// evaluate stiffness matrix
//PerformanceCounter counter;
massSpringSystem->ComputeStiffnessMatrix(u, sparseMatrix);
//counter.StopCounter();
//printf("counter: %G\n", counter.GetElapsedTime());
// project matrix
#if USE_MKL_SPARSE_BLAS
mkl_set_num_threads(8);
//PerformanceCounter counter;
int upperTriangleOnly=1;
int oneIndexed=1;
sparseMatrix->GenerateCompressedRowMajorFormat_four_array(csr_values, csr_columns, csr_pointerB, csr_pointerE, upperTriangleOnly, oneIndexed);
char transa = 'N';
int m = sparseMatrix->GetNumRows();
int n = r;
int k = m;
double alpha = 1.0;
char matdescra[7] = "SUNFXX";
double * val = csr_values;
int * indx = csr_columns;
int * pntrb = csr_pointerB;
int * pntre = csr_pointerE;
double * b = U;
int ldb = m;
double beta = 0.0;
double * c = bufferMatrix;
int ldc = m;
mkl_dcsrmm(&transa, &m, &n, &k, &alpha, matdescra, val, indx, pntrb, pntre,
b, &ldb, &beta, c, &ldc);
//counter.StopCounter();
//printf("counter: %G\n", counter.GetElapsedTime());
#else
for(int i=0; i<r; i++)
sparseMatrix->MultiplyVector(&U[ELT(3*n,0,i)], &bufferMatrix[ELT(3*n,0,i)]);
#endif
modalMatrix->ProjectMatrix(r, bufferMatrix, tangentStiffnessMatrix);
int r2 = r*r;
for(int i=0; i<r2; i++)
tangentStiffnessMatrix[i] *= -1;
}
void Matrix<real>::RemoveColumns(int columnStart, int columnEnd)
{
// write everything to the right of columnEnd to the left
int stride = columnEnd - columnStart;
for(int column=columnEnd; column<n; column++)
{
// write column to column-stride
memcpy(&data[ELT(m, 0, column-stride)], &data[ELT(m, 0, column)], sizeof(double) * m);
}
// free the space
data = (real*) realloc (data, sizeof(real) * m * (n - stride));
n = n - stride;
}
size_t jd_array_remove(jd_array *jda, int idx, size_t count, jd_var *slot) {
unsigned ix = check_idx_open(jda, idx);
size_t avail = jd_array_count(jda) - ix;
if (count > avail) count = avail;
if (slot) {
unsigned i;
for (i = 0; i < count; i++)
jd_assign(slot++, ELT(jda, ix + i));
}
release(jda, ix, count);
memmove(ELT(jda, ix), ELT(jda, ix + count), (avail - count) * sizeof(jd_var));
jda->s.used -= count * sizeof(jd_var);
return count;
}
void Matrix<real>::AppendRowsColumns(const Matrix<real> & bottomLeftBlock, const Matrix<real> & topRightBlock, const Matrix<real> & bottomRightBlock)
{
if (topRightBlock.Getm() != m)
{
printf("Error: mismatch in AppendRowsColumns.\n");
throw 43;
}
if (bottomLeftBlock.Getn() != n)
{
printf("Error: mismatch in AppendRowsColumns.\n");
throw 44;
}
if (bottomRightBlock.Getm() != bottomLeftBlock.Getm())
{
printf("Error: mismatch in AppendRowsColumns.\n");
throw 45;
}
if (bottomRightBlock.Getn() != topRightBlock.Getn())
{
printf("Error: mismatch in AppendRowsColumns.\n");
throw 46;
}
AppendColumns(topRightBlock);
Matrix<real> blockMatrix(bottomLeftBlock.Getm(), n);
real * blockMatrixData = blockMatrix.GetData();
memcpy(blockMatrixData, bottomLeftBlock.GetData(), sizeof(real) * (bottomLeftBlock.Getm() * bottomLeftBlock.Getn()));
memcpy(&blockMatrixData[ELT(bottomLeftBlock.Getm(), 0, bottomLeftBlock.Getn())], bottomRightBlock.GetData(), sizeof(real) * (bottomRightBlock.Getm() * bottomRightBlock.Getn()));
AppendRows(blockMatrix);
}
void SynthesizeVector(int n_inp, int r_inp, real * U_inp, real * q_inp, real * f_inp)
{
#ifdef CBLAS
CBLAS_ORDER order= CblasColMajor;
CBLAS_TRANSPOSE trans= CblasNoTrans;
int m = n_inp;
int n = r_inp;
real alpha = 1;
real * a = U_inp;
int lda = m;
real * x = q_inp;
int incx = 1;
real beta = 0;
real * y = f_inp; // holds the result
int incy = 1;
_cblas_xgemv<sizeof(real)==sizeof(float)>::f(order, trans, m, n, alpha, a, lda, x, incx, beta, y, incy);
#else
int i,j;
for (i=0; i<n_inp; i++) // over all rows, i.e. all coordinates
{
f_inp[i] = 0;
for (j=0; j<r_inp; j++) // over all modes
f_inp[i] += U_inp[ELT(n_inp,i,j)] * q_inp[j];
}
#endif
}
void ProjectVector(int n_inp, int r_inp, real * U_inp, real * q_inp, real * f_inp)
{
// has to make inner product of vector f will all the columns of U
// i.e. multiply U^T * f = q
#ifdef CBLAS
CBLAS_ORDER order= CblasColMajor;
CBLAS_TRANSPOSE trans= CblasTrans;
int m = n_inp;
int n = r_inp;
real alpha = 1;
real * a = U_inp;
int lda = m;
real * x = f_inp;
int incx = 1;
real beta = 0;
real * y = q_inp; // will hold the result
int incy = 1;
_cblas_xgemv<sizeof(real)==sizeof(float)>::f(order, trans, m, n, alpha, a, lda, x, incx, beta, y, incy);
#else
int i,j;
for (j=0; j<r_inp; j++) // over all columns of U
{
// dot product of column j of U with vector f
q_inp[j] = 0;
for (i=0; i<n_inp; i++)
q_inp[j] += U_inp[ELT(n_inp,i,j)] * f_inp[i];
}
#endif
}
void Matrix<real>::SetSubmatrix(int I, int J, const Matrix<real> & submatrix)
{
int subm = submatrix.Getm();
int subn = submatrix.Getn();
real * subdata = submatrix.GetData();
if ((I < 0) || (J < 0) || (I + subm > m) || (J + subn > n))
{
printf("Error: matrix index out of bounds.\n");
throw 21;
}
for(int j=0; j<subn; j++)
for(int i=0; i<subm; i++)
data[ELT(m,I+i,J+j)] = subdata[ELT(subm,i,j)];
}
Matrix<real>::Matrix(int m_, const real * diagonal,
bool freeDataInDestructor_):
m(m_), n(m_), freeDataInDestructor(freeDataInDestructor_)
{
data = (real*) calloc (m*m, sizeof(real));
for(int i=0; i<m; i++)
data[ELT(m,i,i)] = diagonal[i];
}
void ReducedLinearStVKForceModel::GetInternalForce(double * q, double * internalForces)
{
// internalForces = K * q
memset(internalForces, 0, sizeof(double) * r);
for(int i=0; i<r; i++)
for(int j=0; j<r; j++)
internalForces[i] += K[ELT(r,i,j)] * q[j];
}
void SynthesizeSparseVector(int n, int r, real * U, real * q, real * f, int * i_buffer, int k)
{
int i,j;
for (i=0; i<k; i++) // over all non-zero rows
{
f[i] = 0;
for (j=0; j<r; j++) // over all modes
f[i] += U[ELT(n,i_buffer[i],j)] * q[j];
}
}
static jd_var *array_join(jd_var *out, jd_var *sep, jd_array *jda) {
size_t len = 0;
size_t count = jd_array_count(jda);
size_t slen = sep ? jd_length(sep) : 0;
unsigned i;
for (i = 0; i < count; i++) {
if (i) len += slen;
len += jd_length(ELT(jda, i));
}
jd_set_empty_string(out, len);
for (i = 0; i < count; i++) {
if (sep && i) jd_append(out, sep);
jd_append(out, ELT(jda, i));
}
return out;
}
static jd_var *array_stringify(jd_var *out, jd_array *jda) {
size_t count = jd_array_count(jda);
jd_set_array(out, count);
unsigned i;
for (i = 0; i < count; i++) {
jd_stringify(jd_push(out, 1), ELT(jda, i));
}
return out;
}
Matrix<real>::Matrix (int m_, const Matrix & vec, bool freeDataInDestructor_):
m(m_), n(m_), freeDataInDestructor(freeDataInDestructor_)
{
if (m != vec.Getm())
throw 101;
if (vec.Getn() != 1)
throw 102;
data = (real*) calloc (m*m, sizeof(real));
for(int i=0; i<m; i++)
data[ELT(m,i,i)] = vec(i,0);
}
void Matrix<real>::AppendColumns(const Matrix<real> & columns)
{
if (columns.Getm() != m)
{
printf("Error: mismatch in number of rows in AppendColumns.\n");
throw 41;
}
data = (real*) realloc (data, sizeof(real) * (m * (n + columns.Getn())));
memcpy(&data[ELT(m, 0, n)], columns.GetData(), sizeof(real) * m * columns.Getn());
n += columns.Getn();
}
void ProjectSparseVector(int n, int r, real * U, real * q, real * f_buffer, int * i_buffer, int k)
{
// has to make inner product of vector f will all the columns of U
int i,j;
for (j=0; j<r; j++) // over all columns of U
{
// dot product of column j of U with vector f
q[j] = 0;
for (i=0; i<k; i++)
q[j] += U[ELT(n,i_buffer[i],j)] * f_buffer[i];
}
}
void StVKReducedHessianTensor::ContractWithVector(int r, double * Hq, double * q, double * A)
{
// computes A = Hq : q
int quadraticSize = StVKReducedInternalForces::GetQuadraticSize(r);
// multiply Hq and q
cblas_dgemv(CblasColMajor, CblasTrans,
r, quadraticSize,
1.0,
Hq, r,
q, 1,
0.0,
A, 1);
for(int j=r-1; j>=0; j--)
for(int i=r-1; i>=j; i--)
{
int lowerTrianglePos = j * r - (j-1) * j / 2 + (i-j);
A[ELT(r,i,j)] = A[lowerTrianglePos];
A[ELT(r,j,i)] = A[lowerTrianglePos];
}
}
static int left2right(Agraph_t *g, node_t *v, node_t *w)
{
int rv;
#ifdef NOTDEF
adjmatrix_t *M;
M = GD_rank(g)[ND_rank(v)].flat;
if (M == NULL) rv = FALSE;
else {
if (GD_flip(g)) {node_t *t = v; v = w; w = t;}
rv = ELT(M,flatindex(v),flatindex(w));
}
#else
rv = FALSE;
#endif
return rv;
}
int MatrixExp(int m, real * mtx, real t, real * output, int ideg_)
{
INTEGER ideg = ideg_;
INTEGER M = m;
INTEGER ldh = M;
INTEGER lwsp = 4*M*M+ideg+1;
real * wsp = (real*) malloc (sizeof(real) * lwsp);
INTEGER * ipiv = (INTEGER*) malloc (sizeof(INTEGER) * M);
INTEGER iexph = 0;
INTEGER ns = 0;
INTEGER flag = 0;
int mm = m*m;
real maxEntry = 0;
for(int i=0;i<mm; i++)
maxEntry = MAX(maxEntry, fabs(mtx[i]));
if (maxEntry != 0)
{
_xgpadm<sizeof(real)==sizeof(float)>::f(&ideg, &M, &t, mtx,
&ldh, wsp, &lwsp,
ipiv, &iexph, &ns, &flag);
//printf("iexph=%d ns=%d flag=%d\n", (int)iexph, (int)ns, (int)flag);
if (flag != 0)
{
printf("Error: xgpadm returned non-zero exit flag %d.\n", (int)flag);
return flag;
}
memcpy(output, &wsp[iexph-1], sizeof(real) * m * m);
}
else
{
// set result to identity matrix
memset(output, 0, sizeof(real) * m * m);
for(int i=0; i<m; i++)
output[ELT(m,i,i)] = 1.0;
}
free(wsp);
return 0;
}
void MyFrame::OnExportLinearModes(wxCommandEvent& event)
{
wxFileDialog *dlg = new wxFileDialog(this, _T("Export linear modes"),
uiState.currentWorkingDirectory, _T(""), _T("Text Files(*.txt)|*.txt|All files(*.*)|*.*"),
wxFD_SAVE /*| wxHIDE_READONLY*/, wxDefaultPosition);
if ( dlg->ShowModal() == wxID_OK )
{
wxString linearModesFilename( dlg->GetPath().GetData() );
SaveCurrentWorkingDirectory(linearModesFilename);
if( !linearModesFilename.empty() )
{
const char * filename = linearModesFilename.mb_str();
FILE * fout = fopen((char*)filename, "w");
if (fout)
{
double * U = precomputationState.linearModalMatrix->GetMatrix();
int n = precomputationState.linearModalMatrix->Getn();
fprintf(fout,"%d\n%d\n", 3*n, precomputationState.rLin);
for(int i=0; i<3*n; i++)
{
for(int j=0; j<precomputationState.rLin; j++)
{
fprintf(fout, "%.15f ", U[ELT(3*n,i,j)]);
}
fprintf(fout,"\n");
}
fclose(fout);
}
else
{
this->errMsg( _T("Exporting error"),
_T("Unable to export linear modes to ") + linearModesFilename );
dlg->Destroy();
return;
}
}
}
dlg->Destroy();
}
static void
NAME (SCM *const base_ptr, size_t nr_elems, INC_PARAM
SCM less)
{
/* Stack node declarations used to store unfulfilled partition obligations. */
typedef struct {
size_t lo;
size_t hi;
} stack_node;
static const char s_buggy_less[] = "buggy less predicate used when sorting";
#define ELT(i) base_ptr[(i)*INC]
if (nr_elems == 0)
/* Avoid lossage with unsigned arithmetic below. */
return;
if (nr_elems > MAX_THRESH)
{
size_t lo = 0;
size_t hi = nr_elems-1;
stack_node stack[STACK_SIZE];
stack_node *top = stack + 1;
while (STACK_NOT_EMPTY)
{
size_t left;
size_t right;
size_t mid = lo + (hi - lo) / 2;
SCM pivot;
/* Select median value from among LO, MID, and HI. Rearrange
LO and HI so the three values are sorted. This lowers the
probability of picking a pathological pivot value and
skips a comparison for both the left and right. */
SCM_TICK;
if (scm_is_true (scm_call_2 (less, ELT(mid), ELT(lo))))
SWAP (ELT(mid), ELT(lo));
if (scm_is_true (scm_call_2 (less, ELT(hi), ELT(mid))))
SWAP (ELT(mid), ELT(hi));
else
goto jump_over;
if (scm_is_true (scm_call_2 (less, ELT(mid), ELT(lo))))
SWAP (ELT(mid), ELT(lo));
jump_over:;
pivot = ELT(mid);
left = lo + 1;
right = hi - 1;
/* Here's the famous ``collapse the walls'' section of quicksort.
Gotta like those tight inner loops! They are the main reason
that this algorithm runs much faster than others. */
do
{
while (scm_is_true (scm_call_2 (less, ELT(left), pivot)))
{
left += 1;
/* The comparison predicate may be buggy */
if (left > hi)
scm_misc_error (NULL, s_buggy_less, SCM_EOL);
}
while (scm_is_true (scm_call_2 (less, pivot, ELT(right))))
{
right -= 1;
/* The comparison predicate may be buggy */
if (right < lo)
scm_misc_error (NULL, s_buggy_less, SCM_EOL);
}
if (left < right)
{
SWAP (ELT(left), ELT(right));
left += 1;
right -= 1;
}
else if (left == right)
{
left += 1;
right -= 1;
break;
}
}
while (left <= right);
/* Set up pointers for next iteration. First determine whether
left and right partitions are below the threshold size. If so,
ignore one or both. Otherwise, push the larger partition's
bounds on the stack and continue sorting the smaller one. */
if ((size_t) (right - lo) <= MAX_THRESH)
{
if ((size_t) (hi - left) <= MAX_THRESH)
/* Ignore both small partitions. */
POP (lo, hi);
else
/* Ignore small left partition. */
lo = left;
}
else if ((size_t) (hi - left) <= MAX_THRESH)
/* Ignore small right partition. */
hi = right;
else if ((right - lo) > (hi - left))
{
/* Push larger left partition indices. */
PUSH (lo, right);
lo = left;
}
else
{
/* Push larger right partition indices. */
PUSH (left, hi);
hi = right;
}
}
}
/* Once the BASE_PTR array is partially sorted by quicksort the rest is
completely sorted using insertion sort, since this is efficient for
partitions below MAX_THRESH size. BASE_PTR points to the beginning of the
array to sort, and END idexes the very last element in the array (*not*
one beyond it!). */
{
size_t tmp = 0;
size_t end = nr_elems-1;
size_t thresh = min (end, MAX_THRESH);
size_t run;
/* Find smallest element in first threshold and place it at the
array's beginning. This is the smallest array element,
and the operation speeds up insertion sort's inner loop. */
for (run = tmp + 1; run <= thresh; run += 1)
if (scm_is_true (scm_call_2 (less, ELT(run), ELT(tmp))))
tmp = run;
if (tmp != 0)
SWAP (ELT(tmp), ELT(0));
/* Insertion sort, running from left-hand-side up to right-hand-side. */
run = 1;
while (++run <= end)
{
SCM_TICK;
tmp = run - 1;
while (scm_is_true (scm_call_2 (less, ELT(run), ELT(tmp))))
{
/* The comparison predicate may be buggy */
if (tmp == 0)
scm_misc_error (NULL, s_buggy_less, SCM_EOL);
tmp -= 1;
}
tmp += 1;
if (tmp != run)
{
SCM to_insert = ELT(run);
size_t hi, lo;
for (hi = lo = run; --lo >= tmp; hi = lo)
ELT(hi) = ELT(lo);
ELT(hi) = to_insert;
}
}
}
}
int MatrixPCA(ThresholdingSpecification * thresholdingSpecification,
int m, int n, double * A, int * r, double * weights)
{
if (!A)
{
printf("Error: input matrix is NULL.\n");
return -1;
}
if (weights != NULL)
{
// transform row i of A by sqrt(weights[i])
for(int row=0; row<m; row++)
{
double rowWeight = sqrt(weights[row]);
for(int column=0; column<n; column++)
A[ELT(m, row, column)] *= rowWeight;
}
}
bool transpose = false;
if (m > n)
{
transpose = true;
InPlaceTransposeMatrix(m,n,A);
// swap m,n
int bufferi = m;
m = n;
n = bufferi;
}
// do SVD
char jobu = 'O';//overwrites A with U (left singular vectors)
//char jobu = 'N';
char jobvt = 'S';//all rows returned in VT
//char jobvt = 'N';
int ldA = m;
int ldU = m;
int lwork = 64*MAX(3*MIN( m, n)+MAX(m,n), 5*MIN(m,n)-4);
double * work = (double*) malloc (sizeof(double) * lwork);
if (!work)
{
printf("Error: failed to allocate workspace.\n");
return -2;
}
//printf("Workspace size is: %G Mb .\n",1.0 * lwork * sizeof(int) / 1024 / 1024);
// allocate array for singular vectors
double * S = (double *) malloc (sizeof(double) * MIN(m,n));
if (!S)
{
printf("Error: failed to allocate singular vectors.\n");
return -2;
}
double * dummyU = NULL;
// allocate array for VT
int ldVT = MIN(m,n);
double * VT = (double *) malloc (sizeof(double) * ldVT * n);
if (!VT)
{
printf("Error: failed to allocate VT.\n");
return -2;
}
#ifdef __APPLE__
#define DGESVD dgesvd_
#define INTEGER __CLPK_integer
#else
#define DGESVD dgesvd
#define INTEGER int
#endif
INTEGER M = m;
INTEGER N = n;
INTEGER LDA = ldA;
INTEGER LDU = ldU;
INTEGER LDVT = ldVT;
INTEGER LWORK = lwork;
INTEGER INFO;
//printf("Calling LAPACK dgesvd routine...\n");fflush(NULL);
//SUBROUTINE SGESVD( JOBU, JOBVT, M, N, A, LDA, S, U, LDU, VT, LDVT, WORK, LWORK, INFO )
DGESVD (&jobu, &jobvt, &M, &N, A, &LDA,
S, dummyU, &LDU, VT,
&LDVT, work, &LWORK, &INFO);
if (INFO != 0)
{
int code = INFO;
printf("Error: SVD solver returned non-zero exit code: %d.\n", code);
free(VT);
free(S);
free(work);
return code;
}
free(work);
if (transpose)
{
InPlaceTransposeMatrix(m,n,VT);
memcpy(A, VT, sizeof(double) * m * n);
// swap m and n
int bufferii = m;
m = n;
n = bufferii;
}
free(VT);
if (weights != NULL)
{
// transform row i of A by sqrt(weights[i])^{-1}
for(int row=0; row<m; row++)
{
double rowWeight = 1.0 / sqrt(weights[row]);
for(int column=0; column<n; column++)
A[ELT(m, row, column)] *= rowWeight;
}
}
//double totalEnergy = 0;
//printf ("Singular values:\n");fflush(NULL);
//for (int i=0; i< MIN(m,n); i++)
//printf("%f ",S[i]);
//printf ("\n");
// discard unneccesary modes
//printf("Discarding unnecessary components...\n");fflush(NULL);
if (thresholdingSpecification->tresholdingType == ThresholdingSpecification::epsilonBased)
DoTresholding_Epsilon(S,MIN(m,n),r,thresholdingSpecification->epsilon);
else
DoTresholding_NumberOfModes(S,MIN(m,n),r,thresholdingSpecification->rDesired);
// now, variable r has been set to the number of retained modes
free(S);
return 0;
}
static void release(jd_array *jda, unsigned from, size_t count) {
unsigned i;
for (i = from; i < from + count; i++) jd_release(ELT(jda, i));
}
void ComputeStiffnessMatrixNullspace::ComputeNullspace(int n, const double * vertexPos, double * basis, int includeRotationalNullspace, int generateOrthogonalBasis)
{
int basisSize = (includeRotationalNullspace ? 6 : 3);
memset(basis, 0, sizeof(double) * 3 * n * basisSize);
// translational part
for(int i=0; i<n; i++)
for(int j=0; j<3; j++)
basis[ELT(3*n, 3*i+j, j)] = 1.0;
// rotational part
if (includeRotationalNullspace)
{
for(int i=0; i<n; i++)
{
Vec3d p(vertexPos[3*i+0], vertexPos[3*i+1], vertexPos[3*i+2]);
Vec3d e0(1,0,0);
Vec3d e1(0,1,0);
Vec3d e2(0,0,1);
Vec3d r0 = cross(e0, p);
Vec3d r1 = cross(e1, p);
Vec3d r2 = cross(e2, p);
for(int j=0; j<3; j++)
{
basis[ELT(3*n, 3*i+j, 3)] = r0[j];
basis[ELT(3*n, 3*i+j, 4)] = r1[j];
basis[ELT(3*n, 3*i+j, 5)] = r2[j];
}
}
}
if (generateOrthogonalBasis)
{
// normalize translational vectors
for(int v=0; v<3; v++)
{
double norm2 = 0.0;
for(int i=0; i<3*n; i++)
norm2 += basis[ELT(3*n, i, v)] * basis[ELT(3*n, i, v)];
double invNorm = 1.0 / sqrt(norm2);
for(int i=0; i<3*n; i++)
basis[ELT(3*n, i, v)] *= invNorm;
}
// ortho-normalize rotational vectors
if (includeRotationalNullspace)
{
for(int v=0; v<3; v++)
{
for(int j=0; j<3+v; j++)
{
double dotp = 0.0;
for(int i=0; i<3*n; i++)
dotp += basis[ELT(3*n, i, j)] * basis[ELT(3*n, i, 3+v)];
for(int i=0; i<3*n; i++)
basis[ELT(3*n, i, 3+v)] -= dotp * basis[ELT(3*n, i, j)];
double norm2 = 0.0;
for(int i=0; i<3*n; i++)
norm2 += basis[ELT(3*n, i, 3+v)] * basis[ELT(3*n, i, 3+v)];
double invNorm = 1.0 / sqrt(norm2);
for(int i=0; i<3*n; i++)
basis[ELT(3*n, i, 3+v)] *= invNorm;
}
}
}
}
}
jd_var *jd_array_get(jd_array *jda, int idx) {
return ELT(jda, check_idx(jda, idx));
}
