/*
QL algorithm with implicit shifts, to determine the eigenvalues and
eigenvectors of a real, symmetric, tridiagonal matrix, or of a real,
symmetric matrix previously reduced by tred2 x 11.2. On input,
d[1..n] contains the diagonal elements of the tridiagonal matrix.
On output, it returns the eigenvalues. The vector e[1..n] inputs the
subdiagonal elements of the tridiagonal matrix, with e[1] arbitrary.
On output e is destroyed. When finding only the eigenvalues, several
lines may be omitted, as noted in the comments. If the eigenvectors
of a tridiagonal matrix are desired, the matrix z[1..n][1..n] is
input as the identity matrix. If the eigenvectors of a matrix that
has been reduced by tred2 are required, then z is input as the matrix
output by tred2.
In either case, the kth column of z returns the normalized
eigenvector corresponding to d[k].
*/
void tqli(gdouble d[], gdouble e[], gint n, gdouble **z)
{
gint m,l,iter,i,k;
gdouble s,r,p,g,f,dd,c,b;
/* Convenient to renumber the elements of e. */
for (i=1;i<n;i++) e[i-1]=e[i];
e[n-1]=0.0;
for (l=0;l<n;l++) {
iter=0;
do {
/* Look for a single small subdiagonal element to split
the matrix. */
for (m=l;m<n-1;m++) {
dd=fabs(d[m])+fabs(d[m+1]);
if ((gdouble)(fabs(e[m])+dd) == dd) break;
}
if (m != l) {
if (iter++ == 30) nrerror("Too many iterations in tqli");
g=(d[l+1]-d[l])/(2.0*e[l]); /* Form shift. */
r=pythag(g,1.0);
g=d[m]-d[l]+e[l]/(g+SIGN(r,g)); /* This is d_m - k_s */
s=c=1.0;
p=0.0;
/* A plane rotation as in the original QL, followed by Givens
rotations to restore tridiagonal form. */
for (i=m-1;i>=l;i--) {
f=s*e[i];
b=c*e[i];
e[i+1]=(r=pythag(f,g));
if (r == 0.0) { /* Recover from underflow. */
d[i+1] -= p;
e[m]=0.0;
break;
}
s=f/r;
c=g/r;
g=d[i+1]-p;
r=(d[i]-g)*s+2.0*c*b;
p=s*r;
d[i+1]=g+p;
g=c*r-b;
/* Next loop can be omitted if eigenvectors not wanted*/
for (k=0;k<n;k++) { /* Form eigenvectors. */
f=z[k][i+1];
z[k][i+1]=s*z[k][i]+c*f;
z[k][i]=c*z[k][i]-s*f;
}
}
if (r == 0.0 && i >= l) continue;
d[l] -= p;
e[l]=g;
e[m]=0.0;
}
} while (m != l);
}
}
int main() {
int long answer12 = pythag(12);
int long answer = pythag(1000);
int long answer30 = pythag(30);
int long answer_mathy = pyth_mathy(1000);
printf("pythag trip (12) = %ld\n",answer12);
printf("pythage trip (30) = %ld\n",answer30);
printf("pythag trip (1000) = %ld or %ld\n",answer,answer_mathy);
}
// Compute the eigen values and vectors of a symmetric tridiagonal matrix
//
// QL algorithm with implicit shifts, to determine the eigenvalues and eigenvectors
// of a real, symmetric, tridiagonal matrix, or of a real, symmetric matrix
// previously reduced by householder sec. 11.2. On input, d[0..n-1] contains the diagonal
// elements of the tridiagonal matrix. On output, it returns the eigenvalues. The
// vector e[0..n-1] inputs the subdiagonal elements of the tridiagonal matrix, with
// e[0] arbitrary. On output e is destroyed. When finding only the eigenvalues,
// several lines may be omitted, as noted in the comments. If the eigenvectors of
// a tridiagonal matrix are desired, the matrix z[0..n-1][0..n-1] is input as the
// identity matrix. If the eigenvectors of a matrix that has been reduced by householder
// are required, then z is input as the matrix output by householder. In either case,
// the kth column of z returns the normalized eigenvector corresponding to d[k].
//
// input: d - diagonal of symmetric tridiagonal matrix
// e - offdiagonal of symmetric tridiagonal matrix
// z - identity if you want eigensystem of symmetric tridiagonal matrix
// - OR the householder reduction of a symmetric matrix
// output: d - eigenvalues
// z - the corresponding eigen vectors in the COLUMNS!!!
static void eigen(double *d, double *e, int n, double **z)
{
double pythag(double a, double b);
int m, l, iter, i, k;
double s, r, p, g, f, dd, c, b;
// Convenient to renumber the elements of e.
for (i=1; i<n; i++) e[i-1]=e[i];
e[n-1]=0.0;
for (l=0; l<n; l++) {
iter=0;
do {
// Look for a single small subdiagonal element to split the matrix.
for (m=l; m<n-1; m++) {
dd=fabs(d[m])+fabs(d[m+1]);
if ((double)(fabs(e[m])+dd) == dd) break;
}
if (m != l) {
if (iter++ == 30) printf("Too many iterations in tqli");
g=(d[l+1]-d[l])/(2.0*e[l]); // Form shift.
r=pythag(g, 1.0);
g=d[m]-d[l]+e[l]/(g+SIGN(r, g)); // This is dm - ks.
s=c=1.0;
p=0.0;
for (i=m-1; i>=l; i--) { // A plane rotation as in the original QL, followed by Givens
f=s*e[i]; // rotations to restore tridiagonal form.
b=c*e[i];
e[i+1]=(r=pythag(f, g));
if (r == 0.0) { // Recover from underflow.
d[i+1] -= p;
e[m]=0.0;
break;
}
s=f/r;
c=g/r;
g=d[i+1]-p;
r=(d[i]-g)*s+2.0*c*b;
d[i+1]=g+(p=s*r);
g=c*r-b;
// Next loop can be omitted if eigenvectors not wanted
// Form eigenvectors.
for (k=0; k<n; k++) {
f=z[k][i+1];
z[k][i+1]=s*z[k][i]+c*f;
z[k][i]=c*z[k][i]-s*f;
}
}
if (r == 0.0 && i >= l) continue;
d[l] -= p;
e[l]=g;
e[m]=0.0;
}
} while (m != l);
}
}
void tqli(double d[], double e[], int n, double **z)
{
double pythag(double a, double b);
int m,l,iter,i,k,IT=0;
double s,r,p,g,f,dd,c,b;
for (i=2;i<=n;i++) e[i-1]=e[i];
e[n]=0.0;
for (l=1;l<=n;l++) {
iter=0;
do {
for (m=l;m<=n-1;m++) {
dd=fabs(d[m])+fabs(d[m+1]);
if ((double)(fabs(e[m])+dd) == dd) break;
}
if (m != l) {
if (iter++ == 30) nrerror("Too many iterations in tqli");
g=(d[l+1]-d[l])/(2.0*e[l]);
r=pythag(g,1.0);
g=d[m]-d[l]+e[l]/(g+SIGN(r,g));
s=c=1.0;
p=0.0;
for (i=m-1;i>=l;i--) {
f=s*e[i];
b=c*e[i];
e[i+1]=(r=pythag(f,g));
if (r == 0.0) {
d[i+1] -= p;
e[m]=0.0;
break;
}
s=f/r;
c=g/r;
g=d[i+1]-p;
r=(d[i]-g)*s+2.0*c*b;
d[i+1]=g+(p=s*r);
g=c*r-b;
for (k=1;k<=n;k++) {
f=z[k][i+1];
z[k][i+1]=s*z[k][i]+c*f;
z[k][i]=c*z[k][i]-s*f;
}
}
if (r == 0.0 && i >= l) continue;
d[l] -= p;
e[l]=g;
e[m]=0.0;
}
} while (m != l);
IT+=iter;
}
printf("\nnumber of iterations %d\n",IT);
}
void tqli(double *d, double *e, int n, double **z)
{
register int m,l,iter,i,k;
double s,r,p,g,f,dd,c,b;
for(i = 1; i < n; i++) e[i-1] = e[i];
e[n] = 0.0;
for(l = 0; l < n; l++) {
iter = 0;
do {
for(m = l; m < n-1; m++) {
dd = fabs(d[m]) + fabs(d[m+1]);
if((double)(fabs(e[m])+dd) == dd) break;
}
if(m != l) {
if(iter++ == 30) {
printf("\n\nToo many iterations in tqli.\n");
exit(1);
}
g = (d[l+1] - d[l])/(2.0 * e[l]);
r = pythag(g,1.0);
g = d[m]-d[l]+e[l]/(g+SIGN(r,g));
s = c = 1.0;
p = 0.0;
for(i = m-1; i >= l; i--) {
f = s * e[i];
b = c*e[i];
e[i+1] = (r=pythag(f,g));
if(r == 0.0) {
d[i+1] -= p;
e[m] = 0.0;
break;
}
s = f/r;
c = g/r;
g = d[i+1] - p;
r = (d[i] - g) * s + 2.0 * c * b;
d[i+1] = g + (p = s * r);
g = c * r - b;
for(k = 0; k < n; k++) {
f = z[k][i+1];
z[k][i+1] = s * z[k][i] + c * f;
z[k][i] = c * z[k][i] - s * f;
} /* end k-loop */
} /* end i-loop */
if(r == 0.0 && i >= l) continue;
d[l] -= p;
e[l] = g;
e[m] = 0.0;
} /* end if-loop for m != 1 */
} while(m != l);
} /* end l-loop */
} /* End: function tqli(), (C) Copr. 1986-92 Numerical Recipes Software )%. */
int tqli(eusfloat_t d[], eusfloat_t e[], int n, eusfloat_t **z)
{
eusfloat_t pythag(eusfloat_t a, eusfloat_t b);
int m,l,iter,i,k;
eusfloat_t s,r,p,g,f,dd,c,b;
for (i=2;i<=n;i++) e[i-1]=e[i]; // Convenient to renumber the elements of e.
e[n]=0.0;
for (l=1;l<=n;l++) {
iter=0;
do {
for (m=l;m<=n-1;m++) { // Look for a single small subdiagonal element to split the matrix.
dd=fabs(d[m])+fabs(d[m+1]);
if ((eusfloat_t)(fabs(e[m])+dd) == dd) break;
}
if (m != l) {
if (iter++ == 30) {nrerror("Too many iterations in tqli"); return -1;}
g=(d[l+1]-d[l])/(2.0*e[l]); // Form shift.
r=pythag(g,1.0);
g=d[m]-d[l]+e[l]/(g+SIGN(r,g)); // This is dm . ks.
s=c=1.0;
p=0.0;
for (i=m-1;i>=l;i--) { // A plane rotation as in the original QL, followed by Givens rotations to restore tridiagonal form.
f=s*e[i];
b=c*e[i];
e[i+1]=(r=pythag(f,g));
if (r == 0.0) { // Recover from underflow.
d[i+1] -= p;
e[m]=0.0;
break;
}
s=f/r;
c=g/r;
g=d[i+1]-p;
r=(d[i]-g)*s+2.0*c*b;
d[i+1]=g+(p=s*r);
g=c*r-b;
/* Next loop can be omitted if eigenvectors not wanted*/
for (k=1;k<=n;k++) { // Form eigenvectors.
f=z[k][i+1];
z[k][i+1]=s*z[k][i]+c*f;
z[k][i]=c*z[k][i]-s*f;
}
}
if (r == 0.0 && i >= l) continue;
d[l] -= p;
e[l]=g;
e[m]=0.0;
}
} while (m != l);
}
return 1;
}
void tqli(double *d, double *e, int n, double **z)
{
int m,l,iter,i,k;
double s,r,p,g,f,dd,c,b;
for (i=1;i<n;i++) e[i-1]=e[i];
e[n-1]=0.0;
for (l=0;l<n;l++) {
iter=0;
do {
for (m=l;m<n-1;m++) {
dd=fabs(d[m])+fabs(d[m+1]);
if ((double)(fabs(e[m])+dd) == dd) break;
}
if (m != l) {
if (iter++ == 30) {
printf("ERROR: Too many iterations in tqli\n");
Finalize(1);
}
g=(d[l+1]-d[l])/(2.0*e[l]);
r=pythag(g,1.0);
g=d[m]-d[l]+e[l]/(g+SIGN(r,g));
s=c=1.0;
p=0.0;
for (i=m-1;i>=l;i--) {
f=s*e[i];
b=c*e[i];
e[i+1]=(r=pythag(f,g));
if (r == 0.0) {
d[i+1] -= p;
e[m]=0.0;
break;
}
s=f/r;
c=g/r;
g=d[i+1]-p;
r=(d[i]-g)*s+2.0*c*b;
d[i+1]=g+(p=s*r);
g=c*r-b;
for (k=0;k<n;k++) {
f=z[k][i+1];
z[k][i+1]=s*z[k][i]+c*f;
z[k][i]=c*z[k][i]-s*f;
}
}
if (r == 0.0 && i >= l) continue;
d[l] -= p;
e[l]=g;
e[m]=0.0;
}
} while (m != l);
}
}
void C_toolbox_eigen_sym::tqli()
{
int m,l,iter,i,k;
double s,r,p,g,f,dd,c,b;
const double EPS=DBL_EPSILON;
for (i=1;i<n;i++) e[i-1]=e[i];
e[n-1]=0.0;
for (l=0;l<n;l++) {
iter=0;
do {
for (m=l;m<n-1;m++) {
dd=abs(d[m])+abs(d[m+1]);
if (abs(e[m]) <= EPS*dd) break;
}
if (m != l) {
if (iter++ == 90) throw("Too many iterations in tqli"); //used to be 30
g=(d[l+1]-d[l])/(2.0*e[l]);
r=pythag(g,1.0);
g=d[m]-d[l]+e[l]/(g+SIGN(r,g));
s=c=1.0;
p=0.0;
for (i=m-1;i>=l;i--) {
f=s*e[i];
b=c*e[i];
e[i+1]=(r=pythag(f,g));
if (r == 0.0) {
d[i+1] -= p;
e[m]=0.0;
break;
}
s=f/r;
c=g/r;
g=d[i+1]-p;
r=(d[i]-g)*s+2.0*c*b;
d[i+1]=g+(p=s*r);
g=c*r-b;
if (yesvecs) {
for (k=0;k<n;k++) {
f=z[k][i+1];
z[k][i+1]=s*z[k][i]+c*f;
z[k][i]=c*z[k][i]-s*f;
}
}
}
if (r == 0.0 && i >= l) continue;
d[l] -= p;
e[l]=g;
e[m]=0.0;
}
} while (m != l);
}
}
void tqli(float d[], float e[], int n, float **z)
{
float pythag(float a, float b);
int m,l,iter,i,k;
float s,r,p,g,f,dd,c,b;
for (;i<=n;i++) e[i-1]=e[i];
e[n]=0.0;
for (;l<=n;l++) {
iter=0;
do {
for (;m<=n-1;m++) {
dd=fabs(d[m])+fabs(d[m+1]);
if ((float)(fabs(e[m])+dd) == dd) break;
}
if (m != l) {
if (iter++ == 30) nrerror("Too many iterations in tqli");
g=(d[l+1]-d[l])/(2.0*e[l]);
r=pythag(g,1.0);
g=d[m]-d[l]+e[l]/(g+SIGN(r,g));
s=c=1.0;
p=0.0;
for (;i>=l;i--) {
f=s*e[i];
b=c*e[i];
e[i+1]=(r=pythag(f,g));
if (r == 0.0) {
d[i+1] -= p;
e[m]=0.0;
break;
}
s=f/r;
c=g/r;
g=d[i+1]-p;
r=(d[i]-g)*s+2.0*c*b;
d[i+1]=g+(p=s*r);
g=c*r-b;
for (;k<=n;k++) {
f=z[k][i+1];
z[k][i+1]=s*z[k][i]+c*f;
z[k][i]=c*z[k][i]-s*f;
}
}
if (r == 0.0 && i >= l) continue;
d[l] -= p;
e[l]=g;
e[m]=0.0;
}
} while (m != l);
}
}
std::vector<Cell*> Cell::getCellsTouching() {
std::vector<Cell*> returncells;
for (int i=0; i<container->size(); i++) {
Cell* cellp = &(*container)[i];
if (pythag(cellp->x-x,cellp->y-y,cellp->z-z) < cellp->size/2 + size/2) {
returncells.push_back(cellp);
}
}
return returncells;
}
// produces the Cholesky decomposition of EAE where A = chol.t() * chol
// and E produces a LEFT circular shift of the rows and columns from
// 1,...,k-1,k,k+1,...l,l+1,...,p to
// 1,...,k-1,k+1,...l,k,l+1,...,p to
void left_circular_update_Cholesky(UpperTriangularMatrix &chol, int k, int l)
{
int nRC = chol.Nrows();
int i, j;
// I. compute shift of column k to the lth position
Matrix cholCopy = chol;
// a. grab column k
ColumnVector columnK = cholCopy.Column(k);
// b. shift columns k+1,...l to the LEFT
for(j = k+1; j <= l; ++j)
cholCopy.Column(j-1) = cholCopy.Column(j);
// c. copy the elements of columnK into the lth column of cholCopy
cholCopy.Column(l) = 0.0;
for(i = 1; i <= k; ++i)
cholCopy(i,l) = columnK(i);
// II. apply and compute Given's rotations
int nGivens = l-k;
ColumnVector cGivens(nGivens); cGivens = 0.0;
ColumnVector sGivens(nGivens); sGivens = 0.0;
for(j = k; j <= nRC; ++j)
{
ColumnVector columnJ = cholCopy.Column(j);
// apply the previous Givens rotations to columnJ
int imax = j - k; if (imax > nGivens) imax = nGivens;
for(int i = 1; i <= imax; ++i)
{
int gIndex = i;
int topRowIndex = k + i - 1;
GivensRotationR(cGivens(gIndex), sGivens(gIndex),
columnJ(topRowIndex), columnJ(topRowIndex+1));
}
// compute a new Given's rotation when j < l
if(j < l)
{
int gIndex = j-k+1;
columnJ(j) = pythag(columnJ(j), columnJ(j+1), cGivens(gIndex),
sGivens(gIndex));
columnJ(j+1) = 0.0;
}
cholCopy.Column(j) = columnJ;
}
chol << cholCopy;
}
// produces the Cholesky decomposition of EAE where A = chol.t() * chol
// and E produces a RIGHT circular shift of the rows and columns from
// 1,...,k-1,k,k+1,...l,l+1,...,p to
// 1,...,k-1,l,k,k+1,...l-1,l+1,...p
void right_circular_update_Cholesky(UpperTriangularMatrix &chol, int k, int l)
{
int nRC = chol.Nrows();
int i, j;
// I. compute shift of column l to the kth position
Matrix cholCopy = chol;
// a. grab column l
ColumnVector columnL = cholCopy.Column(l);
// b. shift columns k,...l-1 to the RIGHT
for(j = l-1; j >= k; --j)
cholCopy.Column(j+1) = cholCopy.Column(j);
// c. copy the top k-1 elements of columnL into the kth column of cholCopy
cholCopy.Column(k) = 0.0;
for(i = 1; i < k; ++i) cholCopy(i,k) = columnL(i);
// II. determine the l-k Given's rotations
int nGivens = l-k;
ColumnVector cGivens(nGivens); cGivens = 0.0;
ColumnVector sGivens(nGivens); sGivens = 0.0;
for(i = l; i > k; i--)
{
int givensIndex = l-i+1;
columnL(i-1) = pythag(columnL(i-1), columnL(i),
cGivens(givensIndex), sGivens(givensIndex));
columnL(i) = 0.0;
}
// the kth entry of columnL is the new diagonal element in column k of cholCopy
cholCopy(k,k) = columnL(k);
// III. apply these Given's rotations to subsequent columns
// for columns k+1,...,l-1 we only need to apply the last nGivens-(j-k) rotations
for(j = k+1; j <= nRC; ++j)
{
ColumnVector columnJ = cholCopy.Column(j);
int imin = nGivens - (j-k) + 1; if (imin < 1) imin = 1;
for(int gIndex = imin; gIndex <= nGivens; ++gIndex)
{
// apply gIndex Given's rotation
int topRowIndex = k + nGivens - gIndex;
GivensRotationR(cGivens(gIndex), sGivens(gIndex),
columnJ(topRowIndex), columnJ(topRowIndex+1));
}
cholCopy.Column(j) = columnJ;
}
chol << cholCopy;
}
T HouseholderTransform(VectorTemplate<T>& v)
{
Assert(v.n != 0);
if (v.n == 1) return 0;
T alpha, beta, tau ;
VectorTemplate<T> x; x.setRef(v,1);
T xnorm = x.norm();
if (xnorm == 0) {
return 0;
}
alpha = v(0);
beta = - (alpha >= 0.0 ? 1 : -1) * pythag(alpha, xnorm);
tau = (beta - alpha) / beta ;
x.inplaceDiv(alpha-beta);
v(0)=beta;
return tau;
}
void TransformCosSin_Sin(Real a,Real b,Real& c,Real& d)
{
//use sin(x+d) = sin(x)cos(d) + cos(x)sin(d)
//=> a=c*sin(d), b=c*cos(d)
//=> c^2 = a^2+b^2
if(a==0 && b==0) { c=d=0; }
else {
d = Atan2(a,b);
c = pythag(a,b);
}
Real x=0.5;
if(!FuzzyEquals(c*Sin(x+d),a*Cos(x)+b*Sin(x))) {
printf("Error in TransformCosSin\n");
printf("a: %f, b: %f\n",a,b);
printf("c: %f, d: %f\n",c,d);
printf("f(x): %f\n",a*Cos(x)+b*Sin(x));
printf("g(x): %f\n",c*Sin(x+d));
}
Assert(FuzzyEquals(c*Sin(x+d),a*Cos(x)+b*Sin(x)));
}
int updatePid(Pid* pid, float x, float y, int dt = 20) {
pid->valLast[0] = pid->val[0];
pid->valLast[1] = pid->val[1];
pid->val[0] = x;
pid->val[1] = y;
pid->err = pythag(
(pid->targ[0] - pid->val[0]),
(pid->targ[1] - pid->val[1])
);
pid->prop = pid->err * pid->kP;
pid->integ += pid->err * pid->kI;
pid->integ =
(fabs(pid->integ) > pid->integLim)
? pid->integ
: pid->integLim * sgn(pid->integ);
pid->deriv = (pid->val - pid->valLast) * pid->kD * 20 / dt;
pid->out = (int) round(pid->prop + pid->integ + pid->deriv);
return pid->out;
}
// produces the Cholesky decomposition of A - x.t() * x where A = chol.t() * chol
void downdate_Cholesky(UpperTriangularMatrix &chol, RowVector x)
{
int nRC = chol.Nrows();
// solve R^T a = x
LowerTriangularMatrix L = chol.t();
ColumnVector a(nRC); a = 0.0;
int i, j;
for (i = 1; i <= nRC; ++i)
{
// accumulate subtr sum
Real subtrsum = 0.0;
for(int k = 1; k < i; ++k) subtrsum += a(k) * L(i,k);
a(i) = (x(i) - subtrsum) / L(i,i);
}
// test that l2 norm of a is < 1
Real squareNormA = a.SumSquare();
if (squareNormA >= 1.0)
Throw(ProgramException("downdate_Cholesky() fails", chol));
Real alpha = sqrt(1.0 - squareNormA);
// compute and apply Givens rotations to the vector a
ColumnVector cGivens(nRC); cGivens = 0.0;
ColumnVector sGivens(nRC); sGivens = 0.0;
for(i = nRC; i >= 1; i--)
alpha = pythag(alpha, a(i), cGivens(i), sGivens(i));
// apply Givens rotations to the jth column of chol
ColumnVector xtilde(nRC); xtilde = 0.0;
for(j = nRC; j >= 1; j--)
{
// only the first j rotations have an affect on chol,0
for(int k = j; k >= 1; k--)
GivensRotation(cGivens(k), -sGivens(k), chol(k,j), xtilde(j));
}
}
void edgelength(double *nodeXlist,double *nodeYlist,int *n, double *edgelength, int *longlat)
{
int N=*n, i;
double el[1],gel[1];
el[0]=(double)0;
if (longlat[0]==0)
{
for(i=0; i<N-1; i++)
{
el[0]=el[0]+pythag((nodeXlist[i+1]-nodeXlist[i]),(nodeYlist[i+1]-nodeYlist[i]));
}
}
else
{
for(i=0; i<N-1; i++)
{
gc_el(nodeXlist+i+1,nodeXlist+i,nodeYlist+i+1,nodeYlist+i+1, gel);
el[0]=el[0]+gel[0];
}
}
edgelength[0]=el[0];
}
// produces the Cholesky decomposition of A + x.t() * x where A = chol.t() * chol
void update_Cholesky(UpperTriangularMatrix &chol, RowVector x)
{
int nc = chol.Nrows();
ColumnVector cGivens(nc); cGivens = 0.0;
ColumnVector sGivens(nc); sGivens = 0.0;
for(int j = 1; j <= nc; ++j) // process the jth column of chol
{
// apply the previous Givens rotations k = 1,...,j-1 to column j
for(int k = 1; k < j; ++k)
GivensRotation(cGivens(k), sGivens(k), chol(k,j), x(j));
// determine the jth Given's rotation
pythag(chol(j,j), x(j), cGivens(j), sGivens(j));
// apply the jth Given's rotation
{
Real tmp0 = cGivens(j) * chol(j,j) + sGivens(j) * x(j);
chol(j,j) = tmp0; x(j) = 0.0;
}
}
}
LOCAL VOID tql1 P4C(int, n,
double *, d,
double *, e,
int *, ierr)
{
/* System generated locals */
double d__1, d__2;
/* Local variables */
double c, f, g, h;
int i, j, l, m;
double p, r, s, c2, c3 = 0.0;
int l1, l2;
double s2 = 0.0;
int ii;
double dl1, el1;
int mml;
double tst1, tst2;
/* this subroutine is a translation of the algol procedure tql1, */
/* num. math. 11, 293-306(1968) by bowdler, martin, reinsch, and */
/* wilkinson. */
/* handbook for auto. comp., vol.ii-linear algebra, 227-240(1971). */
/* this subroutine finds the eigenvalues of a symmetric */
/* tridiagonal matrix by the ql method. */
/* on input */
/* n is the order of the matrix. */
/* d contains the diagonal elements of the input matrix. */
/* e contains the subdiagonal elements of the input matrix */
/* in its last n-1 positions. e(1) is arbitrary. */
/* on output */
/* d contains the eigenvalues in ascending order. if an */
/* error exit is made, the eigenvalues are correct and */
/* ordered for indices 1,2,...ierr-1, but may not be */
/* the smallest eigenvalues. */
/* e has been destroyed. */
/* ierr is set to */
/* zero for normal return, */
/* j if the j-th eigenvalue has not been */
/* determined after 30 iterations. */
/* calls pythag for dsqrt(a*a + b*b) . */
/* questions and comments should be directed to burton s. garbow, */
/* mathematics and computer science div, argonne national laboratory */
/* this version dated august 1983. */
/* ------------------------------------------------------------------ */
/* Parameter adjustments */
--e;
--d;
/* Function Body */
*ierr = 0;
if (n == 1) {
goto L1001;
}
for (i = 2; i <= n; ++i) {
e[i - 1] = e[i];
}
f = 0.;
tst1 = 0.;
e[n] = 0.;
for (l = 1; l <= n; ++l) {
j = 0;
h = (d__1 = d[l], abs(d__1)) + (d__2 = e[l], abs(d__2));
if (tst1 < h) {
tst1 = h;
}
/* .......... look for small sub-diagonal element .......... */
for (m = l; m <= n; ++m) {
tst2 = tst1 + (d__1 = e[m], abs(d__1));
if (tst2 == tst1) {
goto L120;
}
/* .......... e(n) is always zero, so there is no exit */
/* through the bottom of the loop .......... */
}
L120:
if (m == l) {
goto L210;
}
L130:
if (j == 30) {
goto L1000;
}
++j;
/* .......... form shift .......... */
l1 = l + 1;
l2 = l1 + 1;
g = d[l];
p = (d[l1] - g) / (e[l] * 2.);
r = pythag(p, 1.0);
d[l] = e[l] / (p + d_sign(&r, &p));
d[l1] = e[l] * (p + d_sign(&r, &p));
dl1 = d[l1];
h = g - d[l];
if (l2 > n) {
goto L145;
}
for (i = l2; i <= n; ++i) {
d[i] -= h;
}
L145:
f += h;
/* .......... ql transformation .......... */
p = d[m];
c = 1.;
c2 = c;
el1 = e[l1];
s = 0.;
mml = m - l;
/* .......... for i=m-1 step -1 until l do -- .......... */
for (ii = 1; ii <= mml; ++ii) {
c3 = c2;
c2 = c;
s2 = s;
i = m - ii;
g = c * e[i];
h = c * p;
r = pythag(p, e[i]);
e[i + 1] = s * r;
s = e[i] / r;
c = p / r;
p = c * d[i] - s * g;
d[i + 1] = h + s * (c * g + s * d[i]);
}
p = -s * s2 * c3 * el1 * e[l] / dl1;
e[l] = s * p;
d[l] = c * p;
tst2 = tst1 + (d__1 = e[l], abs(d__1));
if (tst2 > tst1) {
goto L130;
}
L210:
p = d[l] + f;
/* .......... order eigenvalues .......... */
if (l == 1) {
goto L250;
}
/* .......... for i=l step -1 until 2 do -- .......... */
for (ii = 2; ii <= l; ++ii) {
i = l + 2 - ii;
if (p >= d[i - 1]) {
goto L270;
}
d[i] = d[i - 1];
}
L250:
i = 1;
L270:
d[i] = p;
}
goto L1001;
/* .......... set error -- no convergence to an */
/* eigenvalue after 30 iterations .......... */
L1000:
*ierr = l;
L1001:
return;
}
long int C_toolbox_SVD::dsvd(double **a, long int m, long int n, double *w, double **v)
{
long int flag, i, its, j, jj, k, l, nm;
double c, f, h, s, x, y, z;
double anorm = 0.0, g = 0.0, scale = 0.0;
double *rv1;
if (m < n)
{
throw "#rows must be > #cols";
return 0;
}
rv1 = new double[n];//(double *)malloc((unsigned int) n*sizeof(double));
/* Householder reduction to bidiagonal form */
for (i = 0; i < n; i++)
{
/* left-hand reduction */
l = i + 1;
rv1[i] = scale * g;
g = s = scale = 0.0;
if (i < m)
{
for (k = i; k < m; k++)
scale += ABS((double)a[k][i]);
if (scale)
{
for (k = i; k < m; k++)
{
a[k][i] = (double)((double)a[k][i]/scale);
s += ((double)a[k][i] * (double)a[k][i]);
}
f = (double)a[i][i];
g = -SIGN(sqrt(s), f);
h = f * g - s;
a[i][i] = (double)(f - g);
if (i != n - 1)
{
for (j = l; j < n; j++)
{
for (s = 0.0, k = i; k < m; k++)
s += ((double)a[k][i] * (double)a[k][j]);
f = s / h;
for (k = i; k < m; k++)
a[k][j] += (double)(f * (double)a[k][i]);
}
}
for (k = i; k < m; k++)
a[k][i] = (double)((double)a[k][i]*scale);
}
}
w[i] = (double)(scale * g);
/* right-hand reduction */
g = s = scale = 0.0;
if (i < m && i != n - 1)
{
for (k = l; k < n; k++)
scale += ABS((double)a[i][k]);
if (scale)
{
for (k = l; k < n; k++)
{
a[i][k] = (double)((double)a[i][k]/scale);
s += ((double)a[i][k] * (double)a[i][k]);
}
f = (double)a[i][l];
g = -SIGN(sqrt(s), f);
h = f * g - s;
a[i][l] = (double)(f - g);
for (k = l; k < n; k++)
rv1[k] = (double)a[i][k] / h;
if (i != m - 1)
{
for (j = l; j < m; j++)
{
for (s = 0.0, k = l; k < n; k++)
s += ((double)a[j][k] * (double)a[i][k]);
for (k = l; k < n; k++)
a[j][k] += (double)(s * rv1[k]);
}
}
for (k = l; k < n; k++)
a[i][k] = (double)((double)a[i][k]*scale);
}
}
anorm = MAX(anorm, (ABS((double)w[i]) + ABS(rv1[i])));
}
/* accumulate the right-hand transformation */
for (i = n - 1; i >= 0; i--)
{
if (i < n - 1)
{
if (g)
{
for (j = l; j < n; j++)
v[j][i] = (double)(((double)a[i][j] / (double)a[i][l]) / g);
/* double division to avoid underflow */
for (j = l; j < n; j++)
{
for (s = 0.0, k = l; k < n; k++)
s += ((double)a[i][k] * (double)v[k][j]);
for (k = l; k < n; k++)
v[k][j] += (double)(s * (double)v[k][i]);
}
}
for (j = l; j < n; j++)
v[i][j] = v[j][i] = 0.0;
}
v[i][i] = 1.0;
g = rv1[i];
l = i;
}
/* accumulate the left-hand transformation */
for (i = n - 1; i >= 0; i--)
{
l = i + 1;
g = (double)w[i];
if (i < n - 1)
for (j = l; j < n; j++)
a[i][j] = 0.0;
if (g)
{
g = 1.0 / g;
if (i != n - 1)
{
for (j = l; j < n; j++)
{
for (s = 0.0, k = l; k < m; k++)
s += ((double)a[k][i] * (double)a[k][j]);
f = (s / (double)a[i][i]) * g;
for (k = i; k < m; k++)
a[k][j] += (double)(f * (double)a[k][i]);
}
}
for (j = i; j < m; j++)
a[j][i] = (double)((double)a[j][i]*g);
}
else
{
for (j = i; j < m; j++)
a[j][i] = 0.0;
}
++a[i][i];
}
/* diagonalize the bidiagonal form */
for (k = n - 1; k >= 0; k--)
{ /* loop over singular values */
for (its = 0; its < 30; its++)
{ /* loop over allowed iterations */
flag = 1;
for (l = k; l >= 0; l--)
{ /* test for splitting */
nm = l - 1;
if (ABS(rv1[l]) + anorm == anorm)
{
flag = 0;
break;
}
if (ABS((double)w[nm]) + anorm == anorm)
break;
}
if (flag)
{
c = 0.0;
s = 1.0;
for (i = l; i <= k; i++)
{
f = s * rv1[i];
if (ABS(f) + anorm != anorm)
{
g = (double)w[i];
h = pythag(f, g);
w[i] = (double)h;
h = 1.0 / h;
c = g * h;
s = (- f * h);
for (j = 0; j < m; j++)
{
y = (double)a[j][nm];
z = (double)a[j][i];
a[j][nm] = (double)(y * c + z * s);
a[j][i] = (double)(z * c - y * s);
}
}
}
}
z = (double)w[k];
if (l == k)
{ /* convergence */
if (z < 0.0)
{ /* make singular value nonnegative */
w[k] = (double)(-z);
for (j = 0; j < n; j++)
v[j][k] = (-v[j][k]);
}
break;
}
if (its >= 30) {
delete rv1;
throw "No convergence after 30,000! iterations";
return(0);
}
/* shift from bottom 2 x 2 minor */
x = (double)w[l];
nm = k - 1;
y = (double)w[nm];
g = rv1[nm];
h = rv1[k];
f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y);
g = pythag(f, 1.0);
f = ((x - z) * (x + z) + h * ((y / (f + SIGN(g, f))) - h)) / x;
/* next QR transformation */
c = s = 1.0;
for (j = l; j <= nm; j++)
{
i = j + 1;
g = rv1[i];
y = (double)w[i];
h = s * g;
g = c * g;
z = pythag(f, h);
rv1[j] = z;
c = f / z;
s = h / z;
f = x * c + g * s;
g = g * c - x * s;
h = y * s;
y = y * c;
for (jj = 0; jj < n; jj++)
{
x = (double)v[jj][j];
z = (double)v[jj][i];
v[jj][j] = (double)(x * c + z * s);
v[jj][i] = (double)(z * c - x * s);
}
z = pythag(f, h);
w[j] = (double)z;
if (z)
{
z = 1.0 / z;
c = f * z;
s = h * z;
}
f = (c * g) + (s * y);
x = (c * y) - (s * g);
for (jj = 0; jj < m; jj++)
{
y = (double)a[jj][j];
z = (double)a[jj][i];
a[jj][j] = (double)(y * c + z * s);
a[jj][i] = (double)(z * c - y * s);
}
}
rv1[l] = 0.0;
rv1[k] = f;
w[k] = (double)x;
}
}
//free((void*) rv1);
delete rv1;
return 1;
}
/*The lengths of nodeXlist and nodeYlist are all supposed to be 2*/
void footxy(double *nodeXlist,double *nodeYlist, double *X,double *Y, double *dist, double *fx, double *fy)
{
double A,B,C;
int Xmax, Xmin, Ymax, Ymin;
A=nodeYlist[1]-nodeYlist[0];
B=nodeXlist[0]-nodeXlist[1];
C=nodeYlist[0]*nodeXlist[1]-nodeYlist[1]*nodeXlist[0];
dist[0]=fabs(A*X[0]+B*Y[0]+C)/sqrt(A*A+B*B);
if (A==(double)0)
{
if (B==(double)0)
{
fx[0]=nodeXlist[0];
fy[0]=nodeYlist[0];
}
else
{
fy[0]=nodeYlist[0];
fx[0]=X[0];
}
}
else
{
if (B==(double)0)
{
fx[0]=nodeXlist[0];
fy[0]=Y[0];
}
else
{
fx[0]=B*B*X[0]/(A*A+B*B)-A*B*Y[0]/(A*A+B*B)-A*C/(A*A+B*B);
fy[0]=-A*B*X[0]/(A*A+B*B)+A*A*Y[0]/(A*A+B*B)-B*C/(A*A+B*B);
}
}
if (A==(double)0)
{
Xmax=maxindex(nodeXlist, (int)2);
Xmin=minindex(nodeXlist, (int)2);
if (fx[0]<nodeXlist[Xmin])
{
dist[0]=pythag((nodeXlist[Xmin]-X[0]),(nodeYlist[Xmin]-Y[0]));
fx[0]=nodeXlist[Xmin];
}
if (fx[0]>nodeXlist[Xmax])
{
dist[0]=pythag((nodeXlist[Xmax]-X[0]),(nodeYlist[Xmax]-Y[0]));
fx[0]=nodeXlist[Xmin];
}
}
else
{
Ymax=maxindex(nodeYlist, (int)2);
Ymin=minindex(nodeYlist, (int)2);
if (fy[0]<nodeYlist[Ymin])
{
dist[0]=pythag((nodeXlist[Ymin]-X[0]),(nodeYlist[Ymin]-Y[0]));
fx[0]=nodeXlist[Ymin];
fy[0]=nodeYlist[Ymin];
}
if (fy[0]>nodeYlist[Ymax])
{
dist[0]=pythag((nodeXlist[Ymax]-X[0]),(nodeYlist[Ymax]-Y[0]));
fx[0]=nodeXlist[Ymax];
fy[0]=nodeYlist[Ymax];
}
}
}
static CvStatus
icvSVD_32f( float* a, int lda, float* w,
float* u, int ldu, float* v, int ldv,
CvSize size, float* buffer )
{
float* e;
float* temp;
float *w1, *e1;
float *hv;
double ku0 = 0, kv0 = 0;
double anorm = 0;
float *a1 = a, *u0 = u, *v0 = v;
float *u1, *v1;
int ldu1, ldv1;
double scale, h;
int i, j, k, l;
int n = size.width, m = size.height;
int nm, m1, n1;
int iters = 0;
e = buffer;
if( m >= n )
{
w1 = w;
e1 = e + 1;
nm = n;
}
else
{
w1 = e + 1;
e1 = w;
nm = m;
}
temp = buffer + nm;
memset( w, 0, nm * sizeof( w[0] ));
memset( e, 0, nm * sizeof( e[0] ));
m1 = m;
n1 = n;
if( m < n )
goto row_transform;
for( ;; )
{
if( m1 == 0 )
break;
scale = h = 0;
a = a1;
hv = u ? u : w1;
for( j = 0; j < m1; j++, a += lda )
{
double t = a[0];
hv[j] = (float)t;
scale += fabs(t);
}
if( scale != 0 )
{
double f = 1./scale, g, s = 0;
for( j = 0; j < m1; j++ )
{
double t = hv[j]*f;
hv[j] = (float)t;
s += t * t;
}
g = sqrt( s );
f = hv[0];
if( f >= 0 )
g = -g;
hv[0] = (float)(f - g);
h = 1. / (f * g - s);
memset( temp, 0, n1 * sizeof( temp[0] ));
a = a1;
/* calc temp[0:n-i] = a[i:m,i:n]'*hv[0:m-i] */
icvMatrAXPY1_32f( m1, n1 - 1, a + 1, lda, hv, temp + 1 );
for( k = 1; k < n1; k++ ) temp[k] = (float)(temp[k]*h);
a = a1;
/* modify a: a[i:m,i:n] = a[i:m,i:n] + hv[0:m-i]*temp[0:n-i]' */
icvMatrAXPY2_32f( m1, n1 - 1, temp + 1, lda, hv, a + 1 );
*w1++ = (float)(g*scale);
}
/* store -2/(hv'*hv) */
if( u )
{
if( m1 == m )
ku0 = h;
else
hv[-1] = (float)h;
}
a1++;
n1--;
if( v )
v += ldv + 1;
row_transform:
if( n1 == 0 )
break;
scale = h = 0;
a = a1;
hv = v ? v : e1;
for( j = 0; j < n1; j++ )
{
double t = a[j];
hv[j] = (float)t;
scale += fabs(t);
}
if( scale != 0 )
{
double f = 1./scale, g, s = 0;
for( j = 0; j < n1; j++ )
{
double t = hv[j] * f;
hv[j] = (float)t;
s += t * t;
}
g = sqrt( s );
f = hv[0];
if( f >= 0 )
g = -g;
hv[0] = (float)(f - g);
h = 1. / (f * g - s);
/* update a[i:m:i+1:n] = a[i:m,i+1:n] + (a[i:m,i+1:n]*hv[0:m-i])*... */
icvMatrAXPY3_32f( m1, n1, hv, lda, a, h );
*e1++ = (float)(g*scale);
}
/* store -2/(hv'*hv) */
if( v )
{
if( n1 == n )
kv0 = h;
else
hv[-1] = (float)h;
}
a1 += lda;
m1--;
if( u )
u += ldu + 1;
}
m1 -= m1 != 0;
n1 -= n1 != 0;
/* accumulate left transformations */
if( u )
{
m1 = m - m1;
u = u0 + m1 * ldu;
for( i = m1; i < m; i++, u += ldu )
{
memset( u + m1, 0, (m - m1) * sizeof( u[0] ));
u[i] = 1.;
}
for( i = m1 - 1; i >= 0; i-- )
{
double h, s;
l = m - i;
hv = u0 + (ldu + 1) * i;
h = i == 0 ? ku0 : hv[-1];
assert( h <= 0 );
if( h != 0 )
{
u = hv;
icvMatrAXPY3_32f( l, l-1, hv+1, ldu, u+1, h );
s = hv[0] * h;
for( k = 0; k < l; k++ )
hv[k] = (float)(hv[k]*s);
hv[0] += 1;
}
else
{
for( j = 1; j < l; j++ )
hv[j] = hv[j * ldu] = 0;
hv[0] = 1;
}
}
u = u0;
}
/* accumulate right transformations */
if( v )
{
n1 = n - n1;
v = v0 + n1 * ldv;
for( i = n1; i < n; i++, v += ldv )
{
memset( v + n1, 0, (n - n1) * sizeof( v[0] ));
v[i] = 1.;
}
for( i = n1 - 1; i >= 0; i-- )
{
double h, s;
l = n - i;
hv = v0 + (ldv + 1) * i;
h = i == 0 ? kv0 : hv[-1];
assert( h <= 0 );
if( h != 0 )
{
v = hv;
icvMatrAXPY3_32f( l, l-1, hv+1, ldv, v+1, h );
s = hv[0] * h;
for( k = 0; k < l; k++ )
hv[k] = (float)(hv[k]*s);
hv[0] += 1;
}
else
{
for( j = 1; j < l; j++ )
hv[j] = hv[j * ldv] = 0;
hv[0] = 1;
}
}
v = v0;
}
for( i = 0; i < nm; i++ )
{
double tnorm = fabs( w[i] ) + fabs( e[i] );
if( anorm < tnorm )
anorm = tnorm;
}
if( m >= n )
{
m1 = m;
n1 = n;
u1 = u;
ldu1 = ldu;
v1 = v;
ldv1 = ldv;
}
else
{
m1 = n;
n1 = m;
u1 = v;
ldu1 = ldv;
v1 = u;
ldv1 = ldu;
}
/* diagonalization of the bidiagonal form */
for( k = nm - 1; k >= 0; k-- )
{
double z = 0;
iters = 0;
for( ;; ) /* do iterations */
{
double c, s, f, g, h, x, y;
int flag = 0;
/* test for splitting */
for( l = k; l >= 0; l-- )
{
if( anorm + fabs( e[l] ) == anorm )
{
flag = 1;
break;
}
assert( l > 0 );
if( anorm + fabs( w[l - 1] ) == anorm )
break;
}
if( !flag )
{
c = 0;
s = 1;
for( i = l; i <= k; i++ )
{
double f = s * e[i];
e[i] = (float)(e[i]*c);
if( anorm + fabs( f ) == anorm )
break;
g = w[i];
h = pythag( f, g );
w[i] = (float)h;
c = g / h;
s = -f / h;
if( u1 )
{
icvGivens_32f( m1, u1 + ldu1 * (i - 1), u1 + ldu1 * i, c, s );
}
}
}
z = w[k];
if( l == k || iters++ == MAX_ITERS )
break;
/* shift from bottom 2x2 minor */
x = w[l];
y = w[k - 1];
g = e[k - 1];
h = e[k];
f = 0.5 * (((g + z) / h) * ((g - z) / y) + y / h - h / y);
g = pythag( f, 1 );
if( f < 0 )
g = -g;
f = x - (z / x) * z + (h / x) * (y / (f + g) - h);
/* next QR transformation */
c = s = 1;
for( i = l + 1; i <= k; i++ )
{
g = e[i];
y = w[i];
h = s * g;
g *= c;
z = pythag( f, h );
e[i - 1] = (float)z;
c = f / z;
s = h / z;
f = x * c + g * s;
g = -x * s + g * c;
h = y * s;
y *= c;
if( v1 )
{
icvGivens_32f( n1, v1 + ldv1 * (i - 1), v1 + ldv1 * i, c, s );
}
z = pythag( f, h );
w[i - 1] = (float)z;
/* rotation can be arbitrary if z == 0 */
if( z != 0 )
{
c = f / z;
s = h / z;
}
f = c * g + s * y;
x = -s * g + c * y;
if( u1 )
{
icvGivens_32f( m1, u1 + ldu1 * (i - 1), u1 + ldu1 * i, c, s );
}
}
e[l] = 0;
e[k] = (float)f;
w[k] = (float)x;
} /* end of iteration loop */
if( iters > MAX_ITERS )
break;
if( z < 0 )
{
w[k] = (float)(-z);
if( v )
{
for( j = 0; j < n; j++ )
v[j + k * ldv] = -v[j + k * ldv];
}
}
} /* end of diagonalization loop */
/* sort singular values */
for( i = 0; i < nm; i++ )
{
k = i;
for( j = i + 1; j < nm; j++ )
if( w[k] < w[j] )
k = j;
if( k != i )
{
/* swap i & k values */
float t = w[k];
w[k] = w[i];
w[i] = t;
if( v )
{
for( j = 0; j < n; j++ )
{
t = v[j + ldv * k];
v[j + ldv * k] = v[j + ldv * i];
v[j + ldv * i] = t;
}
}
if( u )
{
for( j = 0; j < m; j++ )
{
t = u[j + k * ldu];
u[j + ldu * k] = u[j + i * ldu];
u[j + ldu * i] = t;
}
}
}
}
return CV_NO_ERR;
}
void svdcmp(double **a, int m, int n, double *w, double **v)
{
int flag,i,its,j,jj,k,l,nm;
double c,f,h,s,x,y,z;
double anorm=0.0,g=0.0,scale=0.0;
double *rv1,*dvector(),pythag();
void nrerror(),free_dvector();
l = 0;
nm = 0;
rv1=dvector(1,n);
for (i=1;i<=n;i++) {
l=i+1;
rv1[i]=scale*g;
g=s=scale=0.0;
if (i <= m)
{
for (k=i;k<=m;k++) scale += fabs(a[k][i]);
if (scale)
{
for (k=i;k<=m;k++)
{
a[k][i] /= scale;
s += a[k][i]*a[k][i];
}
f=a[i][i];
g = -SIGN(sqrt(s),f);
h=f*g-s;
a[i][i]=f-g;
for (j=l;j<=n;j++)
{
for (s=0.0,k=i;k<=m;k++) s += a[k][i]*a[k][j];
f=s/h;
for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
}
for (k=i;k<=m;k++) a[k][i] *= scale;
}
}
w[i]=scale*g;
g=s=scale=0.0;
if (i <= m && i != n)
{
for (k=l;k<=n;k++) scale += fabs(a[i][k]);
if (scale)
{
for (k=l;k<=n;k++)
{
a[i][k] /= scale;
s += a[i][k]*a[i][k];
}
f=a[i][l];
g = -SIGN(sqrt(s),f);
h=f*g-s;
a[i][l]=f-g;
for (k=l;k<=n;k++) rv1[k]=a[i][k]/h;
for (j=l;j<=m;j++)
{
for (s=0.0,k=l;k<=n;k++) s += a[j][k]*a[i][k];
for (k=l;k<=n;k++) a[j][k] += s*rv1[k];
}
for (k=l;k<=n;k++) a[i][k] *= scale;
}
}
anorm=max(anorm,(fabs(w[i])+fabs(rv1[i])));
}
for (i=n;i>=1;i--)
{
if (i < n)
{
if (g)
{
for (j=l;j<=n;j++)
v[j][i]=(a[i][j]/a[i][l])/g;
for (j=l;j<=n;j++)
{
for (s=0.0,k=l;k<=n;k++) s += a[i][k]*v[k][j];
for (k=l;k<=n;k++) v[k][j] += s*v[k][i];
}
}
for (j=l;j<=n;j++) v[i][j]=v[j][i]=0.0;
}
v[i][i]=1.0;
g=rv1[i];
l=i;
}
for (i=min(m,n);i>=1;i--)
{
l=i+1;
g=w[i];
for (j=l;j<=n;j++) a[i][j]=0.0;
if (g)
{
g=1.0/g;
for (j=l;j<=n;j++)
{
for (s=0.0,k=l;k<=m;k++) s += a[k][i]*a[k][j];
f=(s/a[i][i])*g;
for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
}
for (j=i;j<=m;j++) a[j][i] *= g;
}
else
for (j=i;j<=m;j++) a[j][i]=0.0;
++a[i][i];
}
for (k=n;k>=1;k--)
{
for (its=1;its<=30;its++)
{
flag=1;
for (l=k;l>=1;l--)
{
nm=l-1;
if (fabs(rv1[l])+anorm == anorm)
{
flag=0;
break;
}
if (fabs(w[nm])+anorm == anorm) break;
}
if (flag)
{
c=0.0;
s=1.0;
for (i=l;i<=k;i++)
{
f=s*rv1[i];
rv1[i]=c*rv1[i];
if (fabs(f)+anorm == anorm) break;
g=w[i];
h=pythag(f,g);
w[i]=h;
h=1.0/h;
c=g*h;
s=(-f*h);
for (j=1;j<=m;j++)
{
y=a[j][nm];
z=a[j][i];
a[j][nm]=y*c+z*s;
a[j][i]=z*c-y*s;
}
}
}
z=w[k];
if (l == k)
{
if (z < 0.0)
{
w[k] = -z;
for (j=1;j<=n;j++)
{
v[j][k]=(-v[j][k]);
}
}
break;
}
if (its == 30)
nrerror("No convergence in 30 SVDCMP iterations");
x=w[l];
nm=k-1;
y=w[nm];
g=rv1[nm];
h=rv1[k];
f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
g=pythag(f,1.0);
f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
c=s=1.0;
for (j=l;j<=nm;j++)
{
i=j+1;
g=rv1[i];
y=w[i];
h=s*g;
g=c*g;
z=pythag(f,h);
rv1[j]=z;
c=f/z;
s=h/z;
f=x*c+g*s;
g=g*c-x*s;
h=y*s;
y *= c;
for (jj=1;jj<=n;jj++)
{
x=v[jj][j];
z=v[jj][i];
v[jj][j]=x*c+z*s;
v[jj][i]=z*c-x*s;
}
z=pythag(f,h);
w[j]=z;
if (z)
{
z=1.0/z;
c=f*z;
s=h*z;
}
f=c*g+s*y;
x=c*y-s*g;
for (jj=1;jj<=m;jj++)
{
y=a[jj][j];
z=a[jj][i];
a[jj][j]=y*c+z*s;
a[jj][i]=z*c-y*s;
}
}
rv1[l]=0.0;
rv1[k]=f;
w[k]=x;
} /* end for its */
} /* end for k */
free_dvector(rv1,1,n);
}
int32_t svdcmp_c(int32_t m, double* a, double* w, double* v) {
// C port of PLINK stats.cpp svdcmp().
// now thread-safe.
double* rv1 = &(w[(uint32_t)m]);
int32_t n = m;
int32_t flag;
int32_t l = 0; // suppress compile warning
int32_t i,its,j,jj,k,nm;
double anorm,c,f,g,h,s,scale,x,y,z;
double temp;
g=scale=anorm=0.0;
for (i=0; i<n; i++) {
l=i+2;
rv1[i]=scale*g;
g=s=scale=0.0;
if (i < m) {
for (k=i; k<m; k++) scale += fabs(a[k * m + i]);
if (scale != 0.0) {
for (k=i; k<m; k++) {
a[k * m + i] /= scale;
s += a[k * m + i]*a[k * m + i];
}
f=a[i * m + i];
g = -SIGN(sqrt(s),f);
h=f*g-s;
a[i * m + i]=f-g;
for (j=l-1; j<n; j++) {
for (s=0.0,k=i; k<m; k++) s += a[k * m + i]*a[k * m + j];
f=s/h;
for (k=i; k<m; k++) a[k * m + j] += f*a[k * m + i];
}
for (k=i; k<m; k++) a[k * m + i] *= scale;
}
}
w[i]=scale *g;
g=s=scale=0.0;
if (i+1 <= m && i+1 != n) {
for (k=l-1; k<n; k++) scale += fabs(a[i * m + k]);
if (scale != 0.0) {
for (k=l-1; k<n; k++) {
a[i * m + k] /= scale;
s += a[i * m + k]*a[i * m + k];
}
f=a[i * m + l-1];
g = -SIGN(sqrt(s),f);
h=f*g-s;
a[i * m + l-1]=f-g;
for (k=l-1; k<n; k++) rv1[k]=a[i * m + k]/h;
for (j=l-1; j<m; j++) {
for (s=0.0,k=l-1; k<n; k++) s += a[j * m + k]*a[i * m + k];
for (k=l-1; k<n; k++) a[j * m + k] += s*rv1[k];
}
for (k=l-1; k<n; k++) a[i * m + k] *= scale;
}
}
anorm=MAXV(anorm,(fabs(w[i])+fabs(rv1[i])));
}
for (i=n-1; i>=0; i--) {
if (i < n-1) {
if (g != 0.0) {
for (j=l; j<n; j++)
v[j * m + i]=(a[i * m + j]/a[i * m + l])/g;
for (j=l; j<n; j++) {
for (s=0.0,k=l; k<n; k++) s += a[i * m + k]*v[k * m + j];
for (k=l; k<n; k++) v[k * m + j] += s*v[k * m + i];
}
}
for (j=l; j<n; j++) v[i * m + j]=v[j * m + i]=0.0;
}
v[i * m + i]=1.0;
g=rv1[i];
l=i;
}
for (i=MINV(m,n)-1; i>=0; i--) {
l=i+1;
g=w[i];
for (j=l; j<n; j++) a[i * m + j]=0.0;
if (g != 0.0) {
g=1.0/g;
for (j=l; j<n; j++) {
for (s=0.0,k=l; k<m; k++) s += a[k * m + i]*a[k * m + j];
f=(s/a[i * m + i])*g;
for (k=i; k<m; k++) a[k * m + j] += f*a[k * m + i];
}
for (j=i; j<m; j++) a[j * m + i] *= g;
} else for (j=i; j<m; j++) a[j * m + i]=0.0;
++a[i * m + i];
}
for (k=n-1; k>=0; k--) {
for (its=0; its<30; its++) {
flag=1;
for (l=k; l>=0; l--) {
nm=l-1;
temp=fabs(rv1[l])+anorm;
if (temp == anorm) {
flag=0;
break;
}
temp=fabs(w[nm])+anorm;
if (temp == anorm) break;
}
if (flag) {
c=0.0;
s=1.0;
for (i=l; i<k+1; i++) {
f=s*rv1[i];
rv1[i]=c*rv1[i];
temp = fabs(f)+anorm;
if (temp == anorm) break;
g=w[i];
h=pythag(f,g);
w[i]=h;
h=1.0/h;
c=g*h;
s = -f*h;
for (j=0; j<m; j++) {
y=a[j * m + nm];
z=a[j * m + i];
a[j * m + nm]=y*c+z*s;
a[j * m + i]=z*c-y*s;
}
}
}
z=w[k];
if (l == k) {
if (z < 0.0) {
w[k] = -z;
for (j=0; j<n; j++) v[j * m + k] = -v[j * m + k];
}
break;
}
if (its == 29)
return 0; // cannot converge: multi-collinearity?
x=w[l];
nm=k-1;
y=w[nm];
g=rv1[nm];
h=rv1[k];
f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
g=pythag(f,1.0);
f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
c=s=1.0;
for (j=l; j<=nm; j++) {
i=j+1;
g=rv1[i];
y=w[i];
h=s*g;
g=c*g;
z=pythag(f,h);
rv1[j]=z;
c=f/z;
s=h/z;
f=x*c+g*s;
g=g*c-x*s;
h=y*s;
y *= c;
for (jj=0; jj<n; jj++) {
x=v[jj * m + j];
z=v[jj * m + i];
v[jj * m + j]=x*c+z*s;
v[jj * m + i]=z*c-x*s;
}
z=pythag(f,h);
w[j]=z;
if (z) {
z=1.0/z;
c=f*z;
s=h*z;
}
f=c*g+s*y;
x=c*y-s*g;
for (jj=0; jj<m; jj++) {
y=a[jj * m + j];
z=a[jj * m + i];
a[jj * m + j]=y*c+z*s;
a[jj * m + i]=z*c-y*s;
}
}
rv1[l]=0.0;
rv1[k]=f;
w[k]=x;
}
}
return 1;
}
/** SVD decomposition
*
* --------------------------------------------------------------------- *
* Reference: "Numerical Recipes By W.H. Press, B. P. Flannery, *
* S.A. Teukolsky and W.T. Vetterling, Cambridge *
* University Press, 1986" [BIBLI 08]. *
* --------------------------------------------------------------------- *
*
* Given a matrix a(m,n), this routine computes its singular value decomposition,
* A = U · W · Vt. The matrix U replaces a on output. The diagonal matrix of singular
* values W is output as a vector w(n). The matrix V (not the transpose Vt) is output
* as v(n,n).
*
* @param a input matrix [m x n] and output matrix U [m x n]
* @param w output diagonal vector of matrix W [n]
* @param v output square matrix V [n x n]
* @param m number of rows of input the matrix
* @param n number of columns of the input matrix
* @return 0 (false) if convergence failed, 1 (true) if decomposition succed
*/
int pprz_svd_float(float **a, float *w, float **v, int m, int n)
{
/* Householder reduction to bidiagonal form. */
int flag, i, its, j, jj, k, l, NM;
float C, F, H, S, X, Y, Z, tmp;
float G = 0.0;
float Scale = 0.0;
float ANorm = 0.0;
float rv1[n];
for (i = 0; i < n; ++i) {
l = i + 1;
rv1[i] = Scale * G;
G = 0.0;
S = 0.0;
Scale = 0.0;
if (i < m) {
for (k = i; k < m; ++k) {
Scale = Scale + fabsf(a[k][i]);
}
if (Scale != 0.0) {
for (k = i; k < m; ++k) {
a[k][i] = a[k][i] / Scale;
S = S + a[k][i] * a[k][i];
}
F = a[i][i];
G = sqrtf(S);
if (F > 0.0) {
G = -G;
}
H = F * G - S;
a[i][i] = F - G;
if (i != (n - 1)) {
for (j = l; j < n; ++j) {
S = 0.0;
for (k = i; k < m; ++k) {
S = S + a[k][i] * a[k][j];
}
F = S / H;
for (k = i; k < m; ++k) {
a[k][j] = a[k][j] + F * a[k][i];
}
}
}
for (k = i; k < m; ++k) {
a[k][i] = Scale * a[k][i];
}
}
}
w[i] = Scale * G;
G = 0.0;
S = 0.0;
Scale = 0.0;
if ((i < m) && (i != (n - 1))) {
for (k = l; k < n; ++k) {
Scale = Scale + fabsf(a[i][k]);
}
if (Scale != 0.0) {
for (k = l; k < n; ++k) {
a[i][k] = a[i][k] / Scale;
S = S + a[i][k] * a[i][k];
}
F = a[i][l];
G = sqrtf(S);
if (F > 0.0) {
G = -G;
}
H = F * G - S;
a[i][l] = F - G;
for (k = l; k < n; ++k) {
rv1[k] = a[i][k] / H;
}
if (i != (m - 1)) {
for (j = l; j < m; ++j) {
S = 0.0;
for (k = l; k < n; ++k) {
S = S + a[j][k] * a[i][k];
}
for (k = l; k < n; ++k) {
a[j][k] = a[j][k] + S * rv1[k];
}
}
}
for (k = l; k < n; ++k) {
a[i][k] = Scale * a[i][k];
}
}
}
tmp = fabsf(w[i]) + fabsf(rv1[i]);
if (tmp > ANorm) {
ANorm = tmp;
}
}
/* Accumulation of right-hand transformations. */
for (i = n - 1; i >= 0; --i) {
if (i < (n - 1)) {
if (G != 0.0) {
for (j = l; j < n; ++j) {
v[j][i] = (a[i][j] / a[i][l]) / G;
}
for (j = l; j < n; ++j) {
S = 0.0;
for (k = l; k < n; ++k) {
S = S + a[i][k] * v[k][j];
}
for (k = l; k < n; ++k) {
v[k][j] = v[k][j] + S * v[k][i];
}
}
}
for (j = l; j < n; ++j) {
v[i][j] = 0.0;
v[j][i] = 0.0;
}
}
v[i][i] = 1.0;
G = rv1[i];
l = i;
}
/* Accumulation of left-hand transformations. */
for (i = n - 1; i >= 0; --i) {
l = i + 1;
G = w[i];
if (i < (n - 1)) {
for (j = l; j < n; ++j) {
a[i][j] = 0.0;
}
}
if (G != 0.0) {
G = 1.0 / G;
if (i != (n - 1)) {
for (j = l; j < n; ++j) {
S = 0.0;
for (k = l; k < m; ++k) {
S = S + a[k][i] * a[k][j];
}
F = (S / a[i][i]) * G;
for (k = i; k < m; ++k) {
a[k][j] = a[k][j] + F * a[k][i];
}
}
}
for (j = i; j < m; ++j) {
a[j][i] = a[j][i] * G;
}
} else {
for (j = i; j < m; ++j) {
a[j][i] = 0.0;
}
}
a[i][i] = a[i][i] + 1.0;
}
/* Diagonalization of the bidiagonal form.
Loop over singular values. */
for (k = (n - 1); k >= 0; --k) {
/* Loop over allowed iterations. */
for (its = 1; its <= 30; ++its) {
/* Test for splitting.
Note that rv1[0] is always zero. */
flag = true;
for (l = k; l >= 0; --l) {
NM = l - 1;
if ((fabsf(rv1[l]) + ANorm) == ANorm) {
flag = false;
break;
} else if ((fabsf(w[NM]) + ANorm) == ANorm) {
break;
}
}
/* Cancellation of rv1[l], if l > 0; */
if (flag) {
C = 0.0;
S = 1.0;
for (i = l; i <= k; ++i) {
F = S * rv1[i];
if ((fabsf(F) + ANorm) != ANorm) {
G = w[i];
//H = sqrtf( F * F + G * G );
H = pythag(F, G);
w[i] = H;
H = 1.0 / H;
C = (G * H);
S = -(F * H);
for (j = 0; j < m; ++j) {
Y = a[j][NM];
Z = a[j][i];
a[j][NM] = (Y * C) + (Z * S);
a[j][i] = -(Y * S) + (Z * C);
}
}
}
}
Z = w[k];
/* Convergence. */
if (l == k) {
/* Singular value is made nonnegative. */
if (Z < 0.0) {
w[k] = -Z;
for (j = 0; j < n; ++j) {
v[j][k] = -v[j][k];
}
}
break;
}
if (its >= 30) {
// No convergence in 30 iterations
return 0;
}
X = w[l];
NM = k - 1;
Y = w[NM];
G = rv1[NM];
H = rv1[k];
F = ((Y - Z) * (Y + Z) + (G - H) * (G + H)) / (2.0 * H * Y);
//G = sqrtf( F * F + 1.0 );
G = pythag(F, 1.0);
tmp = G;
if (F < 0.0) {
tmp = -tmp;
}
F = ((X - Z) * (X + Z) + H * ((Y / (F + tmp)) - H)) / X;
/* Next QR transformation. */
C = 1.0;
S = 1.0;
for (j = l; j <= NM; ++j) {
i = j + 1;
G = rv1[i];
Y = w[i];
H = S * G;
G = C * G;
//Z = sqrtf( F * F + H * H );
Z = pythag(F, H);
rv1[j] = Z;
C = F / Z;
S = H / Z;
F = (X * C) + (G * S);
G = -(X * S) + (G * C);
H = Y * S;
Y = Y * C;
for (jj = 0; jj < n; ++jj) {
X = v[jj][j];
Z = v[jj][i];
v[jj][j] = (X * C) + (Z * S);
v[jj][i] = -(X * S) + (Z * C);
}
//Z = sqrtf( F * F + H * H );
Z = pythag(F, H);
w[j] = Z;
/* Rotation can be arbitrary if Z = 0. */
if (Z != 0.0) {
Z = 1.0 / Z;
C = F * Z;
S = H * Z;
}
F = (C * G) + (S * Y);
X = -(S * G) + (C * Y);
for (jj = 0; jj < m; ++jj) {
Y = a[jj][j];
Z = a[jj][i];
a[jj][j] = (Y * C) + (Z * S);
a[jj][i] = -(Y * S) + (Z * C);
}
}
rv1[l] = 0.0;
rv1[k] = F;
w[k] = X;
}
}
return 1;
}
GLOBAL void
array_tqli(array *d, array *e, array *z) {
int n=d->nr_of_elements,m,l,iter,i,k;
DATATYPE s,r,p,g,f,dd,c,b,hold;
for (i=1;i<n;i++) {
e->current_element=i;
hold=READ_ELEMENT(e);
e->current_element=i-1;
WRITE_ELEMENT(e, hold);
}
e->current_element=n-1;
WRITE_ELEMENT(e, 0.0);
for (l=0;l<n;l++) {
iter=0;
do {
for (m=l;m<n-1;m++) {
/*{{{}}}*/
/*{{{ */
d->current_element=e->current_element=m;
hold=fabs(READ_ELEMENT(d));
d->current_element=m+1;
dd=hold+fabs(READ_ELEMENT(d));
if ((DATATYPE)(fabs(READ_ELEMENT(e))+dd) == dd) break;
/*}}} */
}
if (m != l) {
/*{{{ */
if (iter++ == 30) nrerror("Too many iterations in tqli");
d->current_element=l+1;
hold=READ_ELEMENT(d);
d->current_element=e->current_element=l;
g=(hold-READ_ELEMENT(d))/(2.0*READ_ELEMENT(e));
r=pythag(g,1.0);
hold=READ_ELEMENT(d);
d->current_element=m;
g=READ_ELEMENT(d)-hold+READ_ELEMENT(e)/(g+SIGN(r,g));
s=c=1.0;
p=0.0;
for (i=m-1;i>=l;i--) {
e->current_element=i;
hold=READ_ELEMENT(e);
f=s*hold;
b=c*hold;
e->current_element=d->current_element=i+1;
WRITE_ELEMENT(e, r=pythag(f,g));
if (r == 0.0) {
WRITE_ELEMENT(d, READ_ELEMENT(d)-p);
e->current_element=m;
WRITE_ELEMENT(e, 0.0);
break;
}
s=f/r;
c=g/r;
g=READ_ELEMENT(d)-p;
d->current_element=i;
r=(READ_ELEMENT(d)-g)*s+2.0*c*b;
d->current_element=i+1;
WRITE_ELEMENT(d, g+(p=s*r));
g=c*r-b;
for (k=0;k<n;k++) {
z->current_element=k;
z->current_vector=i+1;
f=READ_ELEMENT(z);
z->current_vector=i;
hold=READ_ELEMENT(z);
z->current_vector=i+1;
WRITE_ELEMENT(z, s*hold+c*f);
z->current_vector=i;
WRITE_ELEMENT(z, c*hold-s*f);
}
}
if (r == 0.0 && i >= l) continue;
d->current_element=e->current_element=l;
WRITE_ELEMENT(d, READ_ELEMENT(d)-p);
WRITE_ELEMENT(e, g);
e->current_element=m;
WRITE_ELEMENT(e, 0.0);
/*}}} */
}
} while (m != l);
}
}
static void
icvSVD_32f( float* a, int lda, int m, int n,
float* w,
float* uT, int lduT, int nu,
float* vT, int ldvT,
float* buffer )
{
float* e;
float* temp;
float *w1, *e1;
float *hv;
double ku0 = 0, kv0 = 0;
double anorm = 0;
float *a1, *u0 = uT, *v0 = vT;
double scale, h;
int i, j, k, l;
int nm, m1, n1;
int nv = n;
int iters = 0;
float* hv0 = (float*)cvStackAlloc( (m+2)*sizeof(hv0[0])) + 1;
e = buffer;
w1 = w;
e1 = e + 1;
nm = n;
temp = buffer + nm;
memset( w, 0, nm * sizeof( w[0] ));
memset( e, 0, nm * sizeof( e[0] ));
m1 = m;
n1 = n;
/* transform a to bi-diagonal form */
for( ;; )
{
int update_u;
int update_v;
if( m1 == 0 )
break;
scale = h = 0;
update_u = uT && m1 > m - nu;
hv = update_u ? uT : hv0;
for( j = 0, a1 = a; j < m1; j++, a1 += lda )
{
double t = a1[0];
scale += fabs( hv[j] = (float)t );
}
if( scale != 0 )
{
double f = 1./scale, g, s = 0;
for( j = 0; j < m1; j++ )
{
double t = (hv[j] = (float)(hv[j]*f));
s += t * t;
}
g = sqrt( s );
f = hv[0];
if( f >= 0 )
g = -g;
hv[0] = (float)(f - g);
h = 1. / (f * g - s);
memset( temp, 0, n1 * sizeof( temp[0] ));
/* calc temp[0:n-i] = a[i:m,i:n]'*hv[0:m-i] */
icvMatrAXPY_32f( m1, n1 - 1, a + 1, lda, hv, temp + 1, 0 );
for( k = 1; k < n1; k++ ) temp[k] = (float)(temp[k]*h);
/* modify a: a[i:m,i:n] = a[i:m,i:n] + hv[0:m-i]*temp[0:n-i]' */
icvMatrAXPY_32f( m1, n1 - 1, temp + 1, 0, hv, a + 1, lda );
*w1 = (float)(g*scale);
}
w1++;
/* store -2/(hv'*hv) */
if( update_u )
{
if( m1 == m )
ku0 = h;
else
hv[-1] = (float)h;
}
a++;
n1--;
if( vT )
vT += ldvT + 1;
if( n1 == 0 )
break;
scale = h = 0;
update_v = vT && n1 > n - nv;
hv = update_v ? vT : hv0;
for( j = 0; j < n1; j++ )
{
double t = a[j];
scale += fabs( hv[j] = (float)t );
}
if( scale != 0 )
{
double f = 1./scale, g, s = 0;
for( j = 0; j < n1; j++ )
{
double t = (hv[j] = (float)(hv[j]*f));
s += t * t;
}
g = sqrt( s );
f = hv[0];
if( f >= 0 )
g = -g;
hv[0] = (float)(f - g);
h = 1. / (f * g - s);
hv[-1] = 0.f;
/* update a[i:m:i+1:n] = a[i:m,i+1:n] + (a[i:m,i+1:n]*hv[0:m-i])*... */
icvMatrAXPY3_32f( m1, n1, hv, lda, a, h );
*e1 = (float)(g*scale);
}
e1++;
/* store -2/(hv'*hv) */
if( update_v )
{
if( n1 == n )
kv0 = h;
else
hv[-1] = (float)h;
}
a += lda;
m1--;
if( uT )
uT += lduT + 1;
}
m1 -= m1 != 0;
n1 -= n1 != 0;
/* accumulate left transformations */
if( uT )
{
m1 = m - m1;
uT = u0 + m1 * lduT;
for( i = m1; i < nu; i++, uT += lduT )
{
memset( uT + m1, 0, (m - m1) * sizeof( uT[0] ));
uT[i] = 1.;
}
for( i = m1 - 1; i >= 0; i-- )
{
double s;
int lh = nu - i;
l = m - i;
hv = u0 + (lduT + 1) * i;
h = i == 0 ? ku0 : hv[-1];
assert( h <= 0 );
if( h != 0 )
{
uT = hv;
icvMatrAXPY3_32f( lh, l-1, hv+1, lduT, uT+1, h );
s = hv[0] * h;
for( k = 0; k < l; k++ ) hv[k] = (float)(hv[k]*s);
hv[0] += 1;
}
else
{
for( j = 1; j < l; j++ )
hv[j] = 0;
for( j = 1; j < lh; j++ )
hv[j * lduT] = 0;
hv[0] = 1;
}
}
uT = u0;
}
/* accumulate right transformations */
if( vT )
{
n1 = n - n1;
vT = v0 + n1 * ldvT;
for( i = n1; i < nv; i++, vT += ldvT )
{
memset( vT + n1, 0, (n - n1) * sizeof( vT[0] ));
vT[i] = 1.;
}
for( i = n1 - 1; i >= 0; i-- )
{
double s;
int lh = nv - i;
l = n - i;
hv = v0 + (ldvT + 1) * i;
h = i == 0 ? kv0 : hv[-1];
assert( h <= 0 );
if( h != 0 )
{
vT = hv;
icvMatrAXPY3_32f( lh, l-1, hv+1, ldvT, vT+1, h );
s = hv[0] * h;
for( k = 0; k < l; k++ ) hv[k] = (float)(hv[k]*s);
hv[0] += 1;
}
else
{
for( j = 1; j < l; j++ )
hv[j] = 0;
for( j = 1; j < lh; j++ )
hv[j * ldvT] = 0;
hv[0] = 1;
}
}
vT = v0;
}
for( i = 0; i < nm; i++ )
{
double tnorm = fabs( w[i] );
tnorm += fabs( e[i] );
if( anorm < tnorm )
anorm = tnorm;
}
anorm *= FLT_EPSILON;
/* diagonalization of the bidiagonal form */
for( k = nm - 1; k >= 0; k-- )
{
double z = 0;
iters = 0;
for( ;; ) /* do iterations */
{
double c, s, f, g, x, y;
int flag = 0;
/* test for splitting */
for( l = k; l >= 0; l-- )
{
if( fabs( e[l] ) <= anorm )
{
flag = 1;
break;
}
assert( l > 0 );
if( fabs( w[l - 1] ) <= anorm )
break;
}
if( !flag )
{
c = 0;
s = 1;
for( i = l; i <= k; i++ )
{
f = s * e[i];
e[i] = (float)(e[i]*c);
if( anorm + fabs( f ) == anorm )
break;
g = w[i];
h = pythag( f, g );
w[i] = (float)h;
c = g / h;
s = -f / h;
if( uT )
icvGivens_32f( m, uT + lduT * (l - 1), uT + lduT * i, c, s );
}
}
z = w[k];
if( l == k || iters++ == MAX_ITERS )
break;
/* shift from bottom 2x2 minor */
x = w[l];
y = w[k - 1];
g = e[k - 1];
h = e[k];
f = 0.5 * (((g + z) / h) * ((g - z) / y) + y / h - h / y);
g = pythag( f, 1 );
if( f < 0 )
g = -g;
f = x - (z / x) * z + (h / x) * (y / (f + g) - h);
/* next QR transformation */
c = s = 1;
for( i = l + 1; i <= k; i++ )
{
g = e[i];
y = w[i];
h = s * g;
g *= c;
z = pythag( f, h );
e[i - 1] = (float)z;
c = f / z;
s = h / z;
f = x * c + g * s;
g = -x * s + g * c;
h = y * s;
y *= c;
if( vT )
icvGivens_32f( n, vT + ldvT * (i - 1), vT + ldvT * i, c, s );
z = pythag( f, h );
w[i - 1] = (float)z;
/* rotation can be arbitrary if z == 0 */
if( z != 0 )
{
c = f / z;
s = h / z;
}
f = c * g + s * y;
x = -s * g + c * y;
if( uT )
icvGivens_32f( m, uT + lduT * (i - 1), uT + lduT * i, c, s );
}
e[l] = 0;
e[k] = (float)f;
w[k] = (float)x;
} /* end of iteration loop */
if( iters > MAX_ITERS )
break;
if( z < 0 )
{
w[k] = (float)(-z);
if( vT )
{
for( j = 0; j < n; j++ )
vT[j + k * ldvT] = -vT[j + k * ldvT];
}
}
} /* end of diagonalization loop */
/* sort singular values and corresponding vectors */
for( i = 0; i < nm; i++ )
{
k = i;
for( j = i + 1; j < nm; j++ )
if( w[k] < w[j] )
k = j;
if( k != i )
{
float t;
CV_SWAP( w[i], w[k], t );
if( vT )
for( j = 0; j < n; j++ )
CV_SWAP( vT[j + ldvT*k], vT[j + ldvT*i], t );
if( uT )
for( j = 0; j < m; j++ )
CV_SWAP( uT[j + lduT*k], uT[j + lduT*i], t );
}
}
}
LOCAL VOID tql2 P6C(int, nm,
int, n,
double *, d,
double *, e,
double *, z,
int *, ierr)
{
/* System generated locals */
double d__1, d__2;
/* Local variables */
double c, f, g, h;
int i, j, k, l, m;
double p, r, s, c2, c3 = 0.0;
int l1, l2;
double s2 = 0.0;
int ii;
double dl1, el1;
int mml;
double tst1, tst2;
/* this subroutine is a translation of the algol procedure tql2, */
/* num. math. 11, 293-306(1968) by bowdler, martin, reinsch, and */
/* wilkinson. */
/* handbook for auto. comp., vol.ii-linear algebra, 227-240(1971). */
/* this subroutine finds the eigenvalues and eigenvectors */
/* of a symmetric tridiagonal matrix by the ql method. */
/* the eigenvectors of a full symmetric matrix can also */
/* be found if tred2 has been used to reduce this */
/* full matrix to tridiagonal form. */
/* on input */
/* nm must be set to the row dimension of two-dimensional */
/* array parameters as declared in the calling program */
/* dimension statement. */
/* n is the order of the matrix. */
/* d contains the diagonal elements of the input matrix. */
/* e contains the subdiagonal elements of the input matrix */
/* in its last n-1 positions. e(1) is arbitrary. */
/* z contains the transformation matrix produced in the */
/* reduction by tred2, if performed. if the eigenvectors */
/* of the tridiagonal matrix are desired, z must contain */
/* the identity matrix. */
/* on output */
/* d contains the eigenvalues in ascending order. if an */
/* error exit is made, the eigenvalues are correct but */
/* unordered for indices 1,2,...,ierr-1. */
/* e has been destroyed. */
/* z contains orthonormal eigenvectors of the symmetric */
/* tridiagonal (or full) matrix. if an error exit is made, */
/* z contains the eigenvectors associated with the stored */
/* eigenvalues. */
/* ierr is set to */
/* zero for normal return, */
/* j if the j-th eigenvalue has not been */
/* determined after 30 iterations. */
/* calls pythag for dsqrt(a*a + b*b) . */
/* questions and comments should be directed to burton s. garbow, */
/* mathematics and computer science div, argonne national laboratory */
/* this version dated august 1983. */
/* ------------------------------------------------------------------ */
/* Parameter adjustments */
z -= nm + 1;
--e;
--d;
/* Function Body */
*ierr = 0;
if (n == 1) {
goto L1001;
}
for (i = 2; i <= n; ++i) {
e[i - 1] = e[i];
}
f = 0.;
tst1 = 0.;
e[n] = 0.;
for (l = 1; l <= n; ++l) {
j = 0;
h = (d__1 = d[l], abs(d__1)) + (d__2 = e[l], abs(d__2));
if (tst1 < h) {
tst1 = h;
}
/* .......... look for small sub-diagonal element .......... */
for (m = l; m <= n; ++m) {
tst2 = tst1 + (d__1 = e[m], abs(d__1));
if (tst2 == tst1) {
goto L120;
}
/* .......... e(n) is always zero, so there is no exit */
/* through the bottom of the loop .......... */
}
L120:
if (m == l) {
goto L220;
}
L130:
if (j == 30) {
goto L1000;
}
++j;
/* .......... form shift .......... */
l1 = l + 1;
l2 = l1 + 1;
g = d[l];
p = (d[l1] - g) / (e[l] * 2.);
r = pythag(p, 1.0);
d[l] = e[l] / (p + d_sign(&r, &p));
d[l1] = e[l] * (p + d_sign(&r, &p));
dl1 = d[l1];
h = g - d[l];
if (l2 > n) {
goto L145;
}
for (i = l2; i <= n; ++i) {
d[i] -= h;
}
L145:
f += h;
/* .......... ql transformation .......... */
p = d[m];
c = 1.;
c2 = c;
el1 = e[l1];
s = 0.;
mml = m - l;
/* .......... for i=m-1 step -1 until l do -- .......... */
for (ii = 1; ii <= mml; ++ii) {
c3 = c2;
c2 = c;
s2 = s;
i = m - ii;
g = c * e[i];
h = c * p;
r = pythag(p, e[i]);
e[i + 1] = s * r;
s = e[i] / r;
c = p / r;
p = c * d[i] - s * g;
d[i + 1] = h + s * (c * g + s * d[i]);
/* .......... form vector .......... */
for (k = 1; k <= n; ++k) {
h = z[k + (i + 1) * nm];
z[k + (i + 1) * nm] = s * z[k + i * nm] + c * h;
z[k + i * nm] = c * z[k + i * nm] - s * h;
}
}
p = -s * s2 * c3 * el1 * e[l] / dl1;
e[l] = s * p;
d[l] = c * p;
tst2 = tst1 + (d__1 = e[l], abs(d__1));
if (tst2 > tst1) {
goto L130;
}
L220:
d[l] += f;
}
/* .......... order eigenvalues and eigenvectors .......... */
for (ii = 2; ii <= n; ++ii) {
i = ii - 1;
k = i;
p = d[i];
for (j = ii; j <= n; ++j) {
if (d[j] >= p) {
goto L260;
}
k = j;
p = d[j];
L260:
;
}
if (k == i) {
goto L300;
}
d[k] = d[i];
d[i] = p;
for (j = 1; j <= n; ++j) {
p = z[j + i * nm];
z[j + i * nm] = z[j + k * nm];
z[j + k * nm] = p;
}
L300:
;
}
goto L1001;
/* .......... set error -- no convergence to an */
/* eigenvalue after 30 iterations .......... */
L1000:
*ierr = l;
L1001:
return;
}
int LinearAlgebra::qtli(vector<double>& d, vector<double>& e, vector<vector<double> >& z) {
try {
int myM, i, iter;
double s, r, p, g, f, dd, c, b;
int n = d.size();
for(int i=1;i<=n;i++){
e[i-1] = e[i];
}
e[n-1] = 0.0000;
for(int l=0;l<n;l++){
iter = 0;
do {
for(myM=l;myM<n-1;myM++){
dd = fabs(d[myM]) + fabs(d[myM+1]);
if(fabs(e[myM])+dd == dd) break;
}
if(myM != l){
if(iter++ == 3000) cerr << "Too many iterations in tqli\n";
g = (d[l+1]-d[l]) / (2.0 * e[l]);
r = pythag(g, 1.0);
g = d[myM] - d[l] + e[l] / (g + SIGN(r,g));
s = c = 1.0;
p = 0.0000;
for(i=myM-1;i>=l;i--){
f = s * e[i];
b = c * e[i];
e[i+1] = (r=pythag(f,g));
if(r==0.0){
d[i+1] -= p;
e[myM] = 0.0000;
break;
}
s = f / r;
c = g / r;
g = d[i+1] - p;
r = (d[i] - g) * s + 2.0 * c * b;
d[i+1] = g + ( p = s * r);
g = c * r - b;
for(int k=0;k<n;k++){
f = z[k][i+1];
z[k][i+1] = s * z[k][i] + c * f;
z[k][i] = c * z[k][i] - s * f;
}
}
if(r == 0.00 && i >= l) continue;
d[l] -= p;
e[l] = g;
e[myM] = 0.0;
}
} while (myM != l);
}
int k;
for(int i=0;i<n;i++){
p=d[k=i];
for(int j=i;j<n;j++){
if(d[j] >= p){
p=d[k=j];
}
}
if(k!=i){
d[k]=d[i];
d[i]=p;
for(int j=0;j<n;j++){
p=z[j][i];
z[j][i] = z[j][k];
z[j][k] = p;
}
}
}
return 0;
}
catch(exception& e) {
m->errorOut(e, "LinearAlgebra", "qtli");
exit(1);
}
}
/**
* Golub-Reinsch SVD.
*/
void
svd_full (const float *a, size_t m, size_t n, float **ou, float **os, float **ov)
{
float eps = 1.e-15;
float tol = 1.e-64 / eps;
int itmax = 50;
int iteration;
int h, i, j, k, l;
float *p = NULL;
float *q = NULL;
float *u = NULL;
float *v = NULL;
float d, e, f, g, s, x, y, z;
l = 0;
g = 0.0;
x = 0.0;
if (m < n)
goto error;
p = mem_alloc (n, sizeof (float));
q = mem_alloc (n, sizeof (float));
u = mem_alloc (m * n, sizeof (float));
v = mem_alloc (n * n, sizeof (float));
if (p == NULL || q == NULL || u == NULL || v == NULL)
goto error;
memcpy (u, a, m * n * sizeof (float));
for (i = 0; i < n; i++) {
p[i] = g;
s = 0.0;
l = i + 1;
for (j = i; j < m; j++)
s += u[j * n + i] * u[j * n + i];
if (s <= tol)
g = 0.0;
else {
f = u[i * n + i];
if (f < 0.0)
g = sqrt (s);
else
g = -sqrt (s);
d = f * g - s;
u[i * n + i] = f - g;
for (j = l; j < n; j++) {
s = 0.0;
for (k = i; k < m; k++)
s += u[k * n + i] * u[k * n + j];
f = s / d;
for (k = i; k < m; k++) {
u[k * n + j] += f * u[k * n + i];
}
}
}
q[i] = g;
s = 0.0;
for (j = l; j < n; j++)
s += u[i * n + j] * u[i * n + j];
if (s <= tol)
g = 0.0;
else {
f = u[i * n + i + 1];
if (f < 0.0)
g = sqrt (s);
else
g = -sqrt (s);
d = f * g - s;
u[i * n + i + 1] = f - g;
for (j = l; j < n; j++)
p[j] = u[i * n + j] / d;
for (j = l; j < m; j++) {
s = 0.0;
for (k = l; k < n; k++)
s += u[j * n + k] * u[i * n + k];
for (k = l; k < n; k++)
u[j * n + k] += s * p[k];
}
}
y = fabs (q[i]) + fabs (p[i]);
if (y > x)
x = y;
}
for (i = n - 1; i > -1; i--) {
if (g != 0.0) {
d = g * u[i * n + i + 1];
for (j = l; j < n; j++)
v[j * n + i] = u[i * n + j] / d;
for (j = l; j < n; j++) {
s = 0.0;
for (k = l; k < n; k++)
s += u[i * n + k] * v[k * n + j];
for (k = l; k < n; k++)
v[k * n + j] += s * v[k * n + i];
}
}
for (j = l; j < n; j++) {
v[i * n + j] = 0.0;
v[j * n + i] = 0.0;
}
v[i * n + i] = 1.0;
g = p[i];
l = i;
}
for (i = n - 1; i > -1; i--) {
l = i + 1;
g = q[i];
for (j = l; j < n; j++)
u[i * n + j] = 0.0;
if (g != 0.0) {
d = u[i * n + i] * g;
for (j = l; j < n; j++) {
s = 0.0;
for (k = l; k < m; k++)
s += u[k * n + i] * u[k * n + j];
f = s / d;
for (k = i; k < m; k++)
u[k * n + j] += f * u[k * n + i];
}
for (j = i; j < m; j++)
u[j * n + i] /= g;
}
else
for (j = i; j < m; j++)
u[j * n + i] = 0.0;
u[i * n + i] += 1.0;
}
eps *= x;
for (k = n - 1; k > -1; k--) {
for (iteration = 0; iteration < itmax; iteration++) {
int conv;
for (l = k; l > -1; l--) {
conv = (fabs (p[l]) <= eps);
if ((conv) || (fabs (q[l - 1]) <= eps))
break;
}
if (!conv) {
e = 0.0;
s = 1.0;
h = l - 1;
for (i = l; i < k + 1; i++) {
f = s * p[i];
p[i] = e * p[i];
if (fabs (f) <= eps)
break;
g = q[i];
d = pythag (f, g);
q[i] = d;
e = g / d;
s = -f / d;
for (j = 0; j < m; j++) {
y = u[j * n + h];
z = u[j * n + i];
u[j * n + h] = y * e + z * s;
u[j * n + i] = -y * s + z * e;
}
}
}
z = q[k];
if (l == k) {
if (z < 0.0) {
q[k] = -z;
for (j = 0; j < n; j++)
v[j * n + k] = -v[j * n + k];
}
break;
}
if (iteration >= itmax - 1)
break;
x = q[l];
y = q[k - 1];
g = p[k - 1];
d = p[k];
f = ((y - z) * (y + z) + (g - d) * (g + d)) / (2.0 * d * y);
g = pythag (f, 1.0);
if (f < 0)
f = ((x - z) * (x + z) + d * (y / (f - g) - d)) / x;
else
f = ((x - z) * (x + z) + d * (y / (f + g) - d)) / x;
e = 1.0;
s = 1.0;
for (i = l + 1; i < k + 1; i++) {
g = p[i];
y = q[i];
d = s * g;
g = e * g;
z = pythag (f, d);
p[i - 1] = z;
e = f / z;
s = d / z;
f = x * e + g * s;
g = -x * s + g * e;
d = y * s;
y = y * e;
for (j = 0; j < n; j++) {
x = v[j * n + i - 1];
z = v[j * n + i];
v[j * n + i - 1] = x * e + z * s;
v[j * n + i] = -x * s + z * e;
}
z = pythag (f, d);
q[i - 1] = z;
e = f / z;
s = d / z;
f = e * g + s * y;
x = -s * g + e * y;
for (j = 0; j < m; j++) {
y = u[j * n + i - 1];
z = u[j * n + i];
u[j * n + i - 1] = y * e + z * s;
u[j * n + i] = -y * s + z * e;
}
}
p[l] = 0.0;
p[k] = f;
q[k] = x;
}
}
goto done;
error:
mem_freenull (q);
mem_freenull (u);
mem_freenull (v);
done:
mem_free (p);
*ou = u;
*os = q;
*ov = v;
}
