175 lines
5.0 KiB
C
175 lines
5.0 KiB
C
#include "cve.h"
|
|
|
|
// mat* Matrix(const unsigned int nrow, const unsigned int ncol) {
|
|
// mat* newMat = (mat*)R_alloc(1, sizeof(mat));
|
|
// newMat->nrow = nrow;
|
|
// newMat->ncol = ncol;
|
|
// newMat->memsize = nrow * ncol;
|
|
// newMat->data = (double*)R_alloc(nrow * ncol, sizeof(double));
|
|
|
|
// return newMat;
|
|
// }
|
|
|
|
double norm(const double *A, const int nrow, const int ncol,
|
|
const char *type) {
|
|
int i, nelem = nrow * ncol;
|
|
int nelemb = (nelem / 4) * 4;
|
|
double sum = 0.0;
|
|
|
|
if (*type == 'F') {
|
|
for (i = 0; i < nelemb; i += 4) {
|
|
sum += A[i] * A[i]
|
|
+ A[i + 1] * A[i + 1]
|
|
+ A[i + 2] * A[i + 2]
|
|
+ A[i + 3] * A[i + 3];
|
|
}
|
|
for (; i < nelem; ++i) {
|
|
sum += A[i] * A[i];
|
|
}
|
|
return sqrt(sum);
|
|
} else {
|
|
error("Unknown norm type.");
|
|
}
|
|
}
|
|
|
|
void matrixprod(const double *A, const int nrowA, const int ncolA,
|
|
const double *B, const int nrowB, const int ncolB,
|
|
double *C) {
|
|
const double one = 1.0;
|
|
const double zero = 0.0;
|
|
|
|
// DGEMM with parameterization:
|
|
// C <- A %*% B
|
|
F77_NAME(dgemm)("N", "N", &nrowA, &ncolB, &ncolA,
|
|
&one, A, &nrowA, B, &nrowB,
|
|
&zero, C, &nrowA);
|
|
}
|
|
|
|
void crossprod(const double *A, const int nrowA, const int ncolA,
|
|
const double *B, const int nrowB, const int ncolB,
|
|
double *C) {
|
|
const double one = 1.0;
|
|
const double zero = 0.0;
|
|
|
|
// DGEMM with parameterization:
|
|
// C <- t(A) %*% B
|
|
F77_NAME(dgemm)("T", "N", &ncolA, &ncolB, &nrowA,
|
|
&one, A, &nrowA, B, &nrowB,
|
|
&zero, C, &ncolA);
|
|
}
|
|
|
|
void nullProj(const double *B, const int nrowB, const int ncolB,
|
|
double *Q) {
|
|
const double minusOne = -1.0;
|
|
const double one = 1.0;
|
|
int i, nelem = nrowB * nrowB;
|
|
|
|
// Initialize as identity matrix.
|
|
memset(Q, 0, sizeof(double) * nrowB * nrowB);
|
|
for (i = 0; i < nelem; i += nrowB + 1) {
|
|
Q[i] = 1.0;
|
|
}
|
|
|
|
// DGEMM with parameterization:
|
|
// C <- (-1.0 * B %*% t(B)) + C
|
|
F77_NAME(dgemm)("N", "T", &nrowB, &nrowB, &ncolB,
|
|
&minusOne, B, &nrowB, B, &nrowB,
|
|
&one, Q, &nrowB);
|
|
}
|
|
|
|
// In place scaling of elements of A.
|
|
void scale(const double s, double *A, const int nelem) {
|
|
int i, nelemb = (nelem / 4) * 4;
|
|
|
|
if (s == 1.) {
|
|
return;
|
|
}
|
|
|
|
for (i = 0; i < nelemb; i += 4) {
|
|
A[i] *= s;
|
|
A[i + 1] *= s;
|
|
A[i + 2] *= s;
|
|
A[i + 3] *= s;
|
|
}
|
|
for (; i < nelem; ++i) {
|
|
A[i] *= s;
|
|
}
|
|
}
|
|
|
|
// A dence skew-symmetric rank 2 update.
|
|
// Perform the update
|
|
// C := alpha (A * B^T - B * A^T) + beta C
|
|
void skew(const int nrow, const int ncol,
|
|
double alpha, const double *A, const double *B,
|
|
double beta,
|
|
double *C) {
|
|
F77_NAME(dgemm)("N", "T",
|
|
&nrow, &nrow, &ncol,
|
|
&alpha, A, &nrow, B, &nrow,
|
|
&beta, C, &nrow);
|
|
|
|
alpha *= -1.0;
|
|
beta = 1.0;
|
|
F77_NAME(dgemm)("N", "T",
|
|
&nrow, &nrow, &ncol,
|
|
&alpha, B, &nrow, A, &nrow,
|
|
&beta, C, &nrow);
|
|
}
|
|
|
|
|
|
/** Cayley transformation of matrix `B` using the Skew-Symmetri matrix `A`.
|
|
* X = (I + A)^-1 (I - A) B
|
|
* by solving the following linear equation:
|
|
* (I + A) X = (I - A) B ==> X = (I + A)^-1 (I - A) B
|
|
* \_____/ \_____/
|
|
* IpA X = ImA B
|
|
* \_______/
|
|
* IpA X = Y ==> X = IpA^-1 Y
|
|
*
|
|
* @param A Skew-Symmetric matrix of dimension `(n, n)`.
|
|
* @param B Matrix of dimensions `(n, m)` with `m <= n`.
|
|
* @return Transformed matrix `X`.
|
|
* @note This opperation is equivalent to the R expression:
|
|
* solve(diag(1, n) + A) %*% (diag(1, n) - A) %*% B
|
|
* or
|
|
* solve(diag(1, n) + A, (diag(1, n) - A) %*% B)
|
|
*/
|
|
void cayleyTransform(const int p, const int q,
|
|
const double *A, const double *B,
|
|
double *X, double *workMem) {
|
|
int i, info, pp = p * p;
|
|
double zero = 0., one = 1.;
|
|
|
|
/* Allocate row permutation array used by `dgesv` */
|
|
int *ipiv = (int*)workMem;
|
|
|
|
/* Create Matrix IpA = I + A (I plus A) */
|
|
double *IpA = (double*)(ipiv + p);
|
|
memcpy(IpA, A, pp * sizeof(double));
|
|
for (i = 0; i < pp; i += p + 1) {
|
|
IpA[i] += 1.; // +1 to diagonal elements.
|
|
}
|
|
|
|
/* Create Matrix ImA = I - A (I minus A) */
|
|
double *ImA = IpA + pp;
|
|
for (i = 0; i < pp; ++i) {
|
|
ImA[i] = -A[i];
|
|
}
|
|
for (i = 0; i < pp; i += p + 1) {
|
|
ImA[i] += 1.; // +1 to diagonal elements.
|
|
}
|
|
|
|
/* Y as matrix-matrix product of ImA and B:
|
|
* Y = 1 * ImA * B + 0 * Y */
|
|
F77_CALL(dgemm)("N", "N", &p, &q, &p,
|
|
&one, ImA, &p, B, &p, &zero, X, &p);
|
|
|
|
/* Solve system IpA Y = X for Y (and store result in X).
|
|
* aka. X = IpA^-1 X */
|
|
F77_CALL(dgesv)(&p, &q, IpA, &p, ipiv, X, &p, &info);
|
|
|
|
if (info) {
|
|
error("[ cayleyTransform ] error in dgesv - info %d", info);
|
|
}
|
|
}
|