← Back to branch summary

~ubuntu-branches/debian/sid/lammps/sid

« back to all changes in this revision

Viewing changes to src/math_vector.h

  • Committer: Package Import Robot
  • Author(s): Anton Gladky
  • Date: 2013-11-20 22:41:36 UTC
  • mfrom: (1.2.2)
  • Revision ID: package-import@ubuntu.com-20131120224136-tzx7leh606fqnckm
Tags: 0~20131119.git7162cf0-1
* [e65b919] Imported Upstream version 0~20131119.git7162cf0
* [f7bddd4] Fix some problems, introduced by upstream recently.
* [3616dfc] Use wrap-and-sort script.
* [7e92030] Ignore quilt dir

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/math_vector.h
 
1
/* ----------------------------------------------------------------------
 
2
   LAMMPS - Large-scale Atomic/Molecular Massively Parallel Simulator
 
3
   http://lammps.sandia.gov, Sandia National Laboratories
 
4
   Steve Plimpton, sjplimp@sandia.gov
 
5
 
 
6
   Copyright (2003) Sandia Corporation.  Under the terms of Contract
 
7
   DE-AC04-94AL85000 with Sandia Corporation, the U.S. Government retains
 
8
   certain rights in this software.  This software is distributed under
 
9
   the GNU General Public License.
 
10
 
 
11
   See the README file in the top-level LAMMPS directory.
 
12
------------------------------------------------------------------------- */
 
13
 
 
14
/* ----------------------------------------------------------------------
 
15
   Contributing author: Pieter J. in 't Veld (SNL)
 
16
------------------------------------------------------------------------- */
 
17
 
 
18
#ifndef LMP_MATH_VECTOR_H
 
19
#define LMP_MATH_VECTOR_H
 
20
 
 
21
#include "math.h"
 
22
#include "string.h"
 
23
 
 
24
#define VECTOR_NULL        {0, 0, 0}
 
25
#define SHAPE_NULL        {0, 0, 0, 0, 0, 0}
 
26
#define FORM_NULL        {0, 0, 0, 0, 0, 0}
 
27
#define MATRIX_NULL        {VECTOR_NULL, VECTOR_NULL, VECTOR_NULL}
 
28
#define VECTOR4_NULL        {0, 0, 0, 0}
 
29
#define QUATERNION_NULL        {0, 0, 0, 0}
 
30
#define FORM4_NULL        {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
 
31
 
 
32
#define FZERO 1e-15
 
33
#define fzero(x) (((x)>-FZERO) && ((x)<FZERO))
 
