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
 193 /*  Get the eigenvalue/eigenvector for a real symmetric matrix (3x3)  */
 194 #if !defined(__WXMAC__) && !defined(__CMDMAC__)
 195 typedef integer        __CLPK_integer;
 196 typedef logical        __CLPK_logical;
 197 typedef real           __CLPK_real;
 198 typedef doublereal     __CLPK_doublereal;
 199 #endif
 200
 201 int
 202 MatrixSymDiagonalize(Mat33 mat, Double *out_values, Vector *out_vectors)
 203 {
 204         __CLPK_integer n, lda, lwork, info;
 205         __CLPK_doublereal a[9], w[3], work[9];
 206         int i, j;
 207         for (i = 0; i < 3; i++) {
 208                 for (j = 0; j < 3; j++) {
 209                         a[i * 3 + j] = mat[i * 3 + j];
 210                 }
 211         }
 212         n = lda = 3;
 213         lwork = 9;
 214         /*  For the meanings of the arguments, consult the LAPACK source;
 215                 http://www.netlib.org/lapack/double/dsyev.f */
 216         dsyev_("V", "U", &n, a, &lda, w, work, &lwork, &info);
 217         if (info == 0) {
 218                 for (i = 0; i < 3; i++) {
 219                         out_values[i] = w[i];
 220                         out_vectors[i].x = a[i * 3];
 221                         out_vectors[i].y = a[i * 3 + 1];
 222                         out_vectors[i].z = a[i * 3 + 2];
 223                 }
 224         }
 225         return info;
 226 }
 227
 228 void
 229 TransformVec(Vector *dst, const Transform tf, const Vector *src)
 230 {
 231         Vector temp;
 232         temp.x = tf[0] * src->x + tf[3] * src->y + tf[6] * src->z + tf[9];
 233         temp.y = tf[1] * src->x + tf[4] * src->y + tf[7] * src->z + tf[10];
 234         temp.z = tf[2] * src->x + tf[5] * src->y + tf[8] * src->z + tf[11];
 235         *dst = temp;
 236 }
 237
 238 void
 239 TransformMul(Transform dst, const Transform src1, const Transform src2)
 240 {
 241         Transform temp;
 242         temp[0] = src1[0] * src2[0] + src1[3] * src2[1] + src1[6] * src2[2];
 243         temp[1] = src1[1] * src2[0] + src1[4] * src2[1] + src1[7] * src2[2];
 244         temp[2] = src1[2] * src2[0] + src1[5] * src2[1] + src1[8] * src2[2];
 245         temp[3] = src1[0] * src2[3] + src1[3] * src2[4] + src1[6] * src2[5];
 246         temp[4] = src1[1] * src2[3] + src1[4] * src2[4] + src1[7] * src2[5];
 247         temp[5] = src1[2] * src2[3] + src1[5] * src2[4] + src1[8] * src2[5];
 248         temp[6] = src1[0] * src2[6] + src1[3] * src2[7] + src1[6] * src2[8];
 249         temp[7] = src1[1] * src2[6] + src1[4] * src2[7] + src1[7] * src2[8];
 250         temp[8] = src1[2] * src2[6] + src1[5] * src2[7] + src1[8] * src2[8];
 251         temp[9] = src1[0] * src2[9] + src1[3] * src2[10] + src1[6] * src2[11] + src1[9];
 252         temp[10] = src1[1] * src2[9] + src1[4] * src2[10] + src1[7] * src2[11] + src1[10];
 253         temp[11] = src1[2] * src2[9] + src1[5] * src2[10] + src1[8] * src2[11] + src1[11];
 254         memmove(dst, temp, sizeof(Transform));
 255 }
 256
 257 Double
 258 TransformDeterminant(const Transform src)
 259 {
 260         return MatrixDeterminant(src);
 261 }
 262
 263 void
 264 TransformTranspose(Transform dst, const Transform src)
 265 {
 266         Double w;
 267         dst[0] = src[0];
 268         dst[4] = src[4];
 269         dst[8] = src[8];
 270         w = src[3]; dst[3] = src[1]; dst[1] = w;
 271         w = src[6]; dst[6] = src[2]; dst[2] = w;
 272         w = src[7]; dst[7] = src[5]; dst[5] = w;
 273         dst[9] = src[9];
 274         dst[10] = src[10];
 275         dst[11] = src[11];
 276 }
 277
 278 int
 279 TransformInvert(Transform dst, const Transform src)
 280 {
 281         Transform temp;
 282         int n = MatrixInvert(temp, src);
 283         if (n == 0) {
 284                 temp[9] = -temp[0] * src[9] - temp[3] * src[10] - temp[6] * src[11];
 285                 temp[10] = -temp[1] * src[9] - temp[4] * src[10] - temp[7] * src[11];
 286                 temp[11] = -temp[2] * src[9] - temp[5] * src[10] - temp[8] * src[11];
 287                 memmove(dst, temp, sizeof(Transform));
 288                 return 0;
 289         } else return n;
 290 }
 291
 292 void
 293 TransformForInversion(Transform dst, const Vector *center)
 294 {
 295         dst[0] = dst[4] = dst[8] = -1.0;
 296         dst[1] = dst[2] = dst[3] = dst[5] = dst[6] = dst[7] = 0.0;
 297         dst[9] = center->x * 2.0;
 298         dst[10] = center->y * 2.0;
 299         dst[11] = center->z * 2.0;
 300 }
 301
 302 int
 303 TransformForReflection(Transform dst, const Vector *axis, const Vector *center)
 304 {
 305         Vector av = *axis;
 306         Double w;
 307         if ((w = VecLength2(av)) < 1e-15)
 308                 return 1;
 309         w = 1.0 / sqrt(w);
 310         VecScaleSelf(av, w);
 311         /*  r' = r - 2 * VecDot(r-c, a) * a; a should be a unit vector */
 312         /*  (x',y',z') = (x,y,z) - 2*((x-cx)*ax + (y-cy)*ay + (z-cz)*az)*(ax,ay,az) */
 313         /*  = (1-2*ax*ax, -2*ax*ay, -2*ax*az)x + (-2*ax*ay, 1-2*ay*ay, -2*ay*az)y
 314          + (-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)  */
 315         dst[0] = 1.0 - 2.0 * av.x * av.x;
 316         dst[1] = dst[3] = -2.0 * av.x * av.y;
 317         dst[2] = dst[6] = -2.0 * av.x * av.z;
 318         dst[4] = 1.0 - 2.0 * av.y * av.y;
 319         dst[5] = dst[7] = -2.0 * av.y * av.z;
 320         dst[8] = 1.0 - 2.0 * av.z * av.z;
 321         w = 2.0 * VecDot(av, *center);
 322         dst[9] = av.x * w;
 323         dst[10] = av.y * w;
 324         dst[11] = av.z * w;
 325         return 0;
 326 }
 327
 328 int
 329 TransformForRotation(Transform dst, const Vector *axis, Double angle, const Vector *center)
 330 {
 331         Transform tf, temp1;
 332         Double w;
 333         Vector av = *axis;
 334         if ((w = VecLength2(av)) < 1e-15)
 335                 return 1;
 336         w = 1.0 / sqrt(w);
 337         VecScaleSelf(av, w);
 338         memset(tf, 0, sizeof(tf));
 339         memset(temp1, 0, sizeof(tf));
 340         tf[0] = tf[4] = tf[8] = 1.0;
 341         tf[1] = tf[2] = tf[3] = tf[5] = tf[6] = tf[7] = 0.0;
 342         tf[9] = -center->x;
 343         tf[10] = -center->y;
 344         tf[11] = -center->z;
 345         MatrixRotation((Double *)temp1, &av, angle);
 346         temp1[9] = center->x;
 347         temp1[10] = center->y;
 348         temp1[11] = center->z;
 349         TransformMul(dst, temp1, tf);
 350         return 0;
 351 }
 352
 353 #pragma mark ==== Array ====
 354
 355 /*  Assign a value to an array. An array is represented by two fields; count and base,
 356  *  where base is a pointer to an array and count is the number of items.
 357  *  The memory block of the array is allocated by 8*item_size. If the index exceeds
 358  *  that limit, then a new memory block is allocated.  */
 359 void *
 360 AssignArray(void *base, Int *count, int item_size, int idx, const void *value)
 361 {
 362         void **bp = (void **)base;
 363         if (*count == 0 || idx / 8 > (*count - 1) / 8) {
 364                 int new_size = (idx / 8 + 1) * 8;
 365                 if (*bp == NULL)
 366                         *bp = calloc(item_size, new_size);
 367                 else
 368                         *bp = realloc(*bp, new_size * item_size);
 369                 if (*bp == NULL)
 370                         return NULL;
 371                 memset((char *)*bp + *count * item_size, 0, (new_size - *count) * item_size);
 372         }
 373         if (idx >= *count)
 374                 *count = idx + 1;
 375         if (value != NULL)
 376                 memcpy((char *)*bp + idx * item_size, value, item_size);
 377         return (char *)*bp + idx * item_size;
 378 }
 379
 380 /*  Allocate a new array. This works consistently with AssignArray().
 381  *  Don't mix calloc()/malloc() with AssignArray(); that causes disasters!
 382  *  (free() is OK though).  */
 383 void *
 384 NewArray(void *base, Int *count, int item_size, int nitems)
 385 {
 386         void **bp = (void *)base;
 387         *bp = NULL;
 388         *count = 0;
 389         return AssignArray(base, count, item_size, nitems - 1, NULL);
 390 }
 391
 392 /*  Insert items to an array.  */
 393 void *
 394 InsertArray(void *base, Int *count, int item_size, int idx, int nitems, const void *value)
 395 {
 396         void **bp = (void *)base;
 397         void *p;
 398         int ocount = *count;
 399         if (nitems <= 0)
 400                 return NULL;
 401         /*  Allocate storage  */
 402         p = AssignArray(base, count, item_size, *count + nitems - 1, NULL);
 403         if (p == NULL)
 404                 return NULL;
 405         /*  Move items if necessary  */
 406         if (idx < ocount)
 407                 memmove((char *)*bp + (idx + nitems) * item_size, (char *)*bp + idx * item_size, (ocount - idx) * item_size);
 408         /*  Copy items  */
 409         if (value != NULL)
 410                 memmove((char *)*bp + idx * item_size, value, nitems * item_size);
 411         else
 412                 memset((char *)*bp + idx * item_size, 0, nitems * item_size);
 413         return (char *)*bp + idx * item_size;
 414 }
 415
 416 void *
 417 DeleteArray(void *base, Int *count, int item_size, int idx, int nitems, void *outValue)
 418 {
 419         void **bp = (void *)base;
 420         if (nitems <= 0 || idx < 0 || idx >= *count)
 421                 return NULL;
 422         if (nitems > *count - idx)
 423                 nitems = *count - idx;
 424         /*  Copy items  */
 425         if (outValue != NULL)
 426                 memmove(outValue, (char *)*bp + idx * item_size, nitems * item_size);
 427         /*  Move items  */
 428         if (idx + nitems < *count)
 429                 memmove((char *)*bp + idx * item_size, (char *)*bp + (idx + nitems) * item_size, (*count - idx - nitems) * item_size);
 430         *count -= nitems;
 431         if (*count == 0) {
 432                 free(*bp);
 433                 *bp = NULL;
 434         }
 435         return NULL;
 436 }
 437
 438 int
 439 ReadLine(char *buf, int size, FILE *stream, int *lineNumber)
 440 {
 441         int i, c;
 442         i = 0;
 443         c = 0;
 444         while (i < size - 1) {
 445                 c = getc(stream);
 446                 if (c == EOF)
 447                         break;
 448                 buf[i++] = c;
 449                 if (c == '\n')
 450                         break;
 451                 else if (c == '\r') {
 452                         c = getc(stream);
 453                         if (c != '\n')
 454                                 ungetc(c, stream);
 455                         c = '\n';
 456                         break;
 457                 }
 458         }
 459         buf[i] = 0;
 460         if (c != '\n' && c != EOF) {
 461                 /*  Skip until the end of line  */
 462                 while (c != '\n' && c != '\r' && c != EOF)
 463                         c = getc(stream);
 464                 if (c == '\r') {
 465                         c = getc(stream);
 466                         if (c != '\n')
 467                                 ungetc(c, stream);
 468                 }
 469         }
 470         if (lineNumber != NULL)
 471                 (*lineNumber)++;
 472         return i;
 473 }
 474
 475 int
 476 ReadFormat(const char *str, const char *fmt,...)
 477 {
 478         va_list ap;
 479         char buf[64];
 480         int c, n, count, len;
 481         Int *ip;
 482         Double *fp;
 483         char *sp;
 484         va_start(ap, fmt);
 485         count = 0;
 486         len = strlen(str);
 487         while (*fmt != 0 && *str != 0) {
 488                 if (isspace(*fmt)) {
 489                         fmt++;
 490                         continue;
 491                 }
 492                 c = tolower(*fmt++);
 493                 if (isdigit(*fmt)) {
 494                         n = strtol(fmt, (char **)&fmt, 0);
 495                         if (n > 63)
 496                                 n = 63;
 497                         else if (n < 1)
 498                                 n = 1;
 499                 } else n = 1;
 500                 if (len < n)
 501                         n = len;
 502                 strncpy(buf, str, n);
 503                 buf[n] = 0;
 504                 str += n;
 505                 len -= n;
 506                 switch (c) {
 507                         case 'i':
 508                                 ip = va_arg(ap, Int *);
 509                                 *ip = atoi(buf);
 510                                 count++;
 511                                 break;
 512                         case 'f':
 513                                 fp = va_arg(ap, Double *);
 514                                 *fp = atof(buf);
 515                                 count++;
 516                                 break;
 517                         case 's':
 518                                 sp = va_arg(ap, char *);
 519                                 sscanf(buf, " %s", sp);
 520                                 count++;
 521                                 break;
 522                         default:
 523                                 break;
 524                 }
 525         }
 526         va_end(ap);
 527         return count;
 528 }