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.1
  11  * Copyright (c) 1991-2001, University of Groningen, The Netherlands
  12  * This program is free software; you can redistribute it and/or
  13  * modify it under the terms of the GNU General Public License
  14  * as published by the Free Software Foundation; either version 2
  15  * of the License, or (at your option) any later version.
  16  *
  17  * If you want to redistribute modifications, please consider that
  18  * scientific software is very special. Version control is crucial -
  19  * bugs must be traceable. We will be happy to consider code for
  20  * inclusion in the official distribution, but derived work must not
  21  * be called official GROMACS. Details are found in the README & COPYING
  22  * files - if they are missing, get the official version at www.gromacs.org.
  23  *
  24  * To help us fund GROMACS development, we humbly ask that you cite
  25  * the papers on the package - you can find them in the top README file.
  26  *
  27  * For more info, check our website at http://www.gromacs.org
  28  *
  29  * And Hey:
  30  * Getting the Right Output Means no Artefacts in Calculating Stuff
  31  */
  32
  33 #ifndef _vec_h
  34 #define _vec_h
  35
  36 #ifdef HAVE_CONFIG_H
  37 #include <config.h>
  38 #define gmx_inline inline
  39 #else
  40 #ifdef __GNUC__
  41 #define gmx_inline __inline
  42 #else
  43 #define inline
  44 #endif
  45 #endif
  46 /*
  47   collection of in-line ready operations:
  48
  49   lookup-table optimized scalar operations:
  50   real invsqrt(real x)
  51   void vecinvsqrt(real in[],real out[],int n)
  52   void vecrecip(real in[],real out[],int n)
  53   real sqr(real x)
  54
  55   vector operations:
  56   void rvec_add(const rvec a,const rvec b,rvec c)  c = a + b
  57   void rvec_inc(rvec a,rvec b)                     a += b
  58   void ivec_inc(ivec a,ivec b)                     a += b
  59   void rvec_sub(const rvec a,const rvec b,rvec c)  c = a - b
  60   void rvec_dec(rvec a,rvec b)                     a -= b
  61   void copy_rvec(const rvec a,rvec b)              b = a (reals)
  62   void copy_ivec(const ivec a,ivec b)              b = a (integers)
  63   void ivec_sub(const ivec a,const ivec b,ivec c)  c = a - b
  64   void svmul(real a,rvec v1,rvec v2)               v2 = a * v1
  65   void clear_rvec(rvec a)                          a = 0
  66   void clear_ivec(rvec a)                          a = 0
  67   void clear_rvecs(int n,rvec v[])
  68   real iprod(rvec a,rvec b)                        = a . b (inner product)
  69   real iiprod(ivec a,ivec b)                       = a . b (integers)
  70   real norm2(rvec a)                               = | a |^2 ( = x*y*z )
  71   real norm(rvec a)                                = | a |
  72   void oprod(rvec a,rvec b,rvec c)                 c = a x b (outer product)
  73   void dprod(rvec a,rvec b,rvec c)                 c = a * b (direct product)
  74   real cos_angle(rvec a,rvec b)
  75   real cos_angle_no_table(rvec a,rvec b)
  76   real distance2(rvec v1, rvec v2)                 = | v2 - v1 |^2
  77   void unitv(rvec src,rvec dest)                   dest = src / |src|
  78   void unitv_no_table(rvec src,rvec dest)          dest = src / |src|
  79
  80   matrix (3x3) operations:
  81   void copy_mat(matrix a,matrix b)                 b = a
  82   void clear_mat(matrix a)                         a = 0
  83   void mmul(matrix a,matrix b,matrix dest)         dest = a . b
  84   void transpose(matrix src,matrix dest)           dest = src*
  85   void tmmul(matrix a,matrix b,matrix dest)        dest = a* . b
  86   void mtmul(matrix a,matrix b,matrix dest)        dest = a . b*
  87   real det(matrix a)                               = det(a)
  88   void m_add(matrix a,matrix b,matrix dest)        dest = a + b
  89   void m_sub(matrix a,matrix b,matrix dest)        dest = a - b
  90   void msmul(matrix m1,real r1,matrix dest)        dest = r1 * m1
  91   void m_inv(matrix src,matrix dest)               dest = src^-1
  92   void mvmul(matrix a,rvec src,rvec dest)          dest = a . src
  93   real trace(matrix m)                             = trace(m)
  94 */
  95
  96 #include "maths.h"
  97 #include "typedefs.h"
  98 #include "sysstuff.h"
  99 #include "macros.h"
 100 #include "fatal.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_INVSQRT
 114 extern const unsigned int cinvsqrtexptab[];
 115 extern const unsigned int cinvsqrtfracttab[];
 116 #endif
 117
 118
 119 typedef union
 120 {
 121   unsigned int bval;
 122   float fval;
 123 } t_convert;
 124
 125
 126 #ifdef SOFTWARE_INVSQRT
 127 static inline real invsqrt(real x)
 128 {
 129   const real  half=0.5;
 130   const real  three=3.0;
 131   t_convert   result,bit_pattern;
 132   unsigned int exp,fract;
 133   float       lu;
 134   real        y;
 135 #ifdef DOUBLE
 136   real        y2;
 137 #endif
 138
 139   bit_pattern.fval=x;
 140   exp   = EXP_ADDR(bit_pattern.bval);
 141   fract = FRACT_ADDR(bit_pattern.bval);
 142   result.bval=cinvsqrtexptab[exp] | cinvsqrtfracttab[fract];
 143   lu    = result.fval;
 144
 145   y=(half*lu*(three-((x*lu)*lu)));
 146 #ifdef DOUBLE
 147   y2=(half*y*(three-((x*y)*y)));
 148
 149   return y2;                    /* 10 Flops */
 150 #else
 151   return y;                     /* 5  Flops */
 152 #endif
 153 }
 154 #define INVSQRT_DONE
 155 #endif /* gmx_invsqrt */
 156
 157 #ifndef INVSQRT_DONE
 158 #define invsqrt(x) (1.0f/sqrt(x))
 159 #endif
 160
 161
 162
 163
 164
 165 static inline real sqr(real x)
 166 {
 167   return (x*x);
 168 }
 169
 170 extern void vecinvsqrt(real in[],real out[],int n);
 171 /* Perform out[i]=1.0/sqrt(in[i]) for n elements */
 172
 173
 174 extern void vecrecip(real in[],real out[],int n);
 175 /* Perform out[i]=1.0/(in[i]) for n elements */
 176
 177 /* Note: If you need a fast version of vecinvsqrt
 178  * and/or vecrecip, call detectcpu() and run the SSE/3DNow/SSE2/Altivec
 179  * versions if your hardware supports it.
 180  *
 181  * To use those routines, your memory HAS TO BE CACHE-ALIGNED.
 182  * Start by allocating 31 bytes more than you need, put
 183  * this in a temp variable (e.g. _buf, so you can free it later), and
 184  * create your aligned array buf with
 185  *
 186  *  buf=(real *) ( ( (unsigned long int)_buf + 31 ) & (~0x1f) );
 187  */
 188
 189
 190 static inline void rvec_add(const rvec a,const rvec b,rvec c)
 191 {
 192   real x,y,z;
 193
 194   x=a[XX]+b[XX];
 195   y=a[YY]+b[YY];
 196   z=a[ZZ]+b[ZZ];
 197
 198   c[XX]=x;
 199   c[YY]=y;
 200   c[ZZ]=z;
 201 }
 202
 203 static inline void rvec_inc(rvec a,rvec b)
 204 {
 205   real x,y,z;
 206
 207   x=a[XX]+b[XX];
 208   y=a[YY]+b[YY];
 209   z=a[ZZ]+b[ZZ];
 210
 211   a[XX]=x;
 212   a[YY]=y;
 213   a[ZZ]=z;
 214 }
 215
 216 static inline void ivec_inc(ivec a,ivec b)
 217 {
 218   int x,y,z;
 219
 220   x=a[XX]+b[XX];
 221   y=a[YY]+b[YY];
 222   z=a[ZZ]+b[ZZ];
 223
 224   a[XX]=x;
 225   a[YY]=y;
 226   a[ZZ]=z;
 227 }
 228
 229 static inline void rvec_sub(const rvec a,const rvec b,rvec c)
 230 {
 231   real x,y,z;
 232
 233   x=a[XX]-b[XX];
 234   y=a[YY]-b[YY];
 235   z=a[ZZ]-b[ZZ];
 236
 237   c[XX]=x;
 238   c[YY]=y;
 239   c[ZZ]=z;
 240 }
 241
 242 static inline void rvec_dec(rvec a,rvec b)
 243 {
 244   real x,y,z;
 245
 246   x=a[XX]-b[XX];
 247   y=a[YY]-b[YY];
 248   z=a[ZZ]-b[ZZ];
 249
 250   a[XX]=x;
 251   a[YY]=y;
 252   a[ZZ]=z;
 253 }
 254
 255 static inline void copy_rvec(const rvec a,rvec b)
 256 {
 257   b[XX]=a[XX];
 258   b[YY]=a[YY];
 259   b[ZZ]=a[ZZ];
 260 }
 261
 262 static inline void copy_ivec(const ivec a,ivec b)
 263 {
 264   b[XX]=a[XX];
 265   b[YY]=a[YY];
 266   b[ZZ]=a[ZZ];
 267 }
 268
 269 static inline void ivec_sub(const ivec a,const ivec b,ivec c)
 270 {
 271   int x,y,z;
 272
 273   x=a[XX]-b[XX];
 274   y=a[YY]-b[YY];
 275   z=a[ZZ]-b[ZZ];
 276
 277   c[XX]=x;
 278   c[YY]=y;
 279   c[ZZ]=z;
 280 }
 281
 282 static inline void copy_mat(matrix a,matrix b)
 283 {
 284   copy_rvec(a[XX],b[XX]);
 285   copy_rvec(a[YY],b[YY]);
 286   copy_rvec(a[ZZ],b[ZZ]);
 287 }
 288
 289 static inline void svmul(real a,rvec v1,rvec v2)
 290 {
 291   v2[XX]=a*v1[XX];
 292   v2[YY]=a*v1[YY];
 293   v2[ZZ]=a*v1[ZZ];
 294 }
 295
 296 static inline real distance2(rvec v1, rvec v2)
 297 {
 298   return sqr(v2[XX]-v1[XX]) + sqr(v2[YY]-v1[YY]) + sqr(v2[ZZ]-v1[ZZ]);
 299 }
 300
 301 static inline void clear_rvec(rvec a)
 302 {
 303   /* The ibm compiler has problems with inlining this
 304    * when we use a const real variable
 305    */
 306   a[XX]=0.0;
 307   a[YY]=0.0;
 308   a[ZZ]=0.0;
 309 }
 310
 311 static inline void clear_ivec(ivec a)
 312 {
 313   a[XX]=0;
 314   a[YY]=0;
 315   a[ZZ]=0;
 316 }
 317
 318 static inline void clear_rvecs(int n,rvec v[])
 319 {
 320   int i;
 321
 322   for(i=0; (i<n); i++)
 323     clear_rvec(v[i]);
 324 }
 325
 326 static inline void clear_mat(matrix a)
 327 {
 328   const real nul=0.0;
 329
 330   a[XX][XX]=a[XX][YY]=a[XX][ZZ]=nul;
 331   a[YY][XX]=a[YY][YY]=a[YY][ZZ]=nul;
 332   a[ZZ][XX]=a[ZZ][YY]=a[ZZ][ZZ]=nul;
 333 }
 334
 335 static inline real iprod(rvec a,rvec b)
 336 {
 337   return (a[XX]*b[XX]+a[YY]*b[YY]+a[ZZ]*b[ZZ]);
 338 }
 339
 340 static inline real iiprod(ivec a,ivec b)
 341 {
 342   return (a[XX]*b[XX]+a[YY]*b[YY]+a[ZZ]*b[ZZ]);
 343 }
 344
 345 static inline real norm2(rvec a)
 346 {
 347   return a[XX]*a[XX]+a[YY]*a[YY]+a[ZZ]*a[ZZ];
 348 }
 349
 350 static inline real norm(rvec a)
 351 {
 352   return sqrt(a[XX]*a[XX]+a[YY]*a[YY]+a[ZZ]*a[ZZ]);
 353 }
 354
 355 static inline real cos_angle(rvec a,rvec b)
 356 {
 357   /*
 358    *                  ax*bx + ay*by + az*bz
 359    * cos-vec (a,b) =  ---------------------
 360    *                      ||a|| * ||b||
 361    */
 362   real   cos;
 363   int    m;
 364   double aa,bb,ip,ipa,ipb; /* For accuracy these must be double! */
 365
 366   ip=ipa=ipb=0;
 367   for(m=0; (m<DIM); m++) {              /* 18           */
 368     aa   = a[m];
 369     bb   = b[m];
 370     ip  += aa*bb;
 371     ipa += aa*aa;
 372     ipb += bb*bb;
 373   }
 374   cos=ip*invsqrt(ipa*ipb);              /*  7           */
 375                                         /* 25 TOTAL     */
 376   if (cos > 1.0)
 377     return  1.0;
 378   if (cos <-1.0)
 379     return -1.0;
 380
 381   return cos;
 382 }
 383
 384 static inline real cos_angle_no_table(rvec a,rvec b)
 385 {
 386   /* This version does not need the invsqrt lookup table */
 387   real   cos;
 388   int    m;
 389   double aa,bb,ip,ipa,ipb; /* For accuracy these must be double! */
 390
 391   ip=ipa=ipb=0;
 392   for(m=0; (m<DIM); m++) {              /* 18           */
 393     aa   = a[m];
 394     bb   = b[m];
 395     ip  += aa*bb;
 396     ipa += aa*aa;
 397     ipb += bb*bb;
 398   }
 399   cos=ip/sqrt(ipa*ipb);                 /* 12           */
 400                                         /* 30 TOTAL     */
 401   if (cos > 1.0)
 402     return  1.0;
 403   if (cos <-1.0)
 404     return -1.0;
 405
 406   return cos;
 407 }
 408
 409 static inline void oprod(rvec a,rvec b,rvec c)
 410 {
 411   c[XX]=a[YY]*b[ZZ]-a[ZZ]*b[YY];
 412   c[YY]=a[ZZ]*b[XX]-a[XX]*b[ZZ];
 413   c[ZZ]=a[XX]*b[YY]-a[YY]*b[XX];
 414 }
 415
 416 static inline void mmul(matrix a,matrix b,matrix dest)
 417 {
 418   dest[XX][XX]=a[XX][XX]*b[XX][XX]+a[XX][YY]*b[YY][XX]+a[XX][ZZ]*b[ZZ][XX];
 419   dest[YY][XX]=a[YY][XX]*b[XX][XX]+a[YY][YY]*b[YY][XX]+a[YY][ZZ]*b[ZZ][XX];
 420   dest[ZZ][XX]=a[ZZ][XX]*b[XX][XX]+a[ZZ][YY]*b[YY][XX]+a[ZZ][ZZ]*b[ZZ][XX];
 421   dest[XX][YY]=a[XX][XX]*b[XX][YY]+a[XX][YY]*b[YY][YY]+a[XX][ZZ]*b[ZZ][YY];
 422   dest[YY][YY]=a[YY][XX]*b[XX][YY]+a[YY][YY]*b[YY][YY]+a[YY][ZZ]*b[ZZ][YY];
 423   dest[ZZ][YY]=a[ZZ][XX]*b[XX][YY]+a[ZZ][YY]*b[YY][YY]+a[ZZ][ZZ]*b[ZZ][YY];
 424   dest[XX][ZZ]=a[XX][XX]*b[XX][ZZ]+a[XX][YY]*b[YY][ZZ]+a[XX][ZZ]*b[ZZ][ZZ];
 425   dest[YY][ZZ]=a[YY][XX]*b[XX][ZZ]+a[YY][YY]*b[YY][ZZ]+a[YY][ZZ]*b[ZZ][ZZ];
 426   dest[ZZ][ZZ]=a[ZZ][XX]*b[XX][ZZ]+a[ZZ][YY]*b[YY][ZZ]+a[ZZ][ZZ]*b[ZZ][ZZ];
 427 }
 428
 429 static inline void transpose(matrix src,matrix dest)
 430 {
 431   dest[XX][XX]=src[XX][XX];
 432   dest[YY][XX]=src[XX][YY];
 433   dest[ZZ][XX]=src[XX][ZZ];
 434   dest[XX][YY]=src[YY][XX];
 435   dest[YY][YY]=src[YY][YY];
 436   dest[ZZ][YY]=src[YY][ZZ];
 437   dest[XX][ZZ]=src[ZZ][XX];
 438   dest[YY][ZZ]=src[ZZ][YY];
 439   dest[ZZ][ZZ]=src[ZZ][ZZ];
 440 }
 441
 442 static inline void tmmul(matrix a,matrix b,matrix dest)
 443 {
 444   /* Computes dest=mmul(transpose(a),b,dest) - used in do_pr_pcoupl */
 445   dest[XX][XX]=a[XX][XX]*b[XX][XX]+a[YY][XX]*b[YY][XX]+a[ZZ][XX]*b[ZZ][XX];
 446   dest[XX][YY]=a[XX][XX]*b[XX][YY]+a[YY][XX]*b[YY][YY]+a[ZZ][XX]*b[ZZ][YY];
 447   dest[XX][ZZ]=a[XX][XX]*b[XX][ZZ]+a[YY][XX]*b[YY][ZZ]+a[ZZ][XX]*b[ZZ][ZZ];
 448   dest[YY][XX]=a[XX][YY]*b[XX][XX]+a[YY][YY]*b[YY][XX]+a[ZZ][YY]*b[ZZ][XX];
 449   dest[YY][YY]=a[XX][YY]*b[XX][YY]+a[YY][YY]*b[YY][YY]+a[ZZ][YY]*b[ZZ][YY];
 450   dest[YY][ZZ]=a[XX][YY]*b[XX][ZZ]+a[YY][YY]*b[YY][ZZ]+a[ZZ][YY]*b[ZZ][ZZ];
 451   dest[ZZ][XX]=a[XX][ZZ]*b[XX][XX]+a[YY][ZZ]*b[YY][XX]+a[ZZ][ZZ]*b[ZZ][XX];
 452   dest[ZZ][YY]=a[XX][ZZ]*b[XX][YY]+a[YY][ZZ]*b[YY][YY]+a[ZZ][ZZ]*b[ZZ][YY];
 453   dest[ZZ][ZZ]=a[XX][ZZ]*b[XX][ZZ]+a[YY][ZZ]*b[YY][ZZ]+a[ZZ][ZZ]*b[ZZ][ZZ];
 454 }
 455
 456 static inline void mtmul(matrix a,matrix b,matrix dest)
 457 {
 458   /* Computes dest=mmul(a,transpose(b),dest) - used in do_pr_pcoupl */
 459   dest[XX][XX]=a[XX][XX]*b[XX][XX]+a[XX][YY]*b[XX][YY]+a[XX][ZZ]*b[XX][ZZ];
 460   dest[XX][YY]=a[XX][XX]*b[YY][XX]+a[XX][YY]*b[YY][YY]+a[XX][ZZ]*b[YY][ZZ];
 461   dest[XX][ZZ]=a[XX][XX]*b[ZZ][XX]+a[XX][YY]*b[ZZ][YY]+a[XX][ZZ]*b[ZZ][ZZ];
 462   dest[YY][XX]=a[YY][XX]*b[XX][XX]+a[YY][YY]*b[XX][YY]+a[YY][ZZ]*b[XX][ZZ];
 463   dest[YY][YY]=a[YY][XX]*b[YY][XX]+a[YY][YY]*b[YY][YY]+a[YY][ZZ]*b[YY][ZZ];
 464   dest[YY][ZZ]=a[YY][XX]*b[ZZ][XX]+a[YY][YY]*b[ZZ][YY]+a[YY][ZZ]*b[ZZ][ZZ];
 465   dest[ZZ][XX]=a[ZZ][XX]*b[XX][XX]+a[ZZ][YY]*b[XX][YY]+a[ZZ][ZZ]*b[XX][ZZ];
 466   dest[ZZ][YY]=a[ZZ][XX]*b[YY][XX]+a[ZZ][YY]*b[YY][YY]+a[ZZ][ZZ]*b[YY][ZZ];
 467   dest[ZZ][ZZ]=a[ZZ][XX]*b[ZZ][XX]+a[ZZ][YY]*b[ZZ][YY]+a[ZZ][ZZ]*b[ZZ][ZZ];
 468 }
 469
 470 static inline real det(matrix a)
 471 {
 472   return ( a[XX][XX]*(a[YY][YY]*a[ZZ][ZZ]-a[ZZ][YY]*a[YY][ZZ])
 473           -a[YY][XX]*(a[XX][YY]*a[ZZ][ZZ]-a[ZZ][YY]*a[XX][ZZ])
 474           +a[ZZ][XX]*(a[XX][YY]*a[YY][ZZ]-a[YY][YY]*a[XX][ZZ]));
 475 }
 476
 477 static inline void m_add(matrix a,matrix b,matrix dest)
 478 {
 479   dest[XX][XX]=a[XX][XX]+b[XX][XX];
 480   dest[XX][YY]=a[XX][YY]+b[XX][YY];
 481   dest[XX][ZZ]=a[XX][ZZ]+b[XX][ZZ];
 482   dest[YY][XX]=a[YY][XX]+b[YY][XX];
 483   dest[YY][YY]=a[YY][YY]+b[YY][YY];
 484   dest[YY][ZZ]=a[YY][ZZ]+b[YY][ZZ];
 485   dest[ZZ][XX]=a[ZZ][XX]+b[ZZ][XX];
 486   dest[ZZ][YY]=a[ZZ][YY]+b[ZZ][YY];
 487   dest[ZZ][ZZ]=a[ZZ][ZZ]+b[ZZ][ZZ];
 488 }
 489
 490 static inline void m_sub(matrix a,matrix b,matrix dest)
 491 {
 492   dest[XX][XX]=a[XX][XX]-b[XX][XX];
 493   dest[XX][YY]=a[XX][YY]-b[XX][YY];
 494   dest[XX][ZZ]=a[XX][ZZ]-b[XX][ZZ];
 495   dest[YY][XX]=a[YY][XX]-b[YY][XX];
 496   dest[YY][YY]=a[YY][YY]-b[YY][YY];
 497   dest[YY][ZZ]=a[YY][ZZ]-b[YY][ZZ];
 498   dest[ZZ][XX]=a[ZZ][XX]-b[ZZ][XX];
 499   dest[ZZ][YY]=a[ZZ][YY]-b[ZZ][YY];
 500   dest[ZZ][ZZ]=a[ZZ][ZZ]-b[ZZ][ZZ];
 501 }
 502
 503 static inline void msmul(matrix m1,real r1,matrix dest)
 504 {
 505   dest[XX][XX]=r1*m1[XX][XX];
 506   dest[XX][YY]=r1*m1[XX][YY];
 507   dest[XX][ZZ]=r1*m1[XX][ZZ];
 508   dest[YY][XX]=r1*m1[YY][XX];
 509   dest[YY][YY]=r1*m1[YY][YY];
 510   dest[YY][ZZ]=r1*m1[YY][ZZ];
 511   dest[ZZ][XX]=r1*m1[ZZ][XX];
 512   dest[ZZ][YY]=r1*m1[ZZ][YY];
 513   dest[ZZ][ZZ]=r1*m1[ZZ][ZZ];
 514 }
 515
 516 static inline void m_inv(matrix src,matrix dest)
 517 {
 518   const real smallreal = 1.0e-24;
 519   const real largereal = 1.0e24;
 520   real  deter,c,fc;
 521
 522   deter = det(src);
 523   c     = 1.0/deter;
 524   fc    = fabs(c);
 525
 526   if ((fc <= smallreal) || (fc >= largereal))
 527     fatal_error(0,"Determinant = %e",deter);
 528
 529   dest[XX][XX]= c*(src[YY][YY]*src[ZZ][ZZ]-src[ZZ][YY]*src[YY][ZZ]);
 530   dest[XX][YY]=-c*(src[XX][YY]*src[ZZ][ZZ]-src[ZZ][YY]*src[XX][ZZ]);
 531   dest[XX][ZZ]= c*(src[XX][YY]*src[YY][ZZ]-src[YY][YY]*src[XX][ZZ]);
 532   dest[YY][XX]=-c*(src[YY][XX]*src[ZZ][ZZ]-src[ZZ][XX]*src[YY][ZZ]);
 533   dest[YY][YY]= c*(src[XX][XX]*src[ZZ][ZZ]-src[ZZ][XX]*src[XX][ZZ]);
 534   dest[YY][ZZ]=-c*(src[XX][XX]*src[YY][ZZ]-src[YY][XX]*src[XX][ZZ]);
 535   dest[ZZ][XX]= c*(src[YY][XX]*src[ZZ][YY]-src[ZZ][XX]*src[YY][YY]);
 536   dest[ZZ][YY]=-c*(src[XX][XX]*src[ZZ][YY]-src[ZZ][XX]*src[XX][YY]);
 537   dest[ZZ][ZZ]= c*(src[XX][XX]*src[YY][YY]-src[YY][XX]*src[XX][YY]);
 538 }
 539
 540 static inline void mvmul(matrix a,rvec src,rvec dest)
 541 {
 542   dest[XX]=a[XX][XX]*src[XX]+a[XX][YY]*src[YY]+a[XX][ZZ]*src[ZZ];
 543   dest[YY]=a[YY][XX]*src[XX]+a[YY][YY]*src[YY]+a[YY][ZZ]*src[ZZ];
 544   dest[ZZ]=a[ZZ][XX]*src[XX]+a[ZZ][YY]*src[YY]+a[ZZ][ZZ]*src[ZZ];
 545 }
 546
 547 static inline void unitv(rvec src,rvec dest)
 548 {
 549   real linv;
 550
 551   linv=invsqrt(norm2(src));
 552   dest[XX]=linv*src[XX];
 553   dest[YY]=linv*src[YY];
 554   dest[ZZ]=linv*src[ZZ];
 555 }
 556
 557 static inline void unitv_no_table(rvec src,rvec dest)
 558 {
 559   real linv;
 560
 561   linv=1.0/sqrt(norm2(src));
 562   dest[XX]=linv*src[XX];
 563   dest[YY]=linv*src[YY];
 564   dest[ZZ]=linv*src[ZZ];
 565 }
 566
 567 static inline real trace(matrix m)
 568 {
 569   return (m[XX][XX]+m[YY][YY]+m[ZZ][ZZ]);
 570 }
 571
 572 static inline real _divide(real a,real b,char *file,int line)
 573 {
 574   if (b == 0.0)
 575     fatal_error(0,"Dividing by zero, file %s, line %d",file,line);
 576   return a/b;
 577 }
 578
 579 static inline int _mod(int a,int b,char *file,int line)
 580 {
 581   if (b == 0)
 582     fatal_error(0,"Modulo zero, file %s, line %d",file,line);
 583   return a % b;
 584 }
 585
 586 #define divide(a,b) _divide((a),(b),__FILE__,__LINE__)
 587 #define mod(a,b)    _mod((a),(b),__FILE__,__LINE__)
 588
 589 #endif  /* _vec_h */
 590
 591