Project

General

Profile

vec.h

gmx/include/vec.h - David van der Spoel, 11/19/2007 07:19 PM

 
1
/*
2
 * $Id: vec.h,v 1.52.2.5 2007/11/19 18:16:58 spoel Exp $
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.2.0
11
 * Written by David van der Spoel, Erik Lindahl, Berk Hess, and others.
12
 * Copyright (c) 1991-2000, University of Groningen, The Netherlands.
13
 * Copyright (c) 2001-2004, The GROMACS development team,
14
 * check out http://www.gromacs.org for more information.
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
 * For more info, check our website at http://www.gromacs.org
32
 * 
33
 * And Hey:
34
 * Gromacs Runs On Most of All Computer Systems
35
 */
36

    
37
#ifndef _vec_h
38
#define _vec_h
39

    
40
#ifdef HAVE_CONFIG_H
41
#include <config.h>
42
#define gmx_inline inline
43
#else
44
#ifdef __GNUC__
45
#define gmx_inline __inline
46
#else
47
#define inline
48
#endif
49
#endif
50
/*
51
  collection of in-line ready operations:
52
  
53
  lookup-table optimized scalar operations:
54
  real invsqrt(real x)
55
  void vecinvsqrt(real in[],real out[],int n)
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 dvec_add(const dvec a,const dvec b,dvec c)  c = a + b
62
  void ivec_add(const ivec a,const ivec b,ivec c)  c = a + b
63
  void rvec_inc(rvec a,const rvec b)               a += b
64
  void ivec_inc(ivec a,const ivec b)               a += b
65
  void rvec_sub(const rvec a,const rvec b,rvec c)  c = a - b
66
  void dvec_sub(const dvec a,const dvec b,dvec c)  c = a - b
67
  void rvec_dec(rvec a,rvec b)                     a -= b
68
  void copy_rvec(const rvec a,rvec b)              b = a (reals)
69
  void copy_dvec(const dvec a,dvec b)              b = a (reals)
70
  void copy_ivec(const ivec a,ivec b)              b = a (integers)
71
  void ivec_sub(const ivec a,const ivec b,ivec c)  c = a - b
72
  void svmul(real a,rvec v1,rvec v2)               v2 = a * v1
73
  void dsvmul(double a,dvec v1,dvec v2)            v2 = a * v1
74
  void clear_rvec(rvec a)                          a = 0
75
  void clear_dvec(dvec a)                          a = 0
76
  void clear_ivec(rvec a)                          a = 0
77
  void clear_rvecs(int n,rvec v[])
78
  real iprod(rvec a,rvec b)                        = a . b (inner product)
79
  double diprod(dvec a,dvec b)                     = a . b (inner product)
80
  real iiprod(ivec a,ivec b)                       = a . b (integers)
81
  real norm2(rvec a)                               = | a |^2 ( = x*y*z )
82
  real norm(rvec a)                                = | a |
83
  double dnorm(dvec a)                             = | a |
84
  void oprod(rvec a,rvec b,rvec c)                 c = a x b (outer product)
85
  void dprod(rvec a,rvec b,rvec c)                 c = a * b (direct product)
86
  real cos_angle(rvec a,rvec b)
87
  real cos_angle_no_table(rvec a,rvec b)
88
  real distance2(rvec v1, rvec v2)                 = | v2 - v1 |^2
89
  void unitv(rvec src,rvec dest)                   dest = src / |src|
90
  void unitv_no_table(rvec src,rvec dest)          dest = src / |src|
91
  
92
  matrix (3x3) operations:
93
    ! indicates that dest should not be the same as a, b or src
94
    the _lowerleft0 varieties work on matrices that have only zeros
95
    in the lowerleft part, such as box matrices, these varieties
96
    could produce less rounding errors, not due to the operations themselves,
97
    but because the compiler can easier recombine the operations
98
  void copy_mat(matrix a,matrix b)                 b = a
99
  void clear_mat(matrix a)                           a = 0
100
  void mmul(matrix a,matrix b,matrix dest)        !  dest = a . b
101
  void mmul_lowerleft0(matrix a,matrix b,matrix dest) dest = a . b
102
  void transpose(matrix src,matrix dest)        !  dest = src*
103
  void tmmul(matrix a,matrix b,matrix dest)        !  dest = a* . b
104
  void mtmul(matrix a,matrix b,matrix dest)        !  dest = a . b*
105
  real det(matrix a)                                   = det(a)
106
  void m_add(matrix a,matrix b,matrix dest)           dest = a + b
107
  void m_sub(matrix a,matrix b,matrix dest)           dest = a - b
108
  void msmul(matrix m1,real r1,matrix dest)           dest = r1 * m1
109
  void m_inv_lowerleft0(matrix src,matrix dest)    dest = src^-1
110
  void m_inv(matrix src,matrix dest)                !  dest = src^-1
111
  void mvmul(matrix a,rvec src,rvec dest)        !  dest = a . src
112
  real trace(matrix m)                             = trace(m)
113
*/
114

    
115
#include "maths.h"
116
#include "typedefs.h"
117
#include "sysstuff.h"
118
#include "macros.h"
119
#include "gmx_fatal.h"
120

    
121
#define EXP_LSB         0x00800000
122
#define EXP_MASK        0x7f800000
123
#define EXP_SHIFT       23
124
#define FRACT_MASK      0x007fffff
125
#define FRACT_SIZE      11              /* significant part of fraction */
126
#define FRACT_SHIFT     (EXP_SHIFT-FRACT_SIZE)
127
#define EXP_ADDR(val)   (((val)&EXP_MASK)>>EXP_SHIFT)
128
#define FRACT_ADDR(val) (((val)&(FRACT_MASK|EXP_LSB))>>FRACT_SHIFT)
129

    
130
#define PR_VEC(a)       a[XX],a[YY],a[ZZ]
131

    
132
#ifdef GMX_SOFTWARE_SQRT
133
extern const unsigned int *  gmx_invsqrt_exptab;
134
extern const unsigned int *  gmx_invsqrt_fracttab;
135
#endif
136

    
137

    
138
typedef union 
139
{
140
  unsigned int bval;
141
  float fval;
142
} t_convert;
143

    
144

    
145
#ifdef GMX_SOFTWARE_SQRT
146
static inline real invsqrt(real x)
147
{
148
  const real  half=0.5;
149
  const real  three=3.0;
150
  t_convert   result,bit_pattern;
151
  unsigned int exp,fract;
152
  real        lu;
153
  real        y;
154
#ifdef GMX_DOUBLE
155
  real        y2;
156
#endif
157
 
158
  bit_pattern.fval=x;
159
  exp   = EXP_ADDR(bit_pattern.bval);
160
  fract = FRACT_ADDR(bit_pattern.bval);
161
  result.bval=gmx_invsqrt_exptab[exp] | gmx_invsqrt_fracttab[fract];
162
  lu    = result.fval;
163
  
164
  y=(half*lu*(three-((x*lu)*lu)));
165
#ifdef GMX_DOUBLE
166
  y2=(half*y*(three-((x*y)*y)));
167
  
168
  return y2;                    /* 10 Flops */
169
#else
170
  return y;                     /* 5  Flops */
171
#endif
172
}
173
#define INVSQRT_DONE 
174
#endif /* gmx_invsqrt */
175

    
176
#ifndef INVSQRT_DONE
177
#define invsqrt(x) (1.0f/sqrt(x))
178
#endif
179

    
180

    
181

    
182

    
183

    
184
static inline real sqr(real x)
185
{
186
  return (x*x);
187
}
188

    
189
extern void vecinvsqrt(real in[],real out[],int n);
190
/* Perform out[i]=1.0/sqrt(in[i]) for n elements */
191

    
192

    
193
extern void vecrecip(real in[],real out[],int n);
194
/* Perform out[i]=1.0/(in[i]) for n elements */
195

    
196
/* Note: If you need a fast version of vecinvsqrt 
197
 * and/or vecrecip, call detectcpu() and run the SSE/3DNow/SSE2/Altivec
198
 * versions if your hardware supports it.
199
 *
200
 * To use those routines, your memory HAS TO BE CACHE-ALIGNED.
201
 * Start by allocating 31 bytes more than you need, put
202
 * this in a temp variable (e.g. _buf, so you can free it later), and
203
 * create your aligned array buf with
204
 * 
205
 *  buf=(real *) ( ( (unsigned long int)_buf + 31 ) & (~0x1f) );
206
 */
