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