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 #if (defined USE_X86_ASM && !defined DOUBLE)
 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 if(cpu_capabilities & X86_3DNOW_SUPPORT)
 191     vecinvsqrt_3dnow(in,out,n);
 192   else
 193 #endif /* no x86 optimizations */
 194 #ifdef INVSQRT_DONE
 195     for(i=0;i<n;i++) {
 196       x=in[i];
 197       bit_pattern.fval=x;
 198       exp   = EXP_ADDR(bit_pattern.bval);
 199       fract = FRACT_ADDR(bit_pattern.bval);
 200       result.bval=cinvsqrtexptab[exp] | cinvsqrtfracttab[fract];
 201       lu    = result.fval;
 202
 203 #ifdef DOUBLE
 204       y=(half*lu*(three-((x*lu)*lu)));
 205       out[i]=(half*y*(three-((x*y)*y)));
 206 #else
 207       out[i]=(half*lu*(three-((x*lu)*lu)));
 208 #endif
 209     }
 210 #else  /* no gmx invsqrt */
 211     for(i=0;i<n;i++)
 212       out[i]=1.0f/sqrt(in[i]);
 213 #endif /* INVSQRT_DONE */
 214 }
 215
 216 #ifdef SOFTWARE_RECIP
 217 static inline real recip(float x)
 218 {
 219   const real  two=2.0;
 220   t_convert   result,bit_pattern;
 221   unsigned int exp,fract;
 222   float       lu;
 223   real        y;
 224 #ifdef DOUBLE
 225   real        y2;
 226 #endif
 227
 228   bit_pattern.fval=x;
 229   exp   = EXP_ADDR(bit_pattern.bval);
 230   fract = FRACT_ADDR(bit_pattern.bval);
 231   result.bval=crecipexptab[exp] | crecipfracttab[fract];
 232   lu    = result.fval;
 233
 234   y=lu*(two-x*lu);
 235 #ifdef DOUBLE
 236   y2=y*(two-x*y);
 237
 238   return y2;                    /* 6 Flops */
 239 #else
 240   return y;                     /* 3  Flops */
 241 #endif
 242 }
 243 #endif
 244
 245
 246 static inline void vecrecip(real in[],real out[],int n)
 247 {
 248 #ifdef SOFTWARE_RECIP
 249   const real  two=2.0;
 250   t_convert   result,bit_pattern;
 251   unsigned int exp,fract;
 252   float       lu,x;
 253 #ifdef DOUBLE
 254   real        y;
 255 #endif
 256 #endif /* SOFTWARE_RECIP */
 257   int i;
 258
 259 #if (defined USE_X86_ASM && !defined DOUBLE)
 260   if((cpu_capabilities & X86_SSE_SUPPORT) && !((unsigned long int)in & 0x1f) && !((unsigned long int)out & 0x1f)) /* SSE data must be cache aligned */
 261     vecrecip_sse(in,out,n);
 262   else if(cpu_capabilities & X86_3DNOW_SUPPORT)
 263     vecrecip_3dnow(in,out,n);
 264   else
 265 #endif /* no x86 optimizations */
 266 #ifdef SOFTWARE_RECIP
 267     for(i=0;i<n;i++) {
 268       x=in[i];
 269       bit_pattern.fval=x;
 270       exp   = EXP_ADDR(bit_pattern.bval);
 271       fract = FRACT_ADDR(bit_pattern.bval);
 272       result.bval=crecipexptab[exp] | crecipfracttab[fract];
 273       lu    = result.fval;
 274
 275 #ifdef DOUBLE
 276       y=lu*(two-x*lu);
 277       out[i]=y*(two-x*y);
 278 #else
 279       out[i]=lu*(two-x*lu);
 280 #endif
 281     }
 282 #else /* No gmx recip */
 283     for(i=0;i<n;i++)
 284       out[i]=1.0f/(in[i]);
 285 #endif /* SOFTWARE_RECIP */
 286 }
 287
 288 static inline real sqr(real x)
 289 {
 290   return (x*x);
 291 }
 292
 293 static inline void rvec_add(const rvec a,const rvec b,rvec c)
 294 {
 295   real x,y,z;
 296
 297   x=a[XX]+b[XX];
 298   y=a[YY]+b[YY];
 299   z=a[ZZ]+b[ZZ];
 300
 301   c[XX]=x;
 302   c[YY]=y;
 303   c[ZZ]=z;
 304 }
 305
 306 static inline void rvec_inc(rvec a,rvec b)
 307 {
 308   real x,y,z;
 309
 310   x=a[XX]+b[XX];
 311   y=a[YY]+b[YY];
 312   z=a[ZZ]+b[ZZ];
 313
 314   a[XX]=x;
 315   a[YY]=y;
 316   a[ZZ]=z;
 317 }
 318
 319 static inline void rvec_sub(const rvec a,const rvec b,rvec c)
 320 {
 321   real x,y,z;
 322
 323   x=a[XX]-b[XX];
 324   y=a[YY]-b[YY];
 325   z=a[ZZ]-b[ZZ];
 326
 327   c[XX]=x;
 328   c[YY]=y;
 329   c[ZZ]=z;
 330 }
 331
 332 static inline void rvec_dec(rvec a,rvec b)
 333 {
 334   real x,y,z;
 335
 336   x=a[XX]-b[XX];
 337   y=a[YY]-b[YY];
 338   z=a[ZZ]-b[ZZ];
 339
 340   a[XX]=x;
 341   a[YY]=y;
 342   a[ZZ]=z;
 343 }
 344
 345 static inline void copy_rvec(const rvec a,rvec b)
 346 {
 347   b[XX]=a[XX];
 348   b[YY]=a[YY];
 349   b[ZZ]=a[ZZ];
 350 }
 351
 352 static inline void copy_ivec(const ivec a,ivec b)
 353 {
 354   b[XX]=a[XX];
 355   b[YY]=a[YY];
 356   b[ZZ]=a[ZZ];
 357 }
 358
 359 static inline void ivec_sub(const ivec a,const ivec b,ivec c)
 360 {
 361   int x,y,z;
 362
 363   x=a[XX]-b[XX];
 364   y=a[YY]-b[YY];
 365   z=a[ZZ]-b[ZZ];
 366
 367   c[XX]=x;
 368   c[YY]=y;
 369   c[ZZ]=z;
 370 }
 371
 372 static inline void copy_mat(matrix a,matrix b)
 373 {
 374   copy_rvec(a[XX],b[XX]);
 375   copy_rvec(a[YY],b[YY]);
 376   copy_rvec(a[ZZ],b[ZZ]);
 377 }
 378
 379 static inline void svmul(real a,rvec v1,rvec v2)
 380 {
 381   v2[XX]=a*v1[XX];
 382   v2[YY]=a*v1[YY];
 383   v2[ZZ]=a*v1[ZZ];
 384 }
 385
 386 static inline real distance2(rvec v1, rvec v2)
 387 {
 388   return sqr(v2[XX]-v1[XX]) + sqr(v2[YY]-v1[YY]) + sqr(v2[ZZ]-v1[ZZ]);
 389 }
 390
 391 static inline void clear_rvec(rvec a)
 392 {
 393   /* The ibm compiler has problems with inlining this
 394    * when we use a const real variable
 395    */
 396   a[XX]=0.0;
 397   a[YY]=0.0;
 398   a[ZZ]=0.0;
 399 }
 400
 401 static inline void clear_rvecs(int n,rvec v[])
 402 {
 403   int i;
 404
 405   for(i=0; (i<n); i++)
 406     clear_rvec(v[i]);
 407 }
 408
 409 static inline void clear_mat(matrix a)
 410 {
 411   const real nul=0.0;
 412
 413   a[XX][XX]=a[XX][YY]=a[XX][ZZ]=nul;
 414   a[YY][XX]=a[YY][YY]=a[YY][ZZ]=nul;
 415   a[ZZ][XX]=a[ZZ][YY]=a[ZZ][ZZ]=nul;
 416 }
 417
 418 static inline real iprod(rvec a,rvec b)
 419 {
 420   return (a[XX]*b[XX]+a[YY]*b[YY]+a[ZZ]*b[ZZ]);
 421 }
 422
 423 static inline real iiprod(ivec a,ivec b)
 424 {
 425   return (a[XX]*b[XX]+a[YY]*b[YY]+a[ZZ]*b[ZZ]);
 426 }
 427
 428 static inline real norm2(rvec a)
 429 {
 430   return a[XX]*a[XX]+a[YY]*a[YY]+a[ZZ]*a[ZZ];
 431 }
 432
 433 static inline real norm(rvec a)
 434 {
 435   return sqrt(a[XX]*a[XX]+a[YY]*a[YY]+a[ZZ]*a[ZZ]);
 436 }
 437
 438 static inline real cos_angle(rvec a,rvec b)
 439 {
 440   /*
 441    *                  ax*bx + ay*by + az*bz
 442    * cos-vec (a,b) =  ---------------------
 443    *                      ||a|| * ||b||
 444    */
 445   real   cos;
 446   int    m;
 447   double aa,bb,ip,ipa,ipb; /* For accuracy these must be double! */
 448
 449   ip=ipa=ipb=0;
 450   for(m=0; (m<DIM); m++) {              /* 18           */
 451     aa   = a[m];
 452     bb   = b[m];
 453     ip  += aa*bb;
 454     ipa += aa*aa;
 455     ipb += bb*bb;
 456   }
 457   cos=ip*invsqrt(ipa*ipb);              /*  7           */
 458                                         /* 25 TOTAL     */
 459   if (cos > 1.0)
 460     return  1.0;
 461   if (cos <-1.0)
 462     return -1.0;
 463
 464   return cos;
 465 }
 466
 467 static inline real cos_angle_no_table(rvec a,rvec b)
 468 {
 469   /* This version does not need the invsqrt lookup table */
 470   real   cos;
 471   int    m;
 472   double aa,bb,ip,ipa,ipb; /* For accuracy these must be double! */
 473
 474   ip=ipa=ipb=0;
 475   for(m=0; (m<DIM); m++) {              /* 18           */
 476     aa   = a[m];
 477     bb   = b[m];
 478     ip  += aa*bb;
 479     ipa += aa*aa;
 480     ipb += bb*bb;
 481   }
 482   cos=ip/sqrt(ipa*ipb);                 /* 12           */
 483                                         /* 30 TOTAL     */
 484   if (cos > 1.0)
 485     return  1.0;
 486   if (cos <-1.0)
 487     return -1.0;
 488
 489   return cos;
 490 }
 491
 492 static inline void oprod(rvec a,rvec b,rvec c)
 493 {
 494   c[XX]=a[YY]*b[ZZ]-a[ZZ]*b[YY];
 495   c[YY]=a[ZZ]*b[XX]-a[XX]*b[ZZ];
 496   c[ZZ]=a[XX]*b[YY]-a[YY]*b[XX];
 497 }
 498
 499 static inline void mmul(matrix a,matrix b,matrix dest)
 500 {
 501   dest[XX][XX]=a[XX][XX]*b[XX][XX]+a[XX][YY]*b[YY][XX]+a[XX][ZZ]*b[ZZ][XX];
 502   dest[YY][XX]=a[YY][XX]*b[XX][XX]+a[YY][YY]*b[YY][XX]+a[YY][ZZ]*b[ZZ][XX];
 503   dest[ZZ][XX]=a[ZZ][XX]*b[XX][XX]+a[ZZ][YY]*b[YY][XX]+a[ZZ][ZZ]*b[ZZ][XX];
 504   dest[XX][YY]=a[XX][XX]*b[XX][YY]+a[XX][YY]*b[YY][YY]+a[XX][ZZ]*b[ZZ][YY];
 505   dest[YY][YY]=a[YY][XX]*b[XX][YY]+a[YY][YY]*b[YY][YY]+a[YY][ZZ]*b[ZZ][YY];
 506   dest[ZZ][YY]=a[ZZ][XX]*b[XX][YY]+a[ZZ][YY]*b[YY][YY]+a[ZZ][ZZ]*b[ZZ][YY];
 507   dest[XX][ZZ]=a[XX][XX]*b[XX][ZZ]+a[XX][YY]*b[YY][ZZ]+a[XX][ZZ]*b[ZZ][ZZ];
 508   dest[YY][ZZ]=a[YY][XX]*b[XX][ZZ]+a[YY][YY]*b[YY][ZZ]+a[YY][ZZ]*b[ZZ][ZZ];
 509   dest[ZZ][ZZ]=a[ZZ][XX]*b[XX][ZZ]+a[ZZ][YY]*b[YY][ZZ]+a[ZZ][ZZ]*b[ZZ][ZZ];
 510 }
 511
 512 static inline void transpose(matrix src,matrix dest)
 513 {
 514   dest[XX][XX]=src[XX][XX];
 515   dest[YY][XX]=src[XX][YY];
 516   dest[ZZ][XX]=src[XX][ZZ];
 517   dest[XX][YY]=src[YY][XX];
 518   dest[YY][YY]=src[YY][YY];
 519   dest[ZZ][YY]=src[YY][ZZ];
 520   dest[XX][ZZ]=src[ZZ][XX];
 521   dest[YY][ZZ]=src[ZZ][YY];
 522   dest[ZZ][ZZ]=src[ZZ][ZZ];
 523 }
 524
 525 static inline void tmmul(matrix a,matrix b,matrix dest)
 526 {
 527   /* Computes dest=mmul(transpose(a),b,dest) - used in do_pr_pcoupl */
 528   dest[XX][XX]=a[XX][XX]*b[XX][XX]+a[YY][XX]*b[YY][XX]+a[ZZ][XX]*b[ZZ][XX];
 529   dest[XX][YY]=a[XX][XX]*b[XX][YY]+a[YY][XX]*b[YY][YY]+a[ZZ][XX]*b[ZZ][YY];
 530   dest[XX][ZZ]=a[XX][XX]*b[XX][ZZ]+a[YY][XX]*b[YY][ZZ]+a[ZZ][XX]*b[ZZ][ZZ];
 531   dest[YY][XX]=a[XX][YY]*b[XX][XX]+a[YY][YY]*b[YY][XX]+a[ZZ][YY]*b[ZZ][XX];
 532   dest[YY][YY]=a[XX][YY]*b[XX][YY]+a[YY][YY]*b[YY][YY]+a[ZZ][YY]*b[ZZ][YY];
 533   dest[YY][ZZ]=a[XX][YY]*b[XX][ZZ]+a[YY][YY]*b[YY][ZZ]+a[ZZ][YY]*b[ZZ][ZZ];
 534   dest[ZZ][XX]=a[XX][ZZ]*b[XX][XX]+a[YY][ZZ]*b[YY][XX]+a[ZZ][ZZ]*b[ZZ][XX];
 535   dest[ZZ][YY]=a[XX][ZZ]*b[XX][YY]+a[YY][ZZ]*b[YY][YY]+a[ZZ][ZZ]*b[ZZ][YY];
 536   dest[ZZ][ZZ]=a[XX][ZZ]*b[XX][ZZ]+a[YY][ZZ]*b[YY][ZZ]+a[ZZ][ZZ]*b[ZZ][ZZ];
 537 }
 538
 539 static inline void mtmul(matrix a,matrix b,matrix dest)
 540 {
 541   /* Computes dest=mmul(a,transpose(b),dest) - used in do_pr_pcoupl */
 542   dest[XX][XX]=a[XX][XX]*b[XX][XX]+a[XX][YY]*b[XX][YY]+a[XX][ZZ]*b[XX][ZZ];
 543   dest[XX][YY]=a[XX][XX]*b[YY][XX]+a[XX][YY]*b[YY][YY]+a[XX][ZZ]*b[YY][ZZ];
 544   dest[XX][ZZ]=a[XX][XX]*b[ZZ][XX]+a[XX][YY]*b[ZZ][YY]+a[XX][ZZ]*b[ZZ][ZZ];
 545   dest[YY][XX]=a[YY][XX]*b[XX][XX]+a[YY][YY]*b[XX][YY]+a[YY][ZZ]*b[XX][ZZ];
 546   dest[YY][YY]=a[YY][XX]*b[YY][XX]+a[YY][YY]*b[YY][YY]+a[YY][ZZ]*b[YY][ZZ];
 547   dest[YY][ZZ]=a[YY][XX]*b[ZZ][XX]+a[YY][YY]*b[ZZ][YY]+a[YY][ZZ]*b[ZZ][ZZ];
 548   dest[ZZ][XX]=a[ZZ][XX]*b[XX][XX]+a[ZZ][YY]*b[XX][YY]+a[ZZ][ZZ]*b[XX][ZZ];
 549   dest[ZZ][YY]=a[ZZ][XX]*b[YY][XX]+a[ZZ][YY]*b[YY][YY]+a[ZZ][ZZ]*b[YY][ZZ];
 550   dest[ZZ][ZZ]=a[ZZ][XX]*b[ZZ][XX]+a[ZZ][YY]*b[ZZ][YY]+a[ZZ][ZZ]*b[ZZ][ZZ];
 551 }
 552
 553 static inline real det(matrix a)
 554 {
 555   return ( a[XX][XX]*(a[YY][YY]*a[ZZ][ZZ]-a[ZZ][YY]*a[YY][ZZ])
 556           -a[YY][XX]*(a[XX][YY]*a[ZZ][ZZ]-a[ZZ][YY]*a[XX][ZZ])
 557           +a[ZZ][XX]*(a[XX][YY]*a[YY][ZZ]-a[YY][YY]*a[XX][ZZ]));
 558 }
 559
 560 static inline void m_add(matrix a,matrix b,matrix dest)
 561 {
 562   dest[XX][XX]=a[XX][XX]+b[XX][XX];
 563   dest[XX][YY]=a[XX][YY]+b[XX][YY];
 564   dest[XX][ZZ]=a[XX][ZZ]+b[XX][ZZ];
 565   dest[YY][XX]=a[YY][XX]+b[YY][XX];
 566   dest[YY][YY]=a[YY][YY]+b[YY][YY];
 567   dest[YY][ZZ]=a[YY][ZZ]+b[YY][ZZ];
 568   dest[ZZ][XX]=a[ZZ][XX]+b[ZZ][XX];
 569   dest[ZZ][YY]=a[ZZ][YY]+b[ZZ][YY];
 570   dest[ZZ][ZZ]=a[ZZ][ZZ]+b[ZZ][ZZ];
 571 }
 572
 573 static inline void m_sub(matrix a,matrix b,matrix dest)
 574 {
 575   dest[XX][XX]=a[XX][XX]-b[XX][XX];
 576   dest[XX][YY]=a[XX][YY]-b[XX][YY];
 577   dest[XX][ZZ]=a[XX][ZZ]-b[XX][ZZ];
 578   dest[YY][XX]=a[YY][XX]-b[YY][XX];
 579   dest[YY][YY]=a[YY][YY]-b[YY][YY];
 580   dest[YY][ZZ]=a[YY][ZZ]-b[YY][ZZ];
 581   dest[ZZ][XX]=a[ZZ][XX]-b[ZZ][XX];
 582   dest[ZZ][YY]=a[ZZ][YY]-b[ZZ][YY];
 583   dest[ZZ][ZZ]=a[ZZ][ZZ]-b[ZZ][ZZ];
 584 }
 585
 586 static inline void msmul(matrix m1,real r1,matrix dest)
 587 {
 588   dest[XX][XX]=r1*m1[XX][XX];
 589   dest[XX][YY]=r1*m1[XX][YY];
 590   dest[XX][ZZ]=r1*m1[XX][ZZ];
 591   dest[YY][XX]=r1*m1[YY][XX];
 592   dest[YY][YY]=r1*m1[YY][YY];
 593   dest[YY][ZZ]=r1*m1[YY][ZZ];
 594   dest[ZZ][XX]=r1*m1[ZZ][XX];
 595   dest[ZZ][YY]=r1*m1[ZZ][YY];
 596   dest[ZZ][ZZ]=r1*m1[ZZ][ZZ];
 597 }
 598
 599 static inline void m_inv(matrix src,matrix dest)
 600 {
 601   const real smallreal = 1.0e-18;
 602   const real largereal = 1.0e18;
 603   real  deter,c,fc;
 604
 605   deter=det(src);
 606   c=1.0/deter;
 607   fc=fabs(c);
 608
 609   if ((fc <= smallreal) || (fc >= largereal))
 610     fatal_error(0,"Determinant = %f",1.0/c);
 611
 612   dest[XX][XX]= c*(src[YY][YY]*src[ZZ][ZZ]-src[ZZ][YY]*src[YY][ZZ]);
 613   dest[XX][YY]=-c*(src[XX][YY]*src[ZZ][ZZ]-src[ZZ][YY]*src[XX][ZZ]);
 614   dest[XX][ZZ]= c*(src[XX][YY]*src[YY][ZZ]-src[YY][YY]*src[XX][ZZ]);
 615   dest[YY][XX]=-c*(src[YY][XX]*src[ZZ][ZZ]-src[ZZ][XX]*src[YY][ZZ]);
 616   dest[YY][YY]= c*(src[XX][XX]*src[ZZ][ZZ]-src[ZZ][XX]*src[XX][ZZ]);
 617   dest[YY][ZZ]=-c*(src[XX][XX]*src[YY][ZZ]-src[YY][XX]*src[XX][ZZ]);
 618   dest[ZZ][XX]= c*(src[YY][XX]*src[ZZ][YY]-src[ZZ][XX]*src[YY][YY]);
 619   dest[ZZ][YY]=-c*(src[XX][XX]*src[ZZ][YY]-src[ZZ][XX]*src[XX][YY]);
 620   dest[ZZ][ZZ]= c*(src[XX][XX]*src[YY][YY]-src[YY][XX]*src[XX][YY]);
 621 }
 622
 623 static inline void mvmul(matrix a,rvec src,rvec dest)
 624 {
 625   dest[XX]=a[XX][XX]*src[XX]+a[XX][YY]*src[YY]+a[XX][ZZ]*src[ZZ];
 626   dest[YY]=a[YY][XX]*src[XX]+a[YY][YY]*src[YY]+a[YY][ZZ]*src[ZZ];
 627   dest[ZZ]=a[ZZ][XX]*src[XX]+a[ZZ][YY]*src[YY]+a[ZZ][ZZ]*src[ZZ];
 628 }
 629
 630 static inline void unitv(rvec src,rvec dest)
 631 {
 632   real linv;
 633
 634   linv=invsqrt(norm2(src));
 635   dest[XX]=linv*src[XX];
 636   dest[YY]=linv*src[YY];
 637   dest[ZZ]=linv*src[ZZ];
 638 }
 639
 640 static inline void unitv_no_table(rvec src,rvec dest)
 641 {
 642   real linv;
 643
 644   linv=1.0/sqrt(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 real trace(matrix m)
 651 {
 652   return (m[XX][XX]+m[YY][YY]+m[ZZ][ZZ]);
 653 }
 654
 655 static inline real _divide(real a,real b,char *file,int line)
 656 {
 657   if (b == 0.0)
 658     fatal_error(0,"Dividing by zero, file %s, line %d",file,line);
 659   return a/b;
 660 }
 661
 662 static inline int _mod(int a,int b,char *file,int line)
 663 {
 664   if (b == 0)
 665     fatal_error(0,"Modulo zero, file %s, line %d",file,line);
 666   return a % b;
 667 }
 668
 669 #define divide(a,b) _divide((a),(b),__FILE__,__LINE__)
 670 #define mod(a,b)    _mod((a),(b),__FILE__,__LINE__)
 671
 672 #endif  /* _vec_h */
 673
 674