207

    
208

    
209
static inline void rvec_add(const rvec a,const rvec b,rvec c)
210
{
211
  real x,y,z;
212
  
213
  x=a[XX]+b[XX];
214
  y=a[YY]+b[YY];
215
  z=a[ZZ]+b[ZZ];
216
  
217
  c[XX]=x;
218
  c[YY]=y;
219
  c[ZZ]=z;
220
}
221

    
222
static inline void dvec_add(const dvec a,const dvec b,dvec c)
223
{
224
  double x,y,z;
225
  
226
  x=a[XX]+b[XX];
227
  y=a[YY]+b[YY];
228
  z=a[ZZ]+b[ZZ];
229
  
230
  c[XX]=x;
231
  c[YY]=y;
232
  c[ZZ]=z;
233
}
234

    
235
static inline void ivec_add(const ivec a,const ivec b,ivec c)
236
{
237
  int x,y,z;
238
  
239
  x=a[XX]+b[XX];
240
  y=a[YY]+b[YY];
241
  z=a[ZZ]+b[ZZ];
242
  
243
  c[XX]=x;
244
  c[YY]=y;
245
  c[ZZ]=z;
246
}
247

    
248
static inline void rvec_inc(rvec a,const rvec b)
249
{
250
  real x,y,z;
251
  
252
  x=a[XX]+b[XX];
253
  y=a[YY]+b[YY];
254
  z=a[ZZ]+b[ZZ];
255
  
256
  a[XX]=x;
257
  a[YY]=y;
258
  a[ZZ]=z;
259
}
260

    
261
static inline void ivec_inc(ivec a,const ivec b)
262
{
263
  int x,y,z;
264

    
265
  x=a[XX]+b[XX];
266
  y=a[YY]+b[YY];
267
  z=a[ZZ]+b[ZZ];
268

    
269
  a[XX]=x;
270
  a[YY]=y;
271
  a[ZZ]=z;
272
}
273

    
274
static inline void rvec_sub(const rvec a,const rvec b,rvec c)
275
{
276
  real x,y,z;
277
  
278
  x=a[XX]-b[XX];
279
  y=a[YY]-b[YY];
280
  z=a[ZZ]-b[ZZ];
281
  
282
  c[XX]=x;
283
  c[YY]=y;
284
  c[ZZ]=z;
285
}
286

    
287
static inline void dvec_sub(const dvec a,const dvec b,dvec c)
288
{
289
  double x,y,z;
290
  
291
  x=a[XX]-b[XX];
292
  y=a[YY]-b[YY];
293
  z=a[ZZ]-b[ZZ];
294
  
295
  c[XX]=x;
296
  c[YY]=y;
297
  c[ZZ]=z;
298
}
299

    
300
static inline void rvec_dec(rvec a,const rvec b)
301
{
302
  real x,y,z;
303
  
304
  x=a[XX]-b[XX];
305
  y=a[YY]-b[YY];
306
  z=a[ZZ]-b[ZZ];
307
  
308
  a[XX]=x;
309
  a[YY]=y;
310
  a[ZZ]=z;
311
}
312

    
313
static inline void copy_rvec(const rvec a,rvec b)
314
{
315
  b[XX]=a[XX];
316
  b[YY]=a[YY];
317
  b[ZZ]=a[ZZ];
318
}
319

    
320
static inline void copy_dvec(const dvec a,dvec b)
321
{
322
  b[XX]=a[XX];
323
  b[YY]=a[YY];
324
  b[ZZ]=a[ZZ];
325
}
326

    
327
static inline void copy_ivec(const ivec a,ivec b)
328
{
329
  b[XX]=a[XX];
330
  b[YY]=a[YY];
331
  b[ZZ]=a[ZZ];
332
}
333

    
334
static inline void ivec_sub(const ivec a,const ivec b,ivec c)
335
{
336
  int x,y,z;
337
  
338
  x=a[XX]-b[XX];
339
  y=a[YY]-b[YY];
340
  z=a[ZZ]-b[ZZ];
341
  
342
  c[XX]=x;
343
  c[YY]=y;
344
  c[ZZ]=z;
345
}
346

    
347
static inline void copy_mat(matrix a,matrix b)
348
{
349
  copy_rvec(a[XX],b[XX]);
350
  copy_rvec(a[YY],b[YY]);
351
  copy_rvec(a[ZZ],b[ZZ]);
352
}
353

    
354
static inline void svmul(real a,const rvec v1,rvec v2)
355
{
356
  v2[XX]=a*v1[XX];
357
  v2[YY]=a*v1[YY];
358
  v2[ZZ]=a*v1[ZZ];
359
}
360

    
361
static inline void dsvmul(double a,const dvec v1,dvec v2)
362
{
363
  v2[XX]=a*v1[XX];
364
  v2[YY]=a*v1[YY];
365
  v2[ZZ]=a*v1[ZZ];
366
}
367

    
368
static inline real distance2(const rvec v1,const rvec v2)
369
{
370
  return sqr(v2[XX]-v1[XX]) + sqr(v2[YY]-v1[YY]) + sqr(v2[ZZ]-v1[ZZ]);
371
}
372

    
373
static inline void clear_rvec(rvec a)
374
{
375
  /* The ibm compiler has problems with inlining this 
376
   * when we use a const real variable
377
   */
378
  a[XX]=0.0;
379
  a[YY]=0.0;
380
  a[ZZ]=0.0;
381
}
382

    
383
static inline void clear_dvec(dvec a)
384
{
385
  /* The ibm compiler has problems with inlining this 
386
   * when we use a const real variable
387
   */
388
  a[XX]=0.0;
389
  a[YY]=0.0;
390
  a[ZZ]=0.0;
391
}
392

    
393
static inline void clear_ivec(ivec a)
394
{
395
  a[XX]=0;
396
  a[YY]=0;
397
  a[ZZ]=0;
398
}
399

    
400
static inline void clear_rvecs(int n,rvec v[])
401
{
402
  int i;
403
  
404
  for(i=0; (i<n); i++) 
405
    clear_rvec(v[i]);
406
}
407

    
408
static inline void clear_mat(matrix a)
409
{
410
  const real nul=0.0;
411
  
412
  a[XX][XX]=a[XX][YY]=a[XX][ZZ]=nul;
413
  a[YY][XX]=a[YY][YY]=a[YY][ZZ]=nul;
414
  a[ZZ][XX]=a[ZZ][YY]=a[ZZ][ZZ]=nul;
415
}
416

    
417
static inline real iprod(const rvec a,const rvec b)
418
{
419
  return (a[XX]*b[XX]+a[YY]*b[YY]+a[ZZ]*b[ZZ]);
420
}
421

    
422
static inline double diprod(const dvec a,const dvec b)
423
{
424
  return (a[XX]*b[XX]+a[YY]*b[YY]+a[ZZ]*b[ZZ]);
425
}
426

    
427
static inline real iiprod(const ivec a,const ivec b)
428
{
429
  return (a[XX]*b[XX]+a[YY]*b[YY]+a[ZZ]*b[ZZ]);
430
}
431

    
432
static inline real norm2(const rvec a)
433
{
434
  return a[XX]*a[XX]+a[YY]*a[YY]+a[ZZ]*a[ZZ];
435
}
436

    
437
static inline real norm(const rvec a)
438
{
439
  return sqrt(a[XX]*a[XX]+a[YY]*a[YY]+a[ZZ]*a[ZZ]);
440
}
441

    
442
static inline double dnorm(const dvec a)
443
{
444
  return sqrt(a[XX]*a[XX]+a[YY]*a[YY]+a[ZZ]*a[ZZ]);
445
}
446

    
447
static inline real cos_angle(const rvec a,const rvec b)
448
{
449
  /* 
450
   *                  ax*bx + ay*by + az*bz
451
   * cos-vec (a,b) =  ---------------------
452
   *                      ||a|| * ||b||
453
   */
454
  real   cos;
455
  int    m;
456
  double aa,bb,ip,ipa,ipb; /* For accuracy these must be double! */
457
  
458
  ip=ipa=ipb=0.0;
459
  for(m=0; (m<DIM); m++) {                /* 18                */
460
    aa   = a[m];
461
    bb   = b[m];
462
    ip  += aa*bb;
463
    ipa += aa*aa;
464
    ipb += bb*bb;
465
  }
466
  cos=ip*invsqrt(ipa*ipb);                /*  7                */
467
                                        /* 25 TOTAL        */
468
  if (cos > 1.0) 
469
    return  1.0; 
470
  if (cos <-1.0) 
471
    return -1.0;
472
  
473
  return cos;
474
}
475

    
476
static inline real cos_angle_non_linear(const rvec a,const rvec b)
477
{
478
  /* 
479
   *                  ax*bx + ay*by + az*bz
480
   * cos-vec (a,b) =  ---------------------
481
   *                      ||a|| * ||b||
482
   */
483
  real   cos;
484
  int    m;
485
  double aa,bb,ip,ipa,ipb,ipab; /* For accuracy these must be double! */
486
  
487
  ip=ipa=ipb=0.0;
488
  for(m=0; (m<DIM); m++) {                /* 18                */
489
    aa   = a[m];
490
    bb   = b[m];
491
    ip  += aa*bb;
492
    ipa += aa*aa;
493
    ipb += bb*bb;
494
  }
495
  ipab = ipa*ipb;
496
  if (fabs(ipab) > GMX_REAL_EPS)
497
    cos = ip*invsqrt(ipab);                /*  7                */
498
  else 
499
    cos = 1;
500
                                        /* 25 TOTAL        */
501
  if (cos >= 1.0) 
502
    return  1.0-GMX_REAL_EPS; 
503
  if (cos <=-1.0) 
504
    return -1.0+GMX_REAL_EPS;
505
  
506
  return cos;
507
}
508

    
509
static inline real cos_angle_no_table(const rvec a,const rvec b)
510
{
511
  /* This version does not need the invsqrt lookup table */
512
  real   cos;
513
  int    m;
514
  double aa,bb,ip,ipa,ipb; /* For accuracy these must be double! */
515
  
516
  ip=ipa=ipb=0.0;
517
  for(m=0; (m<DIM); m++) {                /* 18                */
518
    aa   = a[m];
519
    bb   = b[m];
520
    ip  += aa*bb;
521
    ipa += aa*aa;
522
    ipb += bb*bb;
523
  }
524
  cos=ip/sqrt(ipa*ipb);                 /* 12                */
525
                                        /* 30 TOTAL        */
526
  if (cos > 1.0) 
527
    return  1.0; 
528
  if (cos <-1.0) 
529
    return -1.0;
530
  
531
  return cos;
532
}
533

    
534
static inline void oprod(const rvec a,const rvec b,rvec c)
535
{
536
  c[XX]=a[YY]*b[ZZ]-a[ZZ]*b[YY];
537
  c[YY]=a[ZZ]*b[XX]-a[XX]*b[ZZ];
538
  c[ZZ]=a[XX]*b[YY]-a[YY]*b[XX];
539
}
540

    
541
static inline void mmul_lowerleft0(matrix a,matrix b,matrix dest)
542
{
543
  dest[XX][XX]=a[XX][XX]*b[XX][XX];
544
  dest[XX][YY]=0.0;
545
  dest[XX][ZZ]=0.0;
546
  dest[YY][XX]=a[YY][XX]*b[XX][XX]+a[YY][YY]*b[YY][XX];
547
  dest[YY][YY]=                    a[YY][YY]*b[YY][YY];
548
  dest[YY][ZZ]=0.0;
549
  dest[ZZ][XX]=a[ZZ][XX]*b[XX][XX]+a[ZZ][YY]*b[YY][XX]+a[ZZ][ZZ]*b[ZZ][XX];
550
  dest[ZZ][YY]=                    a[ZZ][YY]*b[YY][YY]+a[ZZ][ZZ]*b[ZZ][YY];
551
  dest[ZZ][ZZ]=                                        a[ZZ][ZZ]*b[ZZ][ZZ];
552
}
553

    
554
static inline void mmul(matrix a,matrix b,matrix dest)
555
{
556
  dest[XX][XX]=a[XX][XX]*b[XX][XX]+a[XX][YY]*b[YY][XX]+a[XX][ZZ]*b[ZZ][XX];
557
  dest[YY][XX]=a[YY][XX]*b[XX][XX]+a[YY][YY]*b[YY][XX]+a[YY][ZZ]*b[ZZ][XX];
558
  dest[ZZ][XX]=a[ZZ][XX]*b[XX][XX]+a[ZZ][YY]*b[YY][XX]+a[ZZ][ZZ]*b[ZZ][XX];
559
  dest[XX][YY]=a[XX][XX]*b[XX][YY]+a[XX][YY]*b[YY][YY]+a[XX][ZZ]*b[ZZ][YY];
560
  dest[YY][YY]=a[YY][XX]*b[XX][YY]+a[YY][YY]*b[YY][YY]+a[YY][ZZ]*b[ZZ][YY];
561
  dest[ZZ][YY]=a[ZZ][XX]*b[XX][YY]+a[ZZ][YY]*b[YY][YY]+a[ZZ][ZZ]*b[ZZ][YY];
562
  dest[XX][ZZ]=a[XX][XX]*b[XX][ZZ]+a[XX][YY]*b[YY][ZZ]+a[XX][ZZ]*b[ZZ][ZZ];
563
  dest[YY][ZZ]=a[YY][XX]*b[XX][ZZ]+a[YY][YY]*b[YY][ZZ]+a[YY][ZZ]*b[ZZ][ZZ];
564
  dest[ZZ][ZZ]=a[ZZ][XX]*b[XX][ZZ]+a[ZZ][YY]*b[YY][ZZ]+a[ZZ][ZZ]*b[ZZ][ZZ];
565
}
566

    
567
static inline void transpose(matrix src,matrix dest)
568
{
569
  dest[XX][XX]=src[XX][XX];
570
  dest[YY][XX]=src[XX][YY];
571
  dest[ZZ][XX]=src[XX][ZZ];
572
  dest[XX][YY]=src[YY][XX];
573
  dest[YY][YY]=src[YY][YY];
574
  dest[ZZ][YY]=src[YY][ZZ];
575
  dest[XX][ZZ]=src[ZZ][XX];
576
  dest[YY][ZZ]=src[ZZ][YY];
577
  dest[ZZ][ZZ]=src[ZZ][ZZ];
578
}
579

    
580
static inline void tmmul(matrix a,matrix b,matrix dest)
581
{
582
  /* Computes dest=mmul(transpose(a),b,dest) - used in do_pr_pcoupl */
583
  dest[XX][XX]=a[XX][XX]*b[XX][XX]+a[YY][XX]*b[YY][XX]+a[ZZ][XX]*b[ZZ][XX];
584
  dest[XX][YY]=a[XX][XX]*b[XX][YY]+a[YY][XX]*b[YY][YY]+a[ZZ][XX]*b[ZZ][YY];
585
  dest[XX][ZZ]=a[XX][XX]*b[XX][ZZ]+a[YY][XX]*b[YY][ZZ]+a[ZZ][XX]*b[ZZ][ZZ];
586
  dest[YY][XX]=a[XX][YY]*b[XX][XX]+a[YY][YY]*b[YY][XX]+a[ZZ][YY]*b[ZZ][XX];
587
  dest[YY][YY]=a[XX][YY]*b[XX][YY]+a[YY][YY]*b[YY][YY]+a[ZZ][YY]*b[ZZ][YY];
588
  dest[YY][ZZ]=a[XX][YY]*b[XX][ZZ]+a[YY][YY]*b[YY][ZZ]+a[ZZ][YY]*b[ZZ][ZZ];
589
  dest[ZZ][XX]=a[XX][ZZ]*b[XX][XX]+a[YY][ZZ]*b[YY][XX]+a[ZZ][ZZ]*b[ZZ][XX];
590
  dest[ZZ][YY]=a[XX][ZZ]*b[XX][YY]+a[YY][ZZ]*b[YY][YY]+a[ZZ][ZZ]*b[ZZ][YY];
591
  dest[ZZ][ZZ]=a[XX][ZZ]*b[XX][ZZ]+a[YY][ZZ]*b[YY][ZZ]+a[ZZ][ZZ]*b[ZZ][ZZ];
592
}
593

    
594
static inline void mtmul(matrix a,matrix b,matrix dest)
595
{
596
  /* Computes dest=mmul(a,transpose(b),dest) - used in do_pr_pcoupl */
597
  dest[XX][XX]=a[XX][XX]*b[XX][XX]+a[XX][YY]*b[XX][YY]+a[XX][ZZ]*b[XX][ZZ];
598
  dest[XX][YY]=a[XX][XX]*b[YY][XX]+a[XX][YY]*b[YY][YY]+a[XX][ZZ]*b[YY][ZZ];
599
  dest[XX][ZZ]=a[XX][XX]*b[ZZ][XX]+a[XX][YY]*b[ZZ][YY]+a[XX][ZZ]*b[ZZ][ZZ];
600
  dest[YY][XX]=a[YY][XX]*b[XX][XX]+a[YY][YY]*b[XX][YY]+a[YY][ZZ]*b[XX][ZZ];
601
  dest[YY][YY]=a[YY][XX]*b[YY][XX]+a[YY][YY]*b[YY][YY]+a[YY][ZZ]*b[YY][ZZ];
602
  dest[YY][ZZ]=a[YY][XX]*b[ZZ][XX]+a[YY][YY]*b[ZZ][YY]+a[YY][ZZ]*b[ZZ][ZZ];
603
  dest[ZZ][XX]=a[ZZ][XX]*b[XX][XX]+a[ZZ][YY]*b[XX][YY]+a[ZZ][ZZ]*b[XX][ZZ];
604
  dest[ZZ][YY]=a[ZZ][XX]*b[YY][XX]+a[ZZ][YY]*b[YY][YY]+a[ZZ][ZZ]*b[YY][ZZ];
605
  dest[ZZ][ZZ]=a[ZZ][XX]*b[ZZ][XX]+a[ZZ][YY]*b[ZZ][YY]+a[ZZ][ZZ]*b[ZZ][ZZ];
606
}
607

    
608
static inline real det(matrix a)
609
{
610
  return ( a[XX][XX]*(a[YY][YY]*a[ZZ][ZZ]-a[ZZ][YY]*a[YY][ZZ])
611
          -a[YY][XX]*(a[XX][YY]*a[ZZ][ZZ]-a[ZZ][YY]*a[XX][ZZ])
612
          +a[ZZ][XX]*(a[XX][YY]*a[YY][ZZ]-a[YY][YY]*a[XX][ZZ]));
613
}
614

    
615
static inline void m_add(matrix a,matrix b,matrix dest)
616
{
617
  dest[XX][XX]=a[XX][XX]+b[XX][XX];
618
  dest[XX][YY]=a[XX][YY]+b[XX][YY];
619
  dest[XX][ZZ]=a[XX][ZZ]+b[XX][ZZ];
620
  dest[YY][XX]=a[YY][XX]+b[YY][XX];
621
  dest[YY][YY]=a[YY][YY]+b[YY][YY];
622
  dest[YY][ZZ]=a[YY][ZZ]+b[YY][ZZ];
623
  dest[ZZ][XX]=a[ZZ][XX]+b[ZZ][XX];
624
  dest[ZZ][YY]=a[ZZ][YY]+b[ZZ][YY];
625
  dest[ZZ][ZZ]=a[ZZ][ZZ]+b[ZZ][ZZ];
626
}
627

    
628
static inline void m_sub(matrix a,matrix b,matrix dest)
629
{
630
  dest[XX][XX]=a[XX][XX]-b[XX][XX];
631
  dest[XX][YY]=a[XX][YY]-b[XX][YY];
632
  dest[XX][ZZ]=a[XX][ZZ]-b[XX][ZZ];
633
  dest[YY][XX]=a[YY][XX]-b[YY][XX];
634
  dest[YY][YY]=a[YY][YY]-b[YY][YY];
635
  dest[YY][ZZ]=a[YY][ZZ]-b[YY][ZZ];
636
  dest[ZZ][XX]=a[ZZ][XX]-b[ZZ][XX];
637
  dest[ZZ][YY]=a[ZZ][YY]-b[ZZ][YY];
638
  dest[ZZ][ZZ]=a[ZZ][ZZ]-b[ZZ][ZZ];
639
}
640

    
641
static inline void msmul(matrix m1,real r1,matrix dest)
642
{
643
  dest[XX][XX]=r1*m1[XX][XX];
644
  dest[XX][YY]=r1*m1[XX][YY];
645
  dest[XX][ZZ]=r1*m1[XX][ZZ];
646
  dest[YY][XX]=r1*m1[YY][XX];
647
  dest[YY][YY]=r1*m1[YY][YY];
648
  dest[YY][ZZ]=r1*m1[YY][ZZ];
649
  dest[ZZ][XX]=r1*m1[ZZ][XX];
650
  dest[ZZ][YY]=r1*m1[ZZ][YY];
651
  dest[ZZ][ZZ]=r1*m1[ZZ][ZZ];
652
}
653

    
654
static inline void m_inv_lowerleft0(matrix src,matrix dest)
655
{
656
  double tmp = src[XX][XX]*src[YY][YY]*src[ZZ][ZZ];
657
  if (gmx_within_tol(tmp,0.0,100*GMX_REAL_MIN))
658
    gmx_fatal(FARGS,"Can not invert matrix, determinant is zero");
659

    
660
  dest[XX][XX] = 1/src[XX][XX];
661
  dest[YY][YY] = 1/src[YY][YY];
662
  dest[ZZ][ZZ] = 1/src[ZZ][ZZ];
663
  dest[ZZ][XX] = (src[YY][XX]*src[ZZ][YY]*dest[YY][YY]
664
                  - src[ZZ][XX])*dest[XX][XX]*dest[ZZ][ZZ];
665
  dest[YY][XX] = -src[YY][XX]*dest[XX][XX]*dest[YY][YY];
666
  dest[ZZ][YY] = -src[ZZ][YY]*dest[YY][YY]*dest[ZZ][ZZ];
667
  dest[XX][YY] = 0.0;
668
  dest[XX][ZZ] = 0.0;
669
  dest[YY][ZZ] = 0.0;
670
}
671

    
672
static inline void m_inv(matrix src,matrix dest)
673
{
674
  const real smallreal = 1.0e-24;
675
  const real largereal = 1.0e24;
676
  real  deter,c,fc;
677

    
678
  deter = det(src);
679
  c     = 1.0/deter;
680
  fc    = fabs(c);
681
  
682
  if ((fc <= smallreal) || (fc >= largereal)) 
683
    gmx_fatal(FARGS,"Can not invert matrix, determinant = %e",deter);
684

    
685
  dest[XX][XX]= c*(src[YY][YY]*src[ZZ][ZZ]-src[ZZ][YY]*src[YY][ZZ]);
686
  dest[XX][YY]=-c*(src[XX][YY]*src[ZZ][ZZ]-src[ZZ][YY]*src[XX][ZZ]);
687
  dest[XX][ZZ]= c*(src[XX][YY]*src[YY][ZZ]-src[YY][YY]*src[XX][ZZ]);
688
  dest[YY][XX]=-c*(src[YY][XX]*src[ZZ][ZZ]-src[ZZ][XX]*src[YY][ZZ]);
689
  dest[YY][YY]= c*(src[XX][XX]*src[ZZ][ZZ]-src[ZZ][XX]*src[XX][ZZ]);
690
  dest[YY][ZZ]=-c*(src[XX][XX]*src[YY][ZZ]-src[YY][XX]*src[XX][ZZ]);
691
  dest[ZZ][XX]= c*(src[YY][XX]*src[ZZ][YY]-src[ZZ][XX]*src[YY][YY]);
692
  dest[ZZ][YY]=-c*(src[XX][XX]*src[ZZ][YY]-src[ZZ][XX]*src[XX][YY]);
693
  dest[ZZ][ZZ]= c*(src[XX][XX]*src[YY][YY]-src[YY][XX]*src[XX][YY]);
694
}
695

    
696
static inline void mvmul(matrix a,const rvec src,rvec dest)
697
{
698
  dest[XX]=a[XX][XX]*src[XX]+a[XX][YY]*src[YY]+a[XX][ZZ]*src[ZZ];
699
  dest[YY]=a[YY][XX]*src[XX]+a[YY][YY]*src[YY]+a[YY][ZZ]*src[ZZ];
700
  dest[ZZ]=a[ZZ][XX]*src[XX]+a[ZZ][YY]*src[YY]+a[ZZ][ZZ]*src[ZZ];
701
}
702

    
703
static inline void unitv(const rvec src,rvec dest)
704
{
705
  real linv;
706
  
707
  linv=invsqrt(norm2(src));
708
  dest[XX]=linv*src[XX];
709
  dest[YY]=linv*src[YY];
710
  dest[ZZ]=linv*src[ZZ];
711
}
712

    
713
static inline void unitv_no_table(const rvec src,rvec dest)
714
{
715
  real linv;
716
  
717
  linv=1.0/sqrt(norm2(src));
718
  dest[XX]=linv*src[XX];
719
  dest[YY]=linv*src[YY];
720
  dest[ZZ]=linv*src[ZZ];
721
}
722

    
723
static inline real trace(matrix m)
724
{
725
  return (m[XX][XX]+m[YY][YY]+m[ZZ][ZZ]);
726
}
727

    
728
static inline real _divide(real a,real b,char *file,int line)
729
{
730
    if (gmx_within_tol(b,0.0,GMX_REAL_MIN)) 
731
        gmx_fatal(FARGS,"Dividing by zero, file %s, line %d",file,line);
732
    return a/b;
733
}
734

    
735
static inline int _mod(int a,int b,char *file,int line)
736
{
737
  if(b==0)
738
    gmx_fatal(FARGS,"Modulo zero, file %s, line %d",file,line);
739
  return a % b;
740
}
741

    
742
#define divide(a,b) _divide((a),(b),__FILE__,__LINE__)
743
#define mod(a,b)    _mod((a),(b),__FILE__,__LINE__)
744

    
745
#endif        /* _vec_h */