34
 
 
35
namespace LAMMPS_NS {
 
36
 
 
37
typedef double vector[3];                // 0:x  1:y  2:z
 
38
typedef int lvector[3];
 
39
typedef double shape[6];                // 0:xx 1:yy 2:zz 3:zy 4:zx 5:yx
 
40
typedef int lshape[6];                        // xy=0  xz=0  yz=0;
 
41
typedef double form[6];                        // 0:xx 1:yy 2:zz 3:zy 4:zx 5:yx
 
42
typedef int lform[6];                        // xy=yx xz=zx yz=zy;
 
43
typedef vector matrix[3];                // 00:xx 11:yy 22:zz 21:zy 20:zx 10:yx
 
44
typedef lvector lmatrix[3];
 
45
typedef double vector4[4];                // 4D vector
 
46
typedef double form4[10];                // 0:00 1:11 2:22 3:33 4:32
 
47
                                        // 5:31 6:30 7:21 8:20 9:10
 
48
                                        // 01=10 02=20 03=30 etc
 
49
typedef double quaternion[4];                // quaternion
 
50
 
 
51
// vector operators
 
52
 
 
53
inline double vec_dot(vector &a, vector &b) {                        // a.b
 
54
  return a[0]*b[0]+a[1]*b[1]+a[2]*b[2]; }
 
55
 
 
56
inline void vec_null(vector &dest) {
 
57
  memset(dest, 0, sizeof(vector)); }
 
58
 
 
59
inline void vec_neg(vector &dest) {                                // -a
 
60
  dest[0] = -dest[0];
 
61
  dest[1] = -dest[1];
 
62
  dest[2] = -dest[2]; }
 
63
 
 
64
inline void vec_norm(vector &dest) {                                 // a/|a|
 
65
  register double f = sqrt(vec_dot(dest, dest));
 
66
  dest[0] /= f;
 
67
  dest[1] /= f;
 
68
  dest[2] /= f; }
 
69
 
 
70
inline void vec_add(vector &dest, vector &src) {                // a+b
 
71
  dest[0] += src[0];
 
72
  dest[1] += src[1];
 
73
  dest[2] += src[2]; }
 
74
 
 
75
inline void vec_subtr(vector &dest, vector &src) {                // a-b
 
76
  dest[0] -= src[0];
 
77
  dest[1] -= src[1];
 
78
  dest[2] -= src[2]; }
 
79
 
 
80
inline void vec_mult(vector &dest, vector &src) {                // a*b
 
81
  dest[0] *= src[0];
 
82
  dest[1] *= src[1];
 
83
  dest[2] *= src[2]; }
 
84
 
 
85
inline void vec_div(vector &dest, vector &src) {                // a/b
 
86
  dest[0] /= src[0];
 
87
  dest[1] /= src[1];
 
88
  dest[2] /= src[2]; }
 
89
 
 
90
inline void vec_cross(vector &dest, vector &src) {                // a x b
 
91
  vector t = {
 
92
    dest[1]*src[2]-dest[2]*src[1],
 
93
    dest[2]*src[0]-dest[0]*src[2],
 
94
    dest[0]*src[1]-dest[1]*src[0]};
 
95
  memcpy(dest, t, sizeof(vector)); }
 
96
 
 
97
inline void vec_scalar_mult(vector &dest, double f) {                // f*a
 
98
  dest[0] *= f;
 
99
  dest[1] *= f;
 
100
  dest[2] *= f; }
 
101
 
 
102
inline void vec_to_lvec(lvector &dest, vector &src) {
 
103
  dest[0] = (int) src[0];
 
104
  dest[1] = (int) src[1];
 
105
  dest[2] = (int) src[2]; }
 
106
 
 
107
inline void lvec_to_vec(vector &dest, lvector &src) {
 
108
  dest[0] = (double) src[0];
 
109
  dest[1] = (double) src[1];
 
110
  dest[2] = (double) src[2]; }
 
111
 
 
112
// shape operators
 
113
 
 
114
inline double shape_det(shape &s) {
 
115
  return s[0]*s[1]*s[2]; }
 
116
 
 
117
inline void shape_null(shape &dest) {
 
118
  memset(dest, 0, sizeof(shape)); }
 
119
 
 
120
inline void shape_unit(shape &dest) {
 
121
  memset(dest, 0, sizeof(shape));
 
122
  dest[0] = dest[1] = dest[2] = 1.0; }
 
123
 
 
124
inline void shape_add(shape &dest, shape &src) {                // h_a+h_b
 
125
  dest[0] += src[0]; dest[1] += src[1]; dest[2] += src[2];
 
126
  dest[3] += src[3]; dest[4] += src[4]; dest[5] += src[5]; }
 
127
 
 
128
inline void shape_subtr(shape &dest, shape &src) {                // h_a-h_b
 
129
  dest[0] -= src[0]; dest[1] -= src[1]; dest[2] -= src[2];
 
130
  dest[3] -= src[3]; dest[4] -= src[4]; dest[5] -= src[5]; }
 
131
 
 
132
inline void shape_inv(shape &h_inv, shape &h) {                        // h^-1
 
133
  h_inv[0] = 1.0/h[0]; h_inv[1] = 1.0/h[1]; h_inv[2] = 1.0/h[2];
 
134
  h_inv[3] = -h[3]/(h[1]*h[2]);
 
135
  h_inv[4] = (h[3]*h[5]-h[1]*h[4])/(h[0]*h[1]*h[2]);
 
136
  h_inv[5] = -h[5]/(h[0]*h[1]); }
 
137
 
 
138
inline void shape_dot(shape &dest, shape &src) {                // h_a.h_b
 
139
  dest[3] = dest[1]*src[3]+dest[3]*src[2];
 
140
  dest[4] = dest[0]*src[4]+dest[5]*src[3]+dest[4]*src[2];
 
141
  dest[5] = dest[0]*src[5]+dest[5]*src[1];
 
142
  dest[0] *= src[0]; dest[1] *= src[1]; dest[2] *= src[2]; }
 
143
 
 
144
inline void shape_scalar_mult(shape &dest, double f) {                // f*h
 
145
  dest[0] *= f; dest[1] *= f; dest[2] *= f;
 
146
  dest[3] *= f; dest[4] *= f; dest[5] *= f; }
 
147
 
 
148
inline void shape_vec_dot(vector &dest, vector &src, shape &h) {// h.a
 
149
  dest[0] = h[0]*src[0]+h[5]*src[1]+h[4]*src[2];
 
150
  dest[1] =             h[1]*src[1]+h[3]*src[2];
 
151
  dest[2] =                         h[2]*src[2]; }
 
152
 
 
153
inline void vec_shape_dot(vector &dest, shape &h, vector &src) {// a.h
 
154
  dest[2] = h[4]*src[0]+h[3]*src[1]+h[2]*src[2];
 
155
  dest[1] = h[5]*src[0]+h[1]*src[1];
 
156
  dest[0] = h[0]*src[0]; }
 
157
 
 
158
inline void shape_to_matrix(matrix &dest, shape &h) {                // m = h
 
159
  dest[0][0] = h[0]; dest[1][0] = h[5]; dest[2][0] = h[4];
 
160
  dest[0][1] =  0.0; dest[1][1] = h[1]; dest[2][1] = h[3];
 
161
  dest[0][2] =  0.0; dest[1][2] =  0.0; dest[2][2] = h[2]; }
 
162
 
 
163
inline void shape_to_lshape(lshape &dest, shape &src) {
 
164
  dest[0] = (long)src[0]; dest[1] = (long)src[1]; dest[2] = (long)src[2];
 
165
  dest[3] = (long)src[3]; dest[4] = (long)src[4]; dest[5] = (long)src[5]; }
 
166
 
 
167
inline void lshape_to_shape(shape &dest, lshape &src) {
 
168
  dest[0] = (double)src[0]; dest[1] = (double)src[1]; dest[2] = (double)src[2];
 
169
  dest[3] = (double)src[3]; dest[4] = (double)src[4]; dest[5] = (double)src[5];}
 
170
 
 
171
// form operators
 
172
 
 
173
inline double form_det(form &m) {                                // |m|
 
174
  return m[0]*(m[1]*m[2]-m[3]*m[3])+
 
175
    m[4]*(2.0*m[3]*m[5]-m[1]*m[4])-m[2]*m[5]*m[5]; }
 
176
 
 
177
inline void form_null(form &dest) {
 
178
  memset(dest, 0, sizeof(form)); }
 
179
 
 
180
inline void form_unit(form &dest) {
 
181
  memset(dest, 0, sizeof(form));
 
182
  dest[0] = dest[1] = dest[2] = 1.0; }
 
183
 
 
184
inline void form_add(form &dest, form &src) {                        // m_a+m_b
 
185
  dest[0] += src[0]; dest[1] += src[1]; dest[2] += src[2];
 
186
  dest[3] += src[3]; dest[4] += src[4]; dest[5] += src[5]; }
 
187
 
 
188
inline void form_subtr(shape &dest, form &src) {                // m_a-m_b
 
189
  dest[0] -= src[0]; dest[1] -= src[1]; dest[2] -= src[2];
 
190
  dest[3] -= src[3]; dest[4] -= src[4]; dest[5] -= src[5]; }
 
191
 
 
192
inline int form_inv(form &m_inv, form &m) {                        // m^-1
 
193
  register double det = form_det(m);
 
194
  if (fzero(det)) return 0;
 
195
  m_inv[0] = (m[1]*m[2]-m[3]*m[3])/det;
 
196
  m_inv[1] = (m[0]*m[2]-m[4]*m[4])/det;
 
197
  m_inv[2] = (m[0]*m[1]-m[5]*m[5])/det;
 
198
  m_inv[3] = (m[4]*m[5]-m[0]*m[3])/det;
 
199
  m_inv[4] = (m[3]*m[5]-m[1]*m[4])/det;
 
200
  m_inv[5] = (m[3]*m[4]-m[2]*m[5])/det;
 
201
  return 1; }
 
202
 
 
203
inline void form_dot(form &dest, form &src) {                        // m_a.m_b
 
204
  form m;
 
205
  memcpy(m, dest, sizeof(form));
 
206
  dest[0] = m[0]*src[0]+m[4]*src[4]+m[5]*src[5];
 
207
  dest[1] = m[1]*src[1]+m[3]*src[3]+m[5]*src[5];
 
208
  dest[2] = m[2]*src[2]+m[3]*src[3]+m[4]*src[4];
 
209
  dest[3] = m[3]*src[2]+m[1]*src[3]+m[5]*src[4];
 
210
  dest[4] = m[4]*src[2]+m[5]*src[3]+m[0]*src[4];
 
211
  dest[5] = m[5]*src[1]+m[4]*src[3]+m[0]*src[5]; }
 
212
 
 
213
inline void form_vec_dot(vector &dest, form &m) {                // m.a
 
214
  vector a;
 
215
  memcpy(a, dest, sizeof(vector));
 
216
  dest[0] = m[0]*a[0]+m[5]*a[1]+m[4]*a[2];
 
217
  dest[1] = m[5]*a[0]+m[1]*a[1]+m[3]*a[2];
 
218
  dest[2] = m[4]*a[0]+m[3]*a[1]+m[2]*a[2]; }
 
219
 
 
220
inline void form_to_lform(lform &dest, form &src) {
 
221
  dest[0] = (long)src[0]; dest[1] = (long)src[1]; dest[2] = (long)src[2];
 
222
  dest[3] = (long)src[3]; dest[4] = (long)src[4]; dest[5] = (long)src[5]; }
 
223
 
 
224
inline void lform_to_form(form &dest, lform &src) {
 
225
  dest[0] = (double)src[0]; dest[1] = (double)src[1]; dest[2] = (double)src[2];
 
226
  dest[3] = (double)src[3]; dest[4] = (double)src[4]; dest[5] = (double)src[5];}
 
227
 
 
228
// matrix operators
 
229
 
 
230
inline double matrix_det(matrix &m) {                                // |m|
 
231
  vector axb;
 
232
  memcpy(&axb, m[0], sizeof(vector));
 
233
  vec_cross(axb, m[1]);
 
234
  return vec_dot(axb, m[2]); }
 
235
 
 
236
inline void matrix_null(matrix &dest) {
 
237
  memset(dest, 0, sizeof(dest)); }
 
238
 
 
239
inline void matrix_unit(matrix &dest) {
 
240
  memset(dest, 0, sizeof(dest));
 
241
  dest[0][0] = dest[1][1] = dest[2][2] = 1.0; }
 
242
 
 
243
inline void matrix_scalar_mult(matrix &dest, double f) {        // f*m
 
244
  vec_scalar_mult(dest[0], f);
 
245
  vec_scalar_mult(dest[1], f);
 
246
  vec_scalar_mult(dest[2], f); }
 
247
 
 
248
inline void matrix_trans(matrix &dest) {                        // m^t
 
249
  double f = dest[0][1]; dest[0][1] = dest[1][0]; dest[1][0] = f;
 
250
  f = dest[0][2]; dest[0][2] = dest[2][0]; dest[2][0] = f;
 
251
  f = dest[1][2]; dest[1][2] = dest[2][1]; dest[2][1] = f; }
 
252
 
 
253
inline int matrix_inv(matrix &dest, matrix &src) {                // m^-1
 
254
  double f = matrix_det(src);
 
255
  if (fzero(f)) return 0;                                // singular matrix
 
256
  memcpy(dest[0], src[1], sizeof(vector));
 
257
  memcpy(dest[1], src[2], sizeof(vector));
 
258
  memcpy(dest[2], src[0], sizeof(vector));
 
259
  vec_cross(dest[0], src[2]);
 
260
  vec_cross(dest[1], src[0]);
 
261
  vec_cross(dest[2], src[1]);
 
262
  matrix_scalar_mult(dest, 1.0/f);
 
263
  matrix_trans(dest);
 
264
  return 0; }
 
265
 
 
266
inline void matrix_vec_dot(vector &dest, vector &src, matrix &m) { // m.a
 
267
  dest[0] = m[0][0]*src[0]+m[1][0]*src[1]+m[2][0]*src[2];
 
268
  dest[1] = m[0][1]*src[0]+m[1][1]*src[1]+m[2][1]*src[2];
 
269
  dest[2] = m[0][2]*src[0]+m[1][2]*src[1]+m[2][2]*src[2]; }
 
270
 
 
271
inline void matrix_to_shape(shape &dest, matrix &src) {                // h = m
 
272
  dest[0] = src[0][0]; dest[1] = src[1][1]; dest[2] = src[2][2];
 
273
  dest[3] = src[2][1]; dest[4] = src[2][0]; dest[5] = src[1][0]; }
 
274
 
 
275
inline void matrix_to_lmatrix(lmatrix &dest, matrix &src) {
 
276
  vec_to_lvec(dest[0], src[0]);
 
277
  vec_to_lvec(dest[1], src[1]);
 
278
  vec_to_lvec(dest[2], src[2]); }
 
279
 
 
280
inline void lmatrix_to_matrix(matrix &dest, lmatrix &src) {
 
281
  lvec_to_vec(dest[0], src[0]);
 
282
  lvec_to_vec(dest[1], src[1]);
 
283
  lvec_to_vec(dest[2], src[2]); }
 
284
 
 
285
// quaternion operators
 
286
 
 
287
inline double quat_dot(quaternion &p, quaternion &q) {                // p.q
 
288
  return p[0]*q[0]+p[1]*q[1]+p[2]*q[2]+p[3]*q[3];
 
289
}
 
290
 
 
291
inline void quat_norm(quaternion &q) {                                // q = q/|q|
 
292
  double f = sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
 
293
  q[0] /= f; q[1] /= f; q[2] /= f; q[3] /= f;
 
294
}
 
295
 
 
296
inline void quat_conj(quaternion &q) {                                // q = conj(q)
 
297
  q[1] = -q[1]; q[2] = -q[2]; q[3] = -q[3];
 
298
}
 
299
 
 
300
inline void quat_mult(quaternion &dest, quaternion &src) {        // dest *= src
 
301
  quaternion q;
 
302
  memcpy(q, dest, sizeof(quaternion));
 
303
  dest[0] =  src[0]*q[3]+src[1]*q[2]-src[2]*q[1]+src[3]*q[0];
 
304
  dest[1] = -src[0]*q[2]+src[1]*q[3]+src[2]*q[0]+src[3]*q[1];
 
305
  dest[2] =  src[0]*q[1]-src[1]*q[0]+src[2]*q[3]+src[3]*q[2];
 
306
  dest[3] = -src[0]*q[0]-src[1]*q[1]-src[2]*q[2]+src[3]*q[3];
 
307
}
 
308
 
 
309
inline void quat_div(quaternion &dest, quaternion &src) {        // dest /= src
 
310
  quaternion q;
 
311
  memcpy(q, dest, sizeof(quaternion));
 
312
  dest[0] =  src[0]*q[3]-src[1]*q[2]+src[2]*q[1]-src[3]*q[0];
 
313
  dest[1] = -src[0]*q[2]-src[1]*q[3]-src[2]*q[0]-src[3]*q[1];
 
314
  dest[2] =  src[0]*q[1]+src[1]*q[0]-src[2]*q[3]-src[3]*q[2];
 
315
  dest[3] = -src[0]*q[0]+src[1]*q[1]+src[2]*q[2]-src[3]*q[3];
 
316
}
 
317
                                                        // dest = q*src*conj(q)
 
318
inline void quat_vec_rot(vector &dest, vector &src, quaternion &q) {
 
319
  quaternion aa={q[0]*q[0], q[1]*q[1], q[2]*q[2], q[3]*q[3]};
 
320
  form ab={q[0]*q[1], q[0]*q[2], q[0]*q[3], q[1]*q[2], q[1]*q[3], q[2]*q[3]};
 
321
  dest[0] = (aa[0]+aa[1]-aa[2]-aa[3])*src[0]+
 
322
            ((ab[3]-ab[2])*src[1]+(ab[1]+ab[4])*src[2])*2.0;
 
323
  dest[1] = (aa[0]-aa[1]+aa[2]-aa[3])*src[1]+
 
324
            ((ab[2]+ab[3])*src[0]+(ab[5]-ab[0])*src[2])*2.0;
 
325
  dest[2] = (aa[0]-aa[1]-aa[2]+aa[3])*src[2]+
 
326
            ((ab[4]-ab[1])*src[0]+(ab[0]+ab[5])*src[1])*2.0;
 
327
}
 
328
 
 
329
// vector4 operators
 
330
 
 
331
inline void vec4_null(vector4 &dest) {
 
332
  memset(dest, 0, sizeof(vector4));
 
333
}
 
334
 
 
335
inline double vec4_dot(vector4 &a, vector4 &b) {
 
336
  return a[0]*b[0]+a[1]*b[1]+a[2]*b[2]+a[3]*b[3]; }
 
337
 
 
338
// form operators
 
339
 
 
340
inline void form4_null(form4 &dest) {
 
341
  memset(dest, 0, sizeof(form4)); }
 
342
 
 
343
inline void form4_unit(form4 &dest) {
 
344
  memset(dest, 0, sizeof(form4));
 
345
  dest[0] = dest[1] = dest[2] = dest[3] = 1.0; }
 
346
 
 
347
inline double form4_det(form4 &m) {
 
348
  register double f = m[6]*m[7]-m[5]*m[8];
 
349
  return m[0]*(
 
350
      m[1]*(m[2]*m[3]-m[4]*m[4])+
 
351
      m[5]*(2.0*m[4]*m[7]-m[2]*m[5])-m[3]*m[7]*m[7])+f*f+
 
352
    -m[1]*(m[2]*m[6]*m[6]+m[8]*(m[3]*m[8]-2.0*m[4]*m[6]))+
 
353
    m[9]*(
 
354
        2.0*(m[2]*m[5]*m[6]+m[3]*m[7]*m[8]-m[4]*(m[6]*m[7]+m[5]*m[8]))+
 
355
        m[9]*(m[4]*m[4]-m[2]*m[3])); }
 
356
 
 
357
inline int form4_inv(form4 &m_inv, form4 &m) {
 
358
  register double det = form4_det(m);
 
359
  if (fzero(det)) return 0;
 
360
  m_inv[0] = (m[1]*(m[2]*m[3]-m[4]*m[4])+
 
361
      m[5]*(2.0*m[4]*m[7]-m[2]*m[5])-m[3]*m[7]*m[7])/det;
 
362
  m_inv[1] = (m[0]*(m[2]*m[3]-m[4]*m[4])+
 
363
      m[6]*(2.0*m[4]*m[8]-m[2]*m[6])-m[3]*m[8]*m[8])/det;
 
364
  m_inv[2] = (m[0]*(m[1]*m[3]-m[5]*m[5])+
 
365
      m[6]*(2.0*m[5]*m[9]-m[1]*m[6])-m[3]*m[9]*m[9])/det;
 
366
  m_inv[3] = (m[0]*(m[1]*m[2]-m[7]*m[7])+
 
367
      m[8]*(2.0*m[7]*m[9]-m[1]*m[8])-m[2]*m[9]*m[9])/det;
 
368
  m_inv[4] = (m[0]*(m[5]*m[7]-m[1]*m[4])+m[1]*m[6]*m[8]+
 
369
      m[9]*(m[4]*m[9]-m[6]*m[7]-m[5]*m[8]))/det;
 
370
  m_inv[5] = (m[0]*(m[4]*m[7]-m[2]*m[5])+m[2]*m[6]*m[9]+
 
371
      m[8]*(m[5]*m[8]-m[6]*m[7]-m[4]*m[9]))/det;
 
372
  m_inv[6] = (m[1]*(m[4]*m[8]-m[2]*m[6])+m[2]*m[5]*m[9]+
 
373
      m[7]*(m[6]*m[7]-m[5]*m[8]-m[4]*m[9]))/det;
 
374
  m_inv[7] = (m[0]*(m[4]*m[5]-m[3]*m[7])+m[3]*m[8]*m[9]+
 
375
      m[6]*(m[6]*m[7]-m[5]*m[8]-m[4]*m[9]))/det;
 
376
  m_inv[8] = (m[1]*(m[4]*m[6]-m[3]*m[8])+m[3]*m[7]*m[9]+
 
377
      m[5]*(m[5]*m[8]-m[6]*m[7]-m[4]*m[9]))/det;
 
378
  m_inv[9] = (m[2]*(m[5]*m[6]-m[3]*m[9])+m[3]*m[7]*m[8]+
 
379
      m[4]*(m[4]*m[9]-m[6]*m[7]-m[5]*m[8]))/det;
 
380
  return 1; }
 
381
 
 
382
inline void form4_vec4_dot(vector4 &dest, form4 &m) {
 
383
  vector4 a;
 
384
  memcpy(a, dest, sizeof(vector4));
 
385
  dest[0] = m[0]*a[0]+m[9]*a[1]+m[7]*a[2]+m[6]*a[3];
 
386
  dest[1] = m[9]*a[0]+m[1]*a[1]+m[6]*a[2]+m[5]*a[3];
 
387
  dest[2] = m[8]*a[0]+m[7]*a[1]+m[2]*a[2]+m[4]*a[3];
 
388
  dest[3] = m[6]*a[0]+m[5]*a[1]+m[4]*a[2]+m[3]*a[3]; }
 
389
 
 
390
// square regression: y = eqn[0] + eqn[1]*x + eqn[2]*x*x
 
391
 
 
392
inline int regress2(vector &eqn, int order, double *x, double *y, int n) {
 
393
  form xtx = FORM_NULL, xtx_inv;
 
394
  vector xty = VECTOR_NULL;
 
395
  double xn, xi, yi;
 
396
  int i;
 
397
 
 
398
  vec_null(eqn);
 
399
  xtx[0] = n;
 
400
  if ((order = order%2)<0) order = -order;                // max: quad regress
 
401
  if (order<1) xtx[1] = 1.0;
 
402
  if (order<2) xtx[2] = 1.0;
 
403
  for (i=0; i<n; ++i) {
 
404
    xty[0] += (yi = y[i]);
 
405
    if (order<1) continue;
 
406
    xty[1] += yi*(xi = xn = x[i]); xtx[5] += xn; xtx[1] += (xn *= xi);
 
407
    if (order<2) continue;
 
408
    xty[2] += yi*xn; xtx[3] += (xn *= xi); xtx[2] += xn*xi;
 
409
  }
 
410
  if (order>1) xtx[4] = xtx[2];
 
411
  if (!form_inv(xtx_inv, xtx)) return 0;
 
412
  memcpy(eqn, xty, sizeof(vector));
 
413
  form_vec_dot(eqn, xtx_inv);
 
414
  return 1; }
 
415
 
 
416
// cubic regression: y = eqn[0] + eqn[1]*x + eqn[2]*x*x + eqn[3]*x*x*x
 
417
 
 
418
inline int regress3(vector4 &eqn, int order, double *x, double *y, int n) {
 
419
  form4 xtx = FORM4_NULL, xtx_inv;
 
420
  vector4 xty = VECTOR4_NULL;
 
421
  double xn, xi, yi;
 
422
  int i;
 
423
 
 
424
  vec4_null(eqn);
 
425
  xtx[0] = n;
 
426
  if ((order = order%3)<0) order = -order;                // max: cubic regress
 
427
  if (order<1) xtx[1] = 1.0;
 
428
  if (order<2) xtx[2] = 1.0;
 
429
  if (order<3) xtx[3] = 1.0;
 
430
  for (i=0; i<n; ++i) {
 
431
    xty[0] += (yi = y[i]);
 
432
    if (order<1) continue;
 
433
    xty[1] += yi*(xi = xn = x[i]); xtx[9] += xn; xtx[1] += (xn *= xi);
 
434
    if (order<2) continue;
 
435
    xty[2] += yi*xn; xtx[7] += (xn *= xi); xtx[2] += xn*xi;
 
436
    if (order<3) continue;
 
437
    xty[3] += yi*xn; xtx[4] += (xn *= xi*xi); xtx[3] += xn*xi;
 
438
  }
 
439
  if (order>1) xtx[8] = xtx[1];
 
440
  if (order>2) { xtx[6] = xtx[7]; xtx[5] = xtx[2]; }
 
441
  if (!form4_inv(xtx_inv, xtx)) return 0;
 
442
  memcpy(eqn, xty, sizeof(vector4));
 
443
  form4_vec4_dot(eqn, xtx_inv);
 
444
  return 1; }
 
445
 
 
446
}
 
447
 
 
448
#endif