include/vec.h

   1 /*
   2  * $Id$
   3  *
   4  *                This source code is part of
   5  *
   6  *                 G   R   O   M   A   C   S
   7  *
   8  *          GROningen MAchine for Chemical Simulations
   9  *
  10  *                        VERSION 3.0
  11  *
  12  * Copyright (c) 1991-2001
  13  * BIOSON Research Institute, Dept. of Biophysical Chemistry
  14  * University of Groningen, The Netherlands
  15  *
  16  * This program is free software; you can redistribute it and/or
  17  * modify it under the terms of the GNU General Public License
  18  * as published by the Free Software Foundation; either version 2
  19  * of the License, or (at your option) any later version.
  20  *
  21  * If you want to redistribute modifications, please consider that
  22  * scientific software is very special. Version control is crucial -
  23  * bugs must be traceable. We will be happy to consider code for
  24  * inclusion in the official distribution, but derived work must not
  25  * be called official GROMACS. Details are found in the README & COPYING
  26  * files - if they are missing, get the official version at www.gromacs.org.
  27  *
  28  * To help us fund GROMACS development, we humbly ask that you cite
  29  * the papers on the package - you can find them in the top README file.
  30  *
  31  * Do check out http://www.gromacs.org , or mail us at gromacs@gromacs.org .
  32  *
  33  * And Hey:
  34  * Giving Russians Opium May Alter Current Situation
  35  */
  36
  37 #ifndef _vec_h
  38 #define _vec_h
  39
  40 static char *SRCID_vec_h = "$Id$";
  41 #ifdef HAVE_CONFIG_H
  42 #include <config.h>
  43 #endif
  44
  45 #ifdef HAVE_IDENT
  46 #ident  "@(#) vec.h 1.8 12/16/92"
  47 #endif /* HAVE_IDENT */
  48
  49 /*
  50   collection of in-line ready operations:
  51
  52   lookup-table optimized scalar operations:
  53   real invsqrt(float x)
  54   void vecinvsqrt(real in[],real out[],int n)
  55   real recip(float x)
  56   void vecrecip(real in[],real out[],int n)
  57   real sqr(real x)
  58
  59   vector operations:
  60   void rvec_add(const rvec a,const rvec b,rvec c)  c = a + b
  61   void rvec_inc(rvec a,rvec b)                     a += b
  62   void rvec_sub(const rvec a,const rvec b,rvec c)  c = a - b
  63   void rvec_dec(rvec a,rvec b)                     a -= b
  64   void copy_rvec(const rvec a,rvec b)              b = a (reals)
  65   void copy_ivec(const ivec a,ivec b)              b = a (integers)
  66   void ivec_sub(const ivec a,const ivec b,ivec c)  c = a - b
  67   void svmul(real a,rvec v1,rvec v2)               v2 = a * v1
  68   void clear_rvec(rvec a)                          a = 0
  69   void clear_rvecs(int n,rvec v[])
  70   real iprod(rvec a,rvec b)                        = a . b (inner product)
  71   real iiprod(ivec a,ivec b)                       = a . b (integers)
  72   real norm2(rvec a)                               = | a |^2 ( = x*y*z )
  73   real norm(rvec a)                                = | a |
  74   void oprod(rvec a,rvec b,rvec c)                 c = a x b (outer product)
  75   void dprod(rvec a,rvec b,rvec c)                 c = a * b (direct product)
  76   real cos_angle(rvec a,rvec b)
  77   real cos_angle_no_table(rvec a,rvec b)
  78   real distance2(rvec v1, rvec v2)                 = | v2 - v1 |^2
  79   void unitv(rvec src,rvec dest)                   dest = src / |src|
  80   void unitv_no_table(rvec src,rvec dest)          dest = src / |src|
  81
  82   matrix (3x3) operations:
  83   void copy_mat(matrix a,matrix b)                 b = a
  84   void clear_mat(matrix a)                         a = 0
  85   void mmul(matrix a,matrix b,matrix dest)         dest = a . b
  86   void transpose(matrix src,matrix dest)           dest = src*
  87   void tmmul(matrix a,matrix b,matrix dest)        dest = a* . b
  88   void mtmul(matrix a,matrix b,matrix dest)        dest = a . b*
  89   real det(matrix a)                               = det(a)
  90   void m_add(matrix a,matrix b,matrix dest)        dest = a + b
  91   void m_sub(matrix a,matrix b,matrix dest)        dest = a - b
  92   void msmul(matrix m1,real r1,matrix dest)        dest = r1 * m1
  93   void m_inv(matrix src,matrix dest)               dest = src^-1
  94   void mvmul(matrix a,rvec src,rvec dest)          dest = a . src
  95   real trace(matrix m)                             = trace(m)
  96 */
  97
  98 #ifdef USE_AXP_ASM
  99 #include "axp_asm.h"
 100 #endif
 101 #include "maths.h"
 102 #include "typedefs.h"
 103 #include "sysstuff.h"
 104 #include "macros.h"
 105 #include "fatal.h"
 106 #include "x86_cpu.h"
 107
 108 #define EXP_LSB         0x00800000
 109 #define EXP_MASK        0x7f800000
 110 #define EXP_SHIFT       23
 111 #define FRACT_MASK      0x007fffff
 112 #define FRACT_SIZE      11              /* significant part of fraction */
 113 #define FRACT_SHIFT     (EXP_SHIFT-FRACT_SIZE)
 114 #define EXP_ADDR(val)   (((val)&EXP_MASK)>>EXP_SHIFT)
 115 #define FRACT_ADDR(val) (((val)&(FRACT_MASK|EXP_LSB))>>FRACT_SHIFT)
 116
 117 #define PR_VEC(a)       a[XX],a[YY],a[ZZ]
 118
 119 #ifdef SOFTWARE_SQRT
 120 extern const unsigned int cinvsqrtexptab[];
 121 extern const unsigned int cinvsqrtfracttab[];
 122 #endif
 123
 124 #ifdef SOFTWARE_RECIP
 125 extern const unsigned int crecipexptab[];
 126 extern const unsigned int crecipfracttab[];
 127 #endif
 128
 129 typedef union
 130 {
 131   unsigned int bval;
 132   float fval;
 133 } t_convert;
 134
 135
 136 #ifdef SOFTWARE_INVSQRT
 137 static inline real invsqrt(float x)
 138 {
 139   const real  half=0.5;
 140   const real  three=3.0;
 141   t_convert   result,bit_pattern;
 142   unsigned int exp,fract;
 143   float       lu;
 144   real        y;
 145 #ifdef DOUBLE
 146   real        y2;
 147 #endif
 148
 149   bit_pattern.fval=x;
 150   exp   = EXP_ADDR(bit_pattern.bval);
 151   fract = FRACT_ADDR(bit_pattern.bval);
 152   result.bval=cinvsqrtexptab[exp] | cinvsqrtfracttab[fract];
 153   lu    = result.fval;
 154
 155   y=(half*lu*(three-((x*lu)*lu)));
 156 #ifdef DOUBLE
 157   y2=(half*y*(three-((x*y)*y)));
 158
 159   return y2;                    /* 10 Flops */
 160 #else
 161   return y;                     /* 5  Flops */
 162 #endif
 163 }
 164 #define INVSQRT_DONE
 165 #endif /* gmx_invsqrt */
 166
 167 #ifndef INVSQRT_DONE
 168 #define invsqrt(x) (1.0f/sqrt(x))
 169 #endif
 170
 171
 172
 173 static inline void vecinvsqrt(real in[],real out[],int n)
 174 {
 175 #ifdef INVSQRT_DONE
 176   const real  half=0.5;
 177   const real  three=3.0;
 178   t_convert   result,bit_pattern;
 179   unsigned int exp,fract;
 180   float       lu,x;
 181 #ifdef DOUBLE
 182   real        y;
 183 #endif
 184 #endif /* INVSQRT_DONE */
 185   int i;
 186
 187 #ifndef DOUBLE
 188 #ifdef USE_3DNOW
 189   if(cpu_capabilities & X86_3DNOW_SUPPORT)
 190     vecinvsqrt_3dnow(in,out,n);
 191   else
 192 #endif
 193 #ifdef USE_SSE
 194   if((cpu_capabilities & X86_SSE_SUPPORT) && !((unsigned long int)in & 0x1f) && !((unsigned long int)out & 0x1f)) /* SSE data must be cache aligned */
 195     vecinvsqrt_sse(in,out,n);
 196   else
 197 #endif /* no x86 optimizations */
 198 #endif /* not double */
 199 #ifdef INVSQRT_DONE
 200     for(i=0;i<n;i++) {
 201       x=in[i];
 202       bit_pattern.fval=x;
 203       exp   = EXP_ADDR(bit_pattern.bval);
 204       fract = FRACT_ADDR(bit_pattern.bval);
 205       result.bval=cinvsqrtexptab[exp] | cinvsqrtfracttab[fract];
 206       lu    = result.fval;
 207
 208 #ifdef DOUBLE
 209       y=(half*lu*(three-((x*lu)*lu)));
 210       out[i]=(half*y*(three-((x*y)*y)));
 211 #else
 212       out[i]=(half*lu*(three-((x*lu)*lu)));
 213 #endif
 214     }
 215 #else  /* no gmx invsqrt */
 216     for(i=0;i<n;i++)
 217       out[i]=1.0f/sqrt(in[i]);
 218 #endif /* INVSQRT_DONE */
 219 }
 220
 221 #ifdef SOFTWARE_RECIP
 222 static inline real recip(float x)
 223 {
 224   const real  two=2.0;
 225   t_convert   result,bit_pattern;
 226   unsigned int exp,fract;
 227   float       lu;
 228   real        y;
 229 #ifdef DOUBLE
 230   real        y2;
 231 #endif
 232
 233   bit_pattern.fval=x;
 234   exp   = EXP_ADDR(bit_pattern.bval);
 235   fract = FRACT_ADDR(bit_pattern.bval);
 236   result.bval=crecipexptab[exp] | crecipfracttab[fract];
 237   lu    = result.fval;
 238
 239   y=lu*(two-x*lu);
 240 #ifdef DOUBLE
 241   y2=y*(two-x*y);
 242
 243   return y2;                    /* 6 Flops */
 244 #else
 245   return y;                     /* 3  Flops */
 246 #endif
 247 }
 248 #endif
 249
 250
 251 static inline void vecrecip(real in[],real out[],int n)
 252 {
 253 #ifdef SOFTWARE_RECIP
 254   const real  two=2.0;
 255   t_convert   result,bit_pattern;
 256   unsigned int exp,fract;
 257   float       lu,x;
 258 #ifdef DOUBLE
 259   real        y;
 260 #endif
 261 #endif /* SOFTWARE_RECIP */
 262   int i;
 263
 264 #ifndef DOUBLE
 265 #ifdef USE_3DNOW
 266   if(cpu_capabilities & X86_3DNOW_SUPPORT)
 267     vecrecip_3dnow(in,out,n);
 268   else
 269 #endif
 270 #ifdef USE_SSE
 271   if((cpu_capabilities & X86_SSE_SUPPORT) && !((unsigned long int)in & 0x1f) && !((unsigned long int)out & 0x1f)) /* SSE data must be cache aligned */
 272     vecrecip_sse(in,out,n);
 273   else
 274 #endif /* no x86 optimizations */
 275 #endif /* not double */
 276 #ifdef SOFTWARE_RECIP
 277     for(i=0;i<n;i++) {
 278       x=in[i];
 279       bit_pattern.fval=x;
 280       exp   = EXP_ADDR(bit_pattern.bval);
 281       fract = FRACT_ADDR(bit_pattern.bval);
 282       result.bval=crecipexptab[exp] | crecipfracttab[fract];
 283       lu    = result.fval;
 284
 285 #ifdef DOUBLE
 286       y=lu*(two-x*lu);
 287       out[i]=y*(two-x*y);
 288 #else
 289       out[i]=lu*(two-x*lu);
 290 #endif
 291     }
 292 #else /* No gmx recip */
 293     for(i=0;i<n;i++)
 294       out[i]=1.0f/(in[i]);
 295 #endif /* SOFTWARE_RECIP */
 296 }
 297
 298 static inline real sqr(real x)
 299 {
 300   return (x*x);
 301 }
 302
 303 static inline void rvec_add(const rvec a,const rvec b,rvec c)
 304 {
 305   real x,y,z;
 306
 307   x=a[XX]+b[XX];
 308   y=a[YY]+b[YY];
 309   z=a[ZZ]+b[ZZ];
 310
 311   c[XX]=x;
 312   c[YY]=y;
 313   c[ZZ]=z;
 314 }
 315
 316 static inline void rvec_inc(rvec a,rvec b)
 317 {
 318   real x,y,z;
 319
 320   x=a[XX]+b[XX];
 321   y=a[YY]+b[YY];
 322   z=a[ZZ]+b[ZZ];
 323
 324   a[XX]=x;
 325   a[YY]=y;
 326   a[ZZ]=z;
 327 }
 328
 329 static inline void rvec_sub(const rvec a,const rvec b,rvec c)
 330 {
 331   real x,y,z;
 332
 333   x=a[XX]-b[XX];
 334   y=a[YY]-b[YY];
 335   z=a[ZZ]-b[ZZ];
 336
 337   c[XX]=x;
 338   c[YY]=y;
 339   c[ZZ]=z;
 340 }
 341
 342 static inline void rvec_dec(rvec a,rvec b)
 343 {
 344   real x,y,z;
 345
 346   x=a[XX]-b[XX];
 347   y=a[YY]-b[YY];
 348   z=a[ZZ]-b[ZZ];
 349
 350   a[XX]=x;
 351   a[YY]=y;
 352   a[ZZ]=z;
 353 }
 354
 355 static inline void copy_rvec(const rvec a,rvec b)
 356 {
 357   b[XX]=a[XX];
 358   b[YY]=a[YY];
 359   b[ZZ]=a[ZZ];
 360 }
 361
 362 static inline void copy_ivec(const ivec a,ivec b)
 363 {
 364   b[XX]=a[XX];
 365   b[YY]=a[YY];
 366   b[ZZ]=a[ZZ];
 367 }
 368
 369 static inline void ivec_sub(const ivec a,const ivec b,ivec c)
 370 {
 371   int x,y,z;
 372
 373   x=a[XX]-b[XX];
 374   y=a[YY]-b[YY];
 375   z=a[ZZ]-b[ZZ];
 376
 377   c[XX]=x;
 378   c[YY]=y;
 379   c[ZZ]=z;
 380 }
 381
 382 static inline void copy_mat(matrix a,matrix b)
 383 {
 384   copy_rvec(a[XX],b[XX]);
 385   copy_rvec(a[YY],b[YY]);
 386   copy_rvec(a[ZZ],b[ZZ]);
 387 }
 388
 389 static inline void svmul(real a,rvec v1,rvec v2)
 390 {
 391   v2[XX]=a*v1[XX];
 392   v2[YY]=a*v1[YY];
 393   v2[ZZ]=a*v1[ZZ];
 394 }
 395
 396 static inline real distance2(rvec v1, rvec v2)
 397 {
 398   return sqr(v2[XX]-v1[XX]) + sqr(v2[YY]-v1[YY]) + sqr(v2[ZZ]-v1[ZZ]);
 399 }
 400
 401 static inline void clear_rvec(rvec a)
 402 {
 403   /* The ibm compiler has problems with inlining this
 404    * when we use a const real variable
 405    */
 406   a[XX]=0.0;
 407   a[YY]=0.0;
 408   a[ZZ]=0.0;
 409 }
 410
 411 static inline void clear_rvecs(int n,rvec v[])
 412 {
 413   int i;
 414
 415   for(i=0; (i<n); i++)
 416     clear_rvec(v[i]);
 417 }
 418
 419 static inline void clear_mat(matrix a)
 420 {
 421   const real nul=0.0;
 422
 423   a[XX][XX]=a[XX][YY]=a[XX][ZZ]=nul;
 424   a[YY][XX]=a[YY][YY]=a[YY][ZZ]=nul;
 425   a[ZZ][XX]=a[ZZ][YY]=a[ZZ][ZZ]=nul;
 426 }
 427
 428 static inline real iprod(rvec a,rvec b)
 429 {
 430   return (a[XX]*b[XX]+a[YY]*b[YY]+a[ZZ]*b[ZZ]);
 431 }
 432
 433 static inline real iiprod(ivec a,ivec b)
 434 {
 435   return (a[XX]*b[XX]+a[YY]*b[YY]+a[ZZ]*b[ZZ]);
 436 }
 437
 438 static inline real norm2(rvec a)
 439 {
 440   return a[XX]*a[XX]+a[YY]*a[YY]+a[ZZ]*a[ZZ];
 441 }
 442
 443 static inline real norm(rvec a)
 444 {
 445   return sqrt(a[XX]*a[XX]+a[YY]*a[YY]+a[ZZ]*a[ZZ]);
 446 }
 447
 448 static inline real cos_angle(rvec a,rvec b)
 449 {
 450   /*
 451    *                  ax*bx + ay*by + az*bz
 452    * cos-vec (a,b) =  ---------------------
 453    *                      ||a|| * ||b||
 454    */
 455   real   cos;
 456   int    m;
 457   double aa,bb,ip,ipa,ipb; /* For accuracy these must be double! */
 458
 459   ip=ipa=ipb=0;
 460   for(m=0; (m<DIM); m++) {              /* 18           */
 461     aa   = a[m];
 462     bb   = b[m];
 463     ip  += aa*bb;
 464     ipa += aa*aa;
 465     ipb += bb*bb;
 466   }
 467   cos=ip*invsqrt(ipa*ipb);              /*  7           */
 468                                         /* 25 TOTAL     */
 469   if (cos > 1.0)
 470     return  1.0;
 471   if (cos <-1.0)
 472     return -1.0;
 473
 474   return cos;
 475 }
 476
 477 static inline real cos_angle_no_table(rvec a,rvec b)
 478 {
 479   /* This version does not need the invsqrt lookup table */
 480   real   cos;
 481   int    m;
 482   double aa,bb,ip,ipa,ipb; /* For accuracy these must be double! */
 483
 484   ip=ipa=ipb=0;
 485   for(m=0; (m<DIM); m++) {              /* 18           */
 486     aa   = a[m];
 487     bb   = b[m];
 488     ip  += aa*bb;
 489     ipa += aa*aa;
 490     ipb += bb*bb;
 491   }
 492   cos=ip/sqrt(ipa*ipb);                 /* 12           */
 493                                         /* 30 TOTAL     */
 494   if (cos > 1.0)
 495     return  1.0;
 496   if (cos <-1.0)
 497     return -1.0;
 498
 499   return cos;
 500 }
 501
 502 static inline void oprod(rvec a,rvec b,rvec c)
 503 {
 504   c[XX]=a[YY]*b[ZZ]-a[ZZ]*b[YY];
 505   c[YY]=a[ZZ]*b[XX]-a[XX]*b[ZZ];
 506   c[ZZ]=a[XX]*b[YY]-a[YY]*b[XX];
 507 }
 508
 509 static inline void mmul(matrix a,matrix b,matrix dest)
 510 {
 511   dest[XX][XX]=a[XX][XX]*b[XX][XX]+a[XX][YY]*b[YY][XX]+a[XX][ZZ]*b[ZZ][XX];
 512   dest[YY][XX]=a[YY][XX]*b[XX][XX]+a[YY][YY]*b[YY][XX]+a[YY][ZZ]*b[ZZ][XX];
 513   dest[ZZ][XX]=a[ZZ][XX]*b[XX][XX]+a[ZZ][YY]*b[YY][XX]+a[ZZ][ZZ]*b[ZZ][XX];
 514   dest[XX][YY]=a[XX][XX]*b[XX][YY]+a[XX][YY]*b[YY][YY]+a[XX][ZZ]*b[ZZ][YY];
 515   dest[YY][YY]=a[YY][XX]*b[XX][YY]+a[YY][YY]*b[YY][YY]+a[YY][ZZ]*b[ZZ][YY];
 516   dest[ZZ][YY]=a[ZZ][XX]*b[XX][YY]+a[ZZ][YY]*b[YY][YY]+a[ZZ][ZZ]*b[ZZ][YY];
 517   dest[XX][ZZ]=a[XX][XX]*b[XX][ZZ]+a[XX][YY]*b[YY][ZZ]+a[XX][ZZ]*b[ZZ][ZZ];
 518   dest[YY][ZZ]=a[YY][XX]*b[XX][ZZ]+a[YY][YY]*b[YY][ZZ]+a[YY][ZZ]*b[ZZ][ZZ];
 519   dest[ZZ][ZZ]=a[ZZ][XX]*b[XX][ZZ]+a[ZZ][YY]*b[YY][ZZ]+a[ZZ][ZZ]*b[ZZ][ZZ];
 520 }
 521
 522 static inline void transpose(matrix src,matrix dest)
 523 {
 524   dest[XX][XX]=src[XX][XX];
 525   dest[YY][XX]=src[XX][YY];
 526   dest[ZZ][XX]=src[XX][ZZ];
 527   dest[XX][YY]=src[YY][XX];
 528   dest[YY][YY]=src[YY][YY];
 529   dest[ZZ][YY]=src[YY][ZZ];
 530   dest[XX][ZZ]=src[ZZ][XX];
 531   dest[YY][ZZ]=src[ZZ][YY];
 532   dest[ZZ][ZZ]=src[ZZ][ZZ];
 533 }
 534
 535 static inline void tmmul(matrix a,matrix b,matrix dest)
 536 {
 537   /* Computes dest=mmul(transpose(a),b,dest) - used in do_pr_pcoupl */
 538   dest[XX][XX]=a[XX][XX]*b[XX][XX]+a[YY][XX]*b[YY][XX]+a[ZZ][XX]*b[ZZ][XX];
 539   dest[XX][YY]=a[XX][XX]*b[XX][YY]+a[YY][XX]*b[YY][YY]+a[ZZ][XX]*b[ZZ][YY];
 540   dest[XX][ZZ]=a[XX][XX]*b[XX][ZZ]+a[YY][XX]*b[YY][ZZ]+a[ZZ][XX]*b[ZZ][ZZ];
 541   dest[YY][XX]=a[XX][YY]*b[XX][XX]+a[YY][YY]*b[YY][XX]+a[ZZ][YY]*b[ZZ][XX];
 542   dest[YY][YY]=a[XX][YY]*b[XX][YY]+a[YY][YY]*b[YY][YY]+a[ZZ][YY]*b[ZZ][YY];
 543   dest[YY][ZZ]=a[XX][YY]*b[XX][ZZ]+a[YY][YY]*b[YY][ZZ]+a[ZZ][YY]*b[ZZ][ZZ];
 544   dest[ZZ][XX]=a[XX][ZZ]*b[XX][XX]+a[YY][ZZ]*b[YY][XX]+a[ZZ][ZZ]*b[ZZ][XX];
 545   dest[ZZ][YY]=a[XX][ZZ]*b[XX][YY]+a[YY][ZZ]*b[YY][YY]+a[ZZ][ZZ]*b[ZZ][YY];
 546   dest[ZZ][ZZ]=a[XX][ZZ]*b[XX][ZZ]+a[YY][ZZ]*b[YY][ZZ]+a[ZZ][ZZ]*b[ZZ][ZZ];
 547 }
 548
 549 static inline void mtmul(matrix a,matrix b,matrix dest)
 550 {
 551   /* Computes dest=mmul(a,transpose(b),dest) - used in do_pr_pcoupl */
 552   dest[XX][XX]=a[XX][XX]*b[XX][XX]+a[XX][YY]*b[XX][YY]+a[XX][ZZ]*b[XX][ZZ];
 553   dest[XX][YY]=a[XX][XX]*b[YY][XX]+a[XX][YY]*b[YY][YY]+a[XX][ZZ]*b[YY][ZZ];
 554   dest[XX][ZZ]=a[XX][XX]*b[ZZ][XX]+a[XX][YY]*b[ZZ][YY]+a[XX][ZZ]*b[ZZ][ZZ];
 555   dest[YY][XX]=a[YY][XX]*b[XX][XX]+a[YY][YY]*b[XX][YY]+a[YY][ZZ]*b[XX][ZZ];
 556   dest[YY][YY]=a[YY][XX]*b[YY][XX]+a[YY][YY]*b[YY][YY]+a[YY][ZZ]*b[YY][ZZ];
 557   dest[YY][ZZ]=a[YY][XX]*b[ZZ][XX]+a[YY][YY]*b[ZZ][YY]+a[YY][ZZ]*b[ZZ][ZZ];
 558   dest[ZZ][XX]=a[ZZ][XX]*b[XX][XX]+a[ZZ][YY]*b[XX][YY]+a[ZZ][ZZ]*b[XX][ZZ];
 559   dest[ZZ][YY]=a[ZZ][XX]*b[YY][XX]+a[ZZ][YY]*b[YY][YY]+a[ZZ][ZZ]*b[YY][ZZ];
 560   dest[ZZ][ZZ]=a[ZZ][XX]*b[ZZ][XX]+a[ZZ][YY]*b[ZZ][YY]+a[ZZ][ZZ]*b[ZZ][ZZ];
 561 }
 562
 563 static inline real det(matrix a)
 564 {
 565   return ( a[XX][XX]*(a[YY][YY]*a[ZZ][ZZ]-a[ZZ][YY]*a[YY][ZZ])
 566           -a[YY][XX]*(a[XX][YY]*a[ZZ][ZZ]-a[ZZ][YY]*a[XX][ZZ])
 567           +a[ZZ][XX]*(a[XX][YY]*a[YY][ZZ]-a[YY][YY]*a[XX][ZZ]));
 568 }
 569
 570 static inline void m_add(matrix a,matrix b,matrix dest)
 571 {
 572   dest[XX][XX]=a[XX][XX]+b[XX][XX];
 573   dest[XX][YY]=a[XX][YY]+b[XX][YY];
 574   dest[XX][ZZ]=a[XX][ZZ]+b[XX][ZZ];
 575   dest[YY][XX]=a[YY][XX]+b[YY][XX];
 576   dest[YY][YY]=a[YY][YY]+b[YY][YY];
 577   dest[YY][ZZ]=a[YY][ZZ]+b[YY][ZZ];
 578   dest[ZZ][XX]=a[ZZ][XX]+b[ZZ][XX];
 579   dest[ZZ][YY]=a[ZZ][YY]+b[ZZ][YY];
 580   dest[ZZ][ZZ]=a[ZZ][ZZ]+b[ZZ][ZZ];
 581 }
 582
 583 static inline void m_sub(matrix a,matrix b,matrix dest)
 584 {
 585   dest[XX][XX]=a[XX][XX]-b[XX][XX];
 586   dest[XX][YY]=a[XX][YY]-b[XX][YY];
 587   dest[XX][ZZ]=a[XX][ZZ]-b[XX][ZZ];
 588   dest[YY][XX]=a[YY][XX]-b[YY][XX];
 589   dest[YY][YY]=a[YY][YY]-b[YY][YY];
 590   dest[YY][ZZ]=a[YY][ZZ]-b[YY][ZZ];
 591   dest[ZZ][XX]=a[ZZ][XX]-b[ZZ][XX];
 592   dest[ZZ][YY]=a[ZZ][YY]-b[ZZ][YY];
 593   dest[ZZ][ZZ]=a[ZZ][ZZ]-b[ZZ][ZZ];
 594 }
 595
 596 static inline void msmul(matrix m1,real r1,matrix dest)
 597 {
 598   dest[XX][XX]=r1*m1[XX][XX];
 599   dest[XX][YY]=r1*m1[XX][YY];
 600   dest[XX][ZZ]=r1*m1[XX][ZZ];
 601   dest[YY][XX]=r1*m1[YY][XX];
 602   dest[YY][YY]=r1*m1[YY][YY];
 603   dest[YY][ZZ]=r1*m1[YY][ZZ];
 604   dest[ZZ][XX]=r1*m1[ZZ][XX];
 605   dest[ZZ][YY]=r1*m1[ZZ][YY];
 606   dest[ZZ][ZZ]=r1*m1[ZZ][ZZ];
 607 }
 608
 609 static inline void m_inv(matrix src,matrix dest)
 610 {
 611   const real smallreal = 1.0e-18;
 612   const real largereal = 1.0e18;
 613   real  deter,c,fc;
 614
 615   deter=det(src);
 616   c=1.0/deter;
 617   fc=fabs(c);
 618
 619   if ((fc <= smallreal) || (fc >= largereal))
 620     fatal_error(0,"Determinant = %f",1.0/c);
 621
 622   dest[XX][XX]= c*(src[YY][YY]*src[ZZ][ZZ]-src[ZZ][YY]*src[YY][ZZ]);
 623   dest[XX][YY]=-c*(src[XX][YY]*src[ZZ][ZZ]-src[ZZ][YY]*src[XX][ZZ]);
 624   dest[XX][ZZ]= c*(src[XX][YY]*src[YY][ZZ]-src[YY][YY]*src[XX][ZZ]);
 625   dest[YY][XX]=-c*(src[YY][XX]*src[ZZ][ZZ]-src[ZZ][XX]*src[YY][ZZ]);
 626   dest[YY][YY]= c*(src[XX][XX]*src[ZZ][ZZ]-src[ZZ][XX]*src[XX][ZZ]);
 627   dest[YY][ZZ]=-c*(src[XX][XX]*src[YY][ZZ]-src[YY][XX]*src[XX][ZZ]);
 628   dest[ZZ][XX]= c*(src[YY][XX]*src[ZZ][YY]-src[ZZ][XX]*src[YY][YY]);
 629   dest[ZZ][YY]=-c*(src[XX][XX]*src[ZZ][YY]-src[ZZ][XX]*src[XX][YY]);
 630   dest[ZZ][ZZ]= c*(src[XX][XX]*src[YY][YY]-src[YY][XX]*src[XX][YY]);
 631 }
 632
 633 static inline void mvmul(matrix a,rvec src,rvec dest)
 634 {
 635   dest[XX]=a[XX][XX]*src[XX]+a[XX][YY]*src[YY]+a[XX][ZZ]*src[ZZ];
 636   dest[YY]=a[YY][XX]*src[XX]+a[YY][YY]*src[YY]+a[YY][ZZ]*src[ZZ];
 637   dest[ZZ]=a[ZZ][XX]*src[XX]+a[ZZ][YY]*src[YY]+a[ZZ][ZZ]*src[ZZ];
 638 }
 639
 640 static inline void unitv(rvec src,rvec dest)
 641 {
 642   real linv;
 643
 644   linv=invsqrt(norm2(src));
 645   dest[XX]=linv*src[XX];
 646   dest[YY]=linv*src[YY];
 647   dest[ZZ]=linv*src[ZZ];
 648 }
 649
 650 static inline void unitv_no_table(rvec src,rvec dest)
 651 {
 652   real linv;
 653
 654   linv=1.0/sqrt(norm2(src));
 655   dest[XX]=linv*src[XX];
 656   dest[YY]=linv*src[YY];
 657   dest[ZZ]=linv*src[ZZ];
 658 }
 659
 660 static inline real trace(matrix m)
 661 {
 662   return (m[XX][XX]+m[YY][YY]+m[ZZ][ZZ]);
 663 }
 664
 665 static inline real _divide(real a,real b,char *file,int line)
 666 {
 667   if (b == 0.0)
 668     fatal_error(0,"Dividing by zero, file %s, line %d",file,line);
 669   return a/b;
 670 }
 671
 672 static inline int _mod(int a,int b,char *file,int line)
 673 {
 674   if (b == 0)
 675     fatal_error(0,"Modulo zero, file %s, line %d",file,line);
 676   return a % b;
 677 }
 678
 679 #define divide(a,b) _divide((a),(b),__FILE__,__LINE__)
 680 #define mod(a,b)    _mod((a),(b),__FILE__,__LINE__)
 681
 682 #endif  /* _vec_h */
 683
 684