MolLib/Types.c

   1 /*
   2  *  Types.c
   3  *
   4  *  Created by Toshi Nagata on 06/03/11.
   5  *  Copyright 2006-2008 Toshi Nagata. All rights reserved.
   6  *
   7  This program is free software; you can redistribute it and/or modify
   8  it under the terms of the GNU General Public License as published by
   9  the Free Software Foundation version 2 of the License.
  10
  11  This program is distributed in the hope that it will be useful,
  12  but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  GNU General Public License for more details.
  15  */
  16
  17 #include "Types.h"
  18 #include <string.h>
  19 #include <stdarg.h>
  20 #include <ctype.h>
  21
  22 static void
  23 defaultPanic(const char *fmt, ...)
  24 {
  25         va_list ap;
  26         va_start(ap, fmt);
  27         vfprintf(stderr, fmt, ap);
  28         abort();
  29 }
  30
  31 static void
  32 defaultWarning(const char *fmt, ...)
  33 {
  34         va_list ap;
  35         va_start(ap, fmt);
  36         vfprintf(stderr, fmt, ap);
  37 }
  38
  39 void (*gPanicFunc)(const char *, ...) = defaultPanic;
  40 void (*gWarningFunc)(const char *, ...) = defaultWarning;
  41
  42 void
  43 SetPanicFunc(void (*func)(const char *, ...))
  44 {
  45         if (func == NULL)
  46                 gPanicFunc = defaultPanic;
  47         else gPanicFunc = func;
  48 }
  49
  50 void
  51 SetWarningFunc(void (*func)(const char *, ...))
  52 {
  53         if (func == NULL)
  54                 gWarningFunc = defaultWarning;
  55         else gWarningFunc = func;
  56 }
  57
  58 void
  59 PanicByOutOfMemory(const char *msg)
  60 {
  61         Panic("Out of memory while %s", msg);
  62 }
  63
  64 void
  65 PanicByInternalError(const char *msg, const char *file, int line)
  66 {
  67         Panic("Internal Error in %s; File %s line %d", msg, file, line);
  68 }
  69
  70
  71 #pragma mark ==== Vector and Matrix ====
  72
  73 int
  74 NormalizeVec(Vector *vdst, const Vector *vsrc)
  75 {
  76         double dval;
  77         dval = 1.0 / VecLength(*vsrc);
  78         VecScale(*vdst, *vsrc, dval);
  79         if (!isfinite(vdst->x) || !isfinite(vdst->y) || !isfinite(vdst->z)) {
  80                 return 1;
  81         }
  82         return 0;
  83 }
  84
  85 void
  86 MatrixRotation(Mat33 dst, const Vector *axis, Double angle)
  87 {
  88         /*  Taken from "Matrix and Quaternion FAQ"
  89                 http://www.j3d.org/matrix_faq/matrfaq_latest.html  */
  90         double rcos = cos((double)angle);
  91         double rsin = sin((double)angle);
  92         dst[0] =            rcos + axis->x*axis->x*(1-rcos);
  93         dst[1] = -axis->z * rsin + axis->x*axis->y*(1-rcos);
  94         dst[2] =  axis->y * rsin + axis->x*axis->z*(1-rcos);
  95         dst[3] =  axis->z * rsin + axis->y*axis->x*(1-rcos);
  96         dst[4] =            rcos + axis->y*axis->y*(1-rcos);
  97         dst[5] = -axis->x * rsin + axis->y*axis->z*(1-rcos);
  98         dst[6] = -axis->y * rsin + axis->z*axis->x*(1-rcos);
  99         dst[7] =  axis->x * rsin + axis->z*axis->y*(1-rcos);
 100         dst[8] =            rcos + axis->z*axis->z*(1-rcos);
 101 }
 102
 103 void
 104 MatrixVec(Vector *dst, const Mat33 mat, const Vector *src)
 105 {
 106         Vector temp;
 107         temp.x = mat[0] * src->x + mat[3] * src->y + mat[6] * src->z;
 108         temp.y = mat[1] * src->x + mat[4] * src->y + mat[7] * src->z;
 109         temp.z = mat[2] * src->x + mat[5] * src->y + mat[8] * src->z;
 110         *dst = temp;
 111 }
 112
 113 void
 114 MatrixMul(Mat33 dst, const Mat33 src1, const Mat33 src2)
 115 {
 116         Mat33 temp;
 117         temp[0] = src1[0] * src2[0] + src1[3] * src2[1] + src1[6] * src2[2];
 118         temp[1] = src1[1] * src2[0] + src1[4] * src2[1] + src1[7] * src2[2];
 119         temp[2] = src1[2] * src2[0] + src1[5] * src2[1] + src1[8] * src2[2];
 120         temp[3] = src1[0] * src2[3] + src1[3] * src2[4] + src1[6] * src2[5];
 121         temp[4] = src1[1] * src2[3] + src1[4] * src2[4] + src1[7] * src2[5];
 122         temp[5] = src1[2] * src2[3] + src1[5] * src2[4] + src1[8] * src2[5];
 123         temp[6] = src1[0] * src2[6] + src1[3] * src2[7] + src1[6] * src2[8];
 124         temp[7] = src1[1] * src2[6] + src1[4] * src2[7] + src1[7] * src2[8];
 125         temp[8] = src1[2] * src2[6] + src1[5] * src2[7] + src1[8] * src2[8];
 126         memmove(dst, temp, sizeof(Mat33));
 127 }
 128
 129 void
 130 MatrixTranspose(Mat33 dst, const Mat33 src)
 131 {
 132         Double w;
 133         dst[0] = src[0];
 134         dst[4] = src[4];
 135         dst[8] = src[8];
 136         w = src[3]; dst[3] = src[1]; dst[1] = w;
 137         w = src[6]; dst[6] = src[2]; dst[2] = w;
 138         w = src[7]; dst[7] = src[5]; dst[5] = w;
 139 }
 140
 141 Double
 142 MatrixDeterminant(const Mat33 src)
 143 {
 144         return src[0] * src[4] * src[8] + src[1] * src[5] * src[6] + src[2] * src[3] * src[7]
 145                 - src[0] * src[5] * src[7] - src[1] * src[3] * src[8] - src[2] * src[4] * src[6];
 146 }
 147
 148 int
 149 MatrixInvert(Mat33 dst, const Mat33 src)
 150 {
 151         Mat33 temp;
 152         Double d = MatrixDeterminant(src);
 153         if (d == 0.0)
 154                 return 1;
 155         d = 1.0 / d;
 156         temp[0] = d * ( src[4] * src[8] - src[5] * src[7]);
 157         temp[1] = d * (-src[1] * src[8] + src[2] * src[7]);
 158         temp[2] = d * ( src[1] * src[5] - src[2] * src[4]);
 159         temp[3] = d * (-src[3] * src[8] + src[5] * src[6]);
 160         temp[4] = d * ( src[0] * src[8] - src[2] * src[6]);
 161         temp[5] = d * (-src[0] * src[5] + src[2] * src[3]);
 162         temp[6] = d * ( src[3] * src[7] - src[4] * src[6]);
 163         temp[7] = d * (-src[0] * src[7] + src[1] * src[6]);
 164         temp[8] = d * ( src[0] * src[4] - src[1] * src[3]);
 165         memmove(dst, temp, sizeof(Mat33));
 166         return 0;
 167 }
 168
 169 void
 170 MatrixScale(Mat33 dst, const Mat33 src, Double factor)
 171 {
 172         dst[0] = src[0] * factor;
 173         dst[1] = src[1] * factor;
 174         dst[2] = src[2] * factor;
 175         dst[3] = src[3] * factor;
 176         dst[4] = src[4] * factor;
 177         dst[5] = src[5] * factor;
 178         dst[6] = src[6] * factor;
 179         dst[7] = src[7] * factor;
 180         dst[8] = src[8] * factor;
 181 }
 182
 183 /*  Get the matrix to rotate (1,0,0)->v1, (0,1,0)->v2, (0,0,1)->v3  */
 184 void
 185 MatrixGeneralRotation(Mat33 dst, const Vector *v1, const Vector *v2, const Vector *v3)
 186 {
 187         dst[0] = v1->x; dst[3] = v2->x; dst[6] = v3->x;
 188         dst[1] = v1->y; dst[4] = v2->y; dst[7] = v3->y;
 189         dst[2] = v1->z; dst[5] = v2->z; dst[8] = v3->z;
 190 }
 191
 192 int
 193 MatrixSymDiagonalize(Mat33 mat, Double *out_values, Vector *out_vectors)
 194 {
 195         __CLPK_integer n, lda, lwork, info;
 196         __CLPK_doublereal a[9], w[3], work[9];
 197         int i, j;
 198         for (i = 0; i < 3; i++) {
 199                 for (j = 0; j < 3; j++) {
 200                         a[i * 3 + j] = mat[i * 3 + j];
 201                 }
 202         }
 203         n = lda = 3;
 204         lwork = 9;
 205         /*  For the meanings of the arguments, consult the LAPACK source;
 206                 http://www.netlib.org/lapack/double/dsyev.f */
 207         dsyev_("V", "U", &n, a, &lda, w, work, &lwork, &info);
 208         if (info == 0) {
 209                 for (i = 0; i < 3; i++) {
 210                         out_values[i] = w[i];
 211                         out_vectors[i].x = a[i * 3];
 212                         out_vectors[i].y = a[i * 3 + 1];
 213                         out_vectors[i].z = a[i * 3 + 2];
 214                 }
 215         }
 216         return info;
 217 }
 218
 219 void
 220 TransformVec(Vector *dst, const Transform tf, const Vector *src)
 221 {
 222         Vector temp;
 223         temp.x = tf[0] * src->x + tf[3] * src->y + tf[6] * src->z + tf[9];
 224         temp.y = tf[1] * src->x + tf[4] * src->y + tf[7] * src->z + tf[10];
 225         temp.z = tf[2] * src->x + tf[5] * src->y + tf[8] * src->z + tf[11];
 226         *dst = temp;
 227 }
 228
 229 void
 230 TransformMul(Transform dst, const Transform src1, const Transform src2)
 231 {
 232         Transform temp;
 233         temp[0] = src1[0] * src2[0] + src1[3] * src2[1] + src1[6] * src2[2];
 234         temp[1] = src1[1] * src2[0] + src1[4] * src2[1] + src1[7] * src2[2];
 235         temp[2] = src1[2] * src2[0] + src1[5] * src2[1] + src1[8] * src2[2];
 236         temp[3] = src1[0] * src2[3] + src1[3] * src2[4] + src1[6] * src2[5];
 237         temp[4] = src1[1] * src2[3] + src1[4] * src2[4] + src1[7] * src2[5];
 238         temp[5] = src1[2] * src2[3] + src1[5] * src2[4] + src1[8] * src2[5];
 239         temp[6] = src1[0] * src2[6] + src1[3] * src2[7] + src1[6] * src2[8];
 240         temp[7] = src1[1] * src2[6] + src1[4] * src2[7] + src1[7] * src2[8];
 241         temp[8] = src1[2] * src2[6] + src1[5] * src2[7] + src1[8] * src2[8];
 242         temp[9] = src1[0] * src2[9] + src1[3] * src2[10] + src1[6] * src2[11] + src1[9];
 243         temp[10] = src1[1] * src2[9] + src1[4] * src2[10] + src1[7] * src2[11] + src1[10];
 244         temp[11] = src1[2] * src2[9] + src1[5] * src2[10] + src1[8] * src2[11] + src1[11];
 245         memmove(dst, temp, sizeof(Transform));
 246 }
 247
 248 Double
 249 TransformDeterminant(const Transform src)
 250 {
 251         return MatrixDeterminant(src);
 252 }
 253
 254 void
 255 TransformTranspose(Transform dst, const Transform src)
 256 {
 257         Double w;
 258         dst[0] = src[0];
 259         dst[4] = src[4];
 260         dst[8] = src[8];
 261         w = src[3]; dst[3] = src[1]; dst[1] = w;
 262         w = src[6]; dst[6] = src[2]; dst[2] = w;
 263         w = src[7]; dst[7] = src[5]; dst[5] = w;
 264         dst[9] = src[9];
 265         dst[10] = src[10];
 266         dst[11] = src[11];
 267 }
 268
 269 int
 270 TransformInvert(Transform dst, const Transform src)
 271 {
 272         Transform temp;
 273         int n = MatrixInvert(temp, src);
 274         if (n == 0) {
 275                 temp[9] = -temp[0] * src[9] - temp[3] * src[10] - temp[6] * src[11];
 276                 temp[10] = -temp[1] * src[9] - temp[4] * src[10] - temp[7] * src[11];
 277                 temp[11] = -temp[2] * src[9] - temp[5] * src[10] - temp[8] * src[11];
 278                 memmove(dst, temp, sizeof(Transform));
 279                 return 0;
 280         } else return n;
 281 }
 282
 283 void
 284 TransformForInversion(Transform dst, const Vector *center)
 285 {
 286         dst[0] = dst[4] = dst[8] = -1.0;
 287         dst[1] = dst[2] = dst[3] = dst[5] = dst[6] = dst[7] = 0.0;
 288         dst[9] = center->x * 2.0;
 289         dst[10] = center->y * 2.0;
 290         dst[11] = center->z * 2.0;
 291 }
 292
 293 int
 294 TransformForReflection(Transform dst, const Vector *axis, const Vector *center)
 295 {
 296         Vector av = *axis;
 297         Double w;
 298         if ((w = VecLength2(av)) < 1e-15)
 299                 return 1;
 300         w = 1.0 / sqrt(w);
 301         VecScaleSelf(av, w);
 302         /*  r' = r - 2 * VecDot(r-c, a) * a; a should be a unit vector */
 303         /*  (x',y',z') = (x,y,z) - 2*((x-cx)*ax + (y-cy)*ay + (z-cz)*az)*(ax,ay,az) */
 304         /*  = (1-2*ax*ax, -2*ax*ay, -2*ax*az)x + (-2*ax*ay, 1-2*ay*ay, -2*ay*az)y
 305          + (-2*ax*az, -2*ay*az, 1-2*az*az)z + (ax*C, ay*C, az*C); C = 2*(ax*cx+ay*cy+az*cz)  */
 306         dst[0] = 1.0 - 2.0 * av.x * av.x;
 307         dst[1] = dst[3] = -2.0 * av.x * av.y;
 308         dst[2] = dst[6] = -2.0 * av.x * av.z;
 309         dst[4] = 1.0 - 2.0 * av.y * av.y;
 310         dst[5] = dst[7] = -2.0 * av.y * av.z;
 311         dst[8] = 1.0 - 2.0 * av.z * av.z;
 312         w = 2.0 * VecDot(av, *center);
 313         dst[9] = av.x * w;
 314         dst[10] = av.y * w;
 315         dst[11] = av.z * w;
 316         return 0;
 317 }
 318
 319 int
 320 TransformForRotation(Transform dst, const Vector *axis, Double angle, const Vector *center)
 321 {
 322         Transform tf, temp1;
 323         Double w;
 324         Vector av = *axis;
 325         if ((w = VecLength2(av)) < 1e-15)
 326                 return 1;
 327         w = 1.0 / sqrt(w);
 328         VecScaleSelf(av, w);
 329         memset(tf, 0, sizeof(tf));
 330         memset(temp1, 0, sizeof(tf));
 331         tf[0] = tf[4] = tf[8] = 1.0;
 332         tf[1] = tf[2] = tf[3] = tf[5] = tf[6] = tf[7] = 0.0;
 333         tf[9] = -center->x;
 334         tf[10] = -center->y;
 335         tf[11] = -center->z;
 336         MatrixRotation((Double *)temp1, &av, angle);
 337         temp1[9] = center->x;
 338         temp1[10] = center->y;
 339         temp1[11] = center->z;
 340         TransformMul(dst, temp1, tf);
 341         return 0;
 342 }
 343
 344 Transform gIdentityTransform = {1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0};
 345
 346 #pragma mark ==== LAMatrix ====
 347
 348 typedef struct LAMatrixTempRecord {
 349         int size;
 350         LAMatrix *mat;
 351 } LAMatrixTempRecord;
 352
 353 static Int sNTempRecords;
 354 static LAMatrixTempRecord *sTempRecords = NULL;
 355
 356 LAMatrix *
 357 LAMatrixAllocTempMatrix(int row, int column)
 358 {
 359         int i, n;
 360         LAMatrixTempRecord *tp;
 361         /*  Look for already allocated records  */
 362         n = -1;
 363         for (i = 0, tp = sTempRecords; i < sNTempRecords; i++, tp++) {
 364                 if (tp->size >= column * row) {
 365                         tp->size = -tp->size;
 366                         tp->mat->column = column;
 367                         tp->mat->row = row;
 368                         memset(tp->mat->data, 0, sizeof(__CLPK_doublereal) * column * row);
 369                         return tp->mat;
 370                 }
 371                 if (tp->size > 0)
 372                         n = i;  /*  Record the unused entry  */
 373         }
 374         if (n == -1) {
 375                 tp = (LAMatrixTempRecord *)AssignArray(&sTempRecords, &sNTempRecords, sizeof(LAMatrixTempRecord), sNTempRecords, NULL);
 376                 tp->mat = NULL;
 377         } else tp = &sTempRecords[n];
 378         tp->mat = (LAMatrix *)realloc(tp->mat, sizeof(LAMatrix) + sizeof(__CLPK_doublereal) * (column * row - 1));
 379         tp->size = -column * row;
 380         tp->mat->column = column;
 381         tp->mat->row = row;
 382         memset(tp->mat->data, 0, sizeof(__CLPK_doublereal) * column * row);
 383         return tp->mat;
 384 }
 385
 386 void
 387 LAMatrixReleaseTempMatrix(LAMatrix *mat)
 388 {
 389         int i;
 390         LAMatrixTempRecord *tp;
 391         /*  Is this record found in sTempRecords?  */
 392         for (i = 0, tp = sTempRecords; i < sNTempRecords; i++, tp++) {
 393                 if (tp->mat == mat) {
 394                         tp->size = -tp->size;
 395                         return;
 396                 }
 397         }
 398         /*  If not, then just free it  */
 399         free(mat);
 400 }
 401
 402 LAMatrix *
 403 LAMatrixNew(int row, int column)
 404 {
 405         LAMatrix *m = (LAMatrix *)calloc(sizeof(LAMatrix) + sizeof(__CLPK_doublereal) * (column * row - 1), 1);
 406         m->column = column;
 407         m->row = row;
 408         return m;
 409 }
 410
 411 LAMatrix *
 412 LAMatrixResize(LAMatrix *mat, int row, int column)
 413 {
 414         if (mat == NULL || mat->row * mat->column < row * column)
 415                 mat = (LAMatrix *)realloc(mat, sizeof(LAMatrix) + sizeof(__CLPK_doublereal) * (column * row - 1));
 416         mat->column = column;
 417         mat->row = row;
 418         return mat;
 419 }
 420
 421 void
 422 LAMatrixRelease(LAMatrix *mat)
 423 {
 424         if (mat != NULL)
 425                 free(mat);
 426 }
 427
 428 LAMatrix *
 429 LAMatrixNewFromMatrix(const LAMatrix *mat)
 430 {
 431         LAMatrix *m = LAMatrixNew(mat->row, mat->column);
 432         memmove(m->data, mat->data, sizeof(m->data[0]) * mat->row * mat->column);
 433         return m;
 434 }
 435
 436 void
 437 LAMatrixMul(int trans1, int trans2, double scale1, const LAMatrix *mat1, const LAMatrix *mat2, double scale2, LAMatrix *mat3)
 438 {
 439 #if defined(__WXMAC__) || defined(__CMDMAC__)
 440         int m, n, k;
 441         if (trans1) {
 442                 trans1 = CblasTrans;
 443                 m = mat1->column;
 444                 k = mat1->row;
 445         } else {
 446                 trans1 = CblasNoTrans;
 447                 m = mat1->row;
 448                 k = mat1->column;
 449         }
 450         if (trans2) {
 451                 trans2 = CblasTrans;
 452                 n = mat2->row;
 453         } else {
 454                 trans2 = CblasNoTrans;
 455                 n = mat2->column;
 456         }
 457         cblas_dgemm(CblasColMajor, trans1, trans2, m, n, k, scale1, mat1->data, mat1->row, mat2->data, mat2->row, scale2, mat3->data, mat3->row);
 458 #else
 459         char ctrans1, ctrans2;
 460         int m, n, k;
 461         if (trans1) {
 462                 ctrans1 = 'T';
 463                 m = mat1->column;
 464                 k = mat1->row;
 465         } else {
 466                 ctrans1 = 'N';
 467                 m = mat1->row;
 468                 k = mat1->column;
 469         }
 470         if (trans2) {
 471                 ctrans2 = 'T';
 472                 n = mat2->row;
 473         } else {
 474                 ctrans2 = 'N';
 475                 n = mat2->column;
 476         }
 477         dgemm_(&ctrans1, &ctrans2, &m, &n, &k, &scale1, mat1->data, &mat1->row, mat2->data, &mat2->row, &scale2, mat3->data, &mat3->row);
 478 #endif
 479 }
 480
 481 int
 482 LAMatrixInvert(LAMatrix *mat1, const LAMatrix *mat2)
 483 {
 484         LAMatrix *tmat1, *tmat2;
 485         __CLPK_integer m, n, lda, info, lwork;
 486         if (mat2->column != mat2->row || mat1->column * mat1->row < mat2->column * mat2->row)
 487                 return -1;  /*  Wrong dimension  */
 488         lwork = m = n = lda = mat1->column;
 489         tmat1 = LAMatrixAllocTempMatrix(n, n);  /*  For work  */
 490         tmat2 = LAMatrixAllocTempMatrix(n, 1);  /*  For piv   */
 491         if (mat1 != mat2)
 492                 memmove(mat1->data, mat2->data, sizeof(__CLPK_doublereal) * n * n);
 493         dgetrf_(&m, &n, mat1->data, &lda, (__CLPK_integer *)tmat2->data, &info);
 494         if (info == 0)
 495                 dgetri_(&n, mat1->data, &lda, (__CLPK_integer *)tmat2->data, tmat1->data, &lwork, &info);
 496         LAMatrixReleaseTempMatrix(tmat1);
 497         LAMatrixReleaseTempMatrix(tmat2);
 498         mat1->column = mat1->row = m;
 499         return info;
 500 }
 501
 502 Double
 503 LAMatrixDeterminant(const LAMatrix *mat)
 504 {
 505         LAMatrix *tmat1, *tmat2;
 506         Double det = 1.0;
 507         __CLPK_integer m, n, mn, i, lda, info;
 508         lda = m = mat->row;
 509         n = mat->column;
 510         if (m < n)
 511                 mn = m;
 512         else mn = n;
 513         tmat1 = LAMatrixAllocTempMatrix(m, n);
 514         tmat2 = LAMatrixAllocTempMatrix(mn, 1);  /* For piv */
 515         memmove(tmat1->data, mat->data, sizeof(__CLPK_doublereal) * m * n);
 516         dgetrf_(&m, &n, tmat1->data, &lda, (__CLPK_integer *)tmat2->data, &info);
 517         if (info == 0) {
 518                 for (i = 0; i < mn - 1; i++) {
 519                         if (((__CLPK_integer *)tmat2->data)[i] != i + 1)
 520                                 det = -det;
 521                 }
 522                 for (i = 0; i < mn; i++)
 523                         det *= tmat1->data[(m + 1) * i];
 524         } else det = 0.0;
 525         LAMatrixReleaseTempMatrix(tmat1);
 526         LAMatrixReleaseTempMatrix(tmat2);
 527         return det;
 528 }
 529
 530 void
 531 LAMatrixTranspose(LAMatrix *mat1, const LAMatrix *mat2)
 532 {
 533         LAMatrix *mat3;
 534         int i, j;
 535         if (mat1 == mat2) {
 536                 mat3 = LAMatrixAllocTempMatrix(mat2->row, mat2->column);
 537                 memmove(mat3->data, mat2->data, sizeof(__CLPK_doublereal) * mat2->column * mat2->row);
 538         } else mat3 = (LAMatrix *)mat2;
 539         if (mat1->column * mat1->row >= mat3->column * mat3->row) {
 540                 mat1->column = mat3->row;
 541                 mat1->row = mat3->column;
 542                 for (i = 0; i < mat1->column; i++) {
 543                         for (j = 0; j < mat1->row; j++) {
 544                                 mat1->data[i * mat1->row + j] = mat3->data[j * mat1->column + i];
 545                         }
 546                 }
 547         }
 548         if (mat1 == mat2)
 549                 LAMatrixReleaseTempMatrix(mat3);
 550 }
 551
 552 /*  Diagonalize the symmetric matrix  */
 553 /*  eigenValues = (m, 1) Matrix, eigenVectors = (m, m) Matrix (column vectors are the eigenvectors),
 554     mat2 = (m, m) symmetric Matrix  */
 555 int
 556 LAMatrixSymDiagonalize(LAMatrix *eigenValues, LAMatrix *eigenVectors, const LAMatrix *mat)
 557 {
 558         __CLPK_integer n, lda, lwork, info;
 559         __CLPK_doublereal dwork;
 560         LAMatrix *tmat1;
 561         if (mat->column != mat->row || eigenVectors->column * eigenVectors->row < mat->column * mat->row || eigenValues->column * eigenValues->row < mat->column)
 562                 return -1;  /*  Illegal dimension  */
 563         n = lda = mat->column;
 564         memmove(eigenVectors->data, mat->data, sizeof(__CLPK_doublereal) * n * n);
 565         lwork = -1;  /*  workspace query  */
 566         dsyev_("V", "U", &n, eigenVectors->data, &lda, eigenValues->data, &dwork, &lwork, &info);
 567         if (info == 0) {
 568                 lwork = dwork;
 569                 tmat1 = LAMatrixAllocTempMatrix(lwork, 1);
 570                 dsyev_("V", "U", &n, eigenVectors->data, &lda, eigenValues->data, tmat1->data, &lwork, &info);
 571                 LAMatrixReleaseTempMatrix(tmat1);
 572         }
 573         eigenValues->row = n;
 574         eigenValues->column = 1;
 575         eigenVectors->row = eigenVectors->column = n;
 576         return info;
 577 }
 578
 579 /*  Singular value decomposition  */
 580 /*  M = U W Vt, M: (m, n) Matrix, U: (m, m) orthogonal matrix, W: min(m, n) column vector, Vt: (n, n) orthogonal matrix (not V)  */
 581 int
 582 LAMatrixSingularValueDecomposition(LAMatrix *matU, LAMatrix *matW, LAMatrix *matV, const LAMatrix *mat)
 583 {
 584         __CLPK_integer m, n, lda, num, *iwork, lwork, info;
 585         __CLPK_doublereal workSize, *work, *matData;
 586         m = mat->row;
 587         n = mat->column;
 588         lda = m;
 589         num = (m < n ? m : n);
 590     if (matW->row != num || matW->column != 1 || matU->row != m || matU->column != m || matV->row != n || matV->column != n)
 591         return -1;  /*  Illegal dimension  */
 592         matData = (__CLPK_doublereal *)malloc(sizeof(__CLPK_doublereal) * n * m);
 593         memmove(matData, mat->data, sizeof(__CLPK_doublereal) * n * m);
 594         iwork = (__CLPK_integer *)malloc(sizeof(__CLPK_integer) * num * 8);
 595         lwork = -1;
 596         info = 0;
 597         dgesdd_("A", &m, &n, matData, &lda, matW->data, matU->data, &m, matV->data, &n, &workSize, &lwork, iwork, &info);
 598
 599         if (info != 0) {
 600                 free(matData);
 601                 free(iwork);
 602                 return info;
 603         }
 604         lwork = workSize;
 605         work = (__CLPK_doublereal *)malloc(sizeof(__CLPK_doublereal) * lwork);
 606         dgesdd_("A", &m, &n, matData, &lda, matW->data, matU->data, &m, matV->data, &n, work, &lwork, iwork, &info);
 607         free(work);
 608         free(iwork);
 609         free(matData);
 610         return info;
 611 }
 612
 613 #pragma mark ==== Array ====
 614
 615 /*  Assign a value to an array. An array is represented by two fields; count and base,
 616  *  where base is a pointer to an array and count is the number of items.
 617  *  The memory block of the array is allocated by 8*item_size. If the index exceeds
 618  *  that limit, then a new memory block is allocated.  */
 619 void *
 620 AssignArray(void *base, Int *count, int item_size, int idx, const void *value)
 621 {
 622         void **bp = (void **)base;
 623         if (*count == 0 || idx / 8 > (*count - 1) / 8) {
 624                 int new_size = (idx / 8 + 1) * 8;
 625                 if (*bp == NULL)
 626                         *bp = calloc(item_size, new_size);
 627                 else
 628                         *bp = realloc(*bp, new_size * item_size);
 629                 if (*bp == NULL)
 630                         return NULL;
 631                 memset((char *)*bp + *count * item_size, 0, (new_size - *count) * item_size);
 632         }
 633         if (idx >= *count)
 634                 *count = idx + 1;
 635         if (value != NULL)
 636                 memcpy((char *)*bp + idx * item_size, value, item_size);
 637         return (char *)*bp + idx * item_size;
 638 }
 639
 640 /*  Allocate a new array. This works consistently with AssignArray().
 641  *  Don't mix calloc()/malloc() with AssignArray(); that causes disasters!
 642  *  (free() is OK though).  */
 643 void *
 644 NewArray(void *base, Int *count, int item_size, int nitems)
 645 {
 646         void **bp = (void *)base;
 647         *bp = NULL;
 648         *count = 0;
 649         if (nitems > 0)
 650                 return AssignArray(base, count, item_size, nitems - 1, NULL);
 651         else return NULL;
 652 }
 653
 654 /*  Insert items to an array.  */
 655 void *
 656 InsertArray(void *base, Int *count, int item_size, int idx, int nitems, const void *value)
 657 {
 658         void **bp = (void *)base;
 659         void *p;
 660         int ocount = *count;
 661         if (nitems <= 0)
 662                 return NULL;
 663         /*  Allocate storage  */
 664         p = AssignArray(base, count, item_size, *count + nitems - 1, NULL);
 665         if (p == NULL)
 666                 return NULL;
 667         /*  Move items if necessary  */
 668         if (idx < ocount)
 669                 memmove((char *)*bp + (idx + nitems) * item_size, (char *)*bp + idx * item_size, (ocount - idx) * item_size);
 670         /*  Copy items  */
 671         if (value != NULL)
 672                 memmove((char *)*bp + idx * item_size, value, nitems * item_size);
 673         else
 674                 memset((char *)*bp + idx * item_size, 0, nitems * item_size);
 675         return (char *)*bp + idx * item_size;
 676 }
 677
 678 void *
 679 DeleteArray(void *base, Int *count, int item_size, int idx, int nitems, void *outValue)
 680 {
 681         void **bp = (void *)base;
 682         if (nitems <= 0 || idx < 0 || idx >= *count)
 683                 return NULL;
 684         if (nitems > *count - idx)
 685                 nitems = *count - idx;
 686         /*  Copy items  */
 687         if (outValue != NULL)
 688                 memmove(outValue, (char *)*bp + idx * item_size, nitems * item_size);
 689         /*  Move items  */
 690         if (idx + nitems < *count)
 691                 memmove((char *)*bp + idx * item_size, (char *)*bp + (idx + nitems) * item_size, (*count - idx - nitems) * item_size);
 692         *count -= nitems;
 693         if (*count == 0) {
 694                 free(*bp);
 695                 *bp = NULL;
 696         }
 697         return NULL;
 698 }
 699
 700 int
 701 ReadLine(char *buf, int size, FILE *stream, int *lineNumber)
 702 {
 703         int i, c;
 704         i = 0;
 705         c = 0;
 706         while (i < size - 1) {
 707                 c = getc(stream);
 708                 if (c == EOF)
 709                         break;
 710                 buf[i++] = c;
 711                 if (c == '\n')
 712                         break;
 713                 else if (c == '\r') {
 714                         c = getc(stream);
 715                         if (c != '\n')
 716                                 ungetc(c, stream);
 717                         c = '\n';
 718                         buf[i - 1] = c;
 719                         break;
 720                 }
 721         }
 722         buf[i] = 0;
 723         if (c != '\n' && c != EOF) {
 724                 /*  Skip until the end of line  */
 725                 while (c != '\n' && c != '\r' && c != EOF)
 726                         c = getc(stream);
 727                 if (c == '\r') {
 728                         c = getc(stream);
 729                         if (c != '\n')
 730                                 ungetc(c, stream);
 731                 }
 732         }
 733         if (lineNumber != NULL)
 734                 (*lineNumber)++;
 735         return i;
 736 }
 737
 738 int
 739 ReadFormat(const char *str, const char *fmt,...)
 740 {
 741         va_list ap;
 742         char buf[64];
 743         int c, n, count, len;
 744         Int *ip;
 745         Double *fp;
 746         char *sp;
 747         va_start(ap, fmt);
 748         count = 0;
 749         len = strlen(str);
 750         while (*fmt != 0 && *str != 0) {
 751                 if (isspace(*fmt)) {
 752                         fmt++;
 753                         continue;
 754                 }
 755                 c = tolower(*fmt++);
 756                 if (isdigit(*fmt)) {
 757                         n = strtol(fmt, (char **)&fmt, 0);
 758                         if (n > 63)
 759                                 n = 63;
 760                         else if (n < 1)
 761                                 n = 1;
 762                 } else n = 1;
 763                 if (len < n)
 764                         n = len;
 765                 strncpy(buf, str, n);
 766                 buf[n] = 0;
 767                 str += n;
 768                 len -= n;
 769                 switch (c) {
 770                         case 'i':
 771                                 ip = va_arg(ap, Int *);
 772                                 *ip = atoi(buf);
 773                                 count++;
 774                                 break;
 775                         case 'f':
 776                                 fp = va_arg(ap, Double *);
 777                                 *fp = atof(buf);
 778                                 count++;
 779                                 break;
 780                         case 's':
 781                                 sp = va_arg(ap, char *);
 782                                 sscanf(buf, " %s", sp);
 783                                 count++;
 784                                 break;
 785                         default:
 786                                 break;
 787                 }
 788         }
 789         va_end(ap);
 790         return count;
 791 }