Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F86980475
math_vector.h
No One
Temporary
Actions
Download File
Edit File
Delete File
View Transforms
Subscribe
Mute Notifications
Award Token
Subscribers
None
File Metadata
Details
File Info
Storage
Attached
Created
Wed, Oct 9, 19:15
Size
16 KB
Mime Type
text/x-c
Expires
Fri, Oct 11, 19:15 (1 d, 23 h)
Engine
blob
Format
Raw Data
Handle
21514686
Attached To
rLAMMPS lammps
math_vector.h
View Options
/* -*- c++ -*- ----------------------------------------------------------
LAMMPS - Large-scale Atomic/Molecular Massively Parallel Simulator
http://lammps.sandia.gov, Sandia National Laboratories
Steve Plimpton, sjplimp@sandia.gov
Copyright (2003) Sandia Corporation. Under the terms of Contract
DE-AC04-94AL85000 with Sandia Corporation, the U.S. Government retains
certain rights in this software. This software is distributed under
the GNU General Public License.
See the README file in the top-level LAMMPS directory.
------------------------------------------------------------------------- */
/* ----------------------------------------------------------------------
Contributing author: Pieter J. in 't Veld (SNL)
------------------------------------------------------------------------- */
#ifndef LMP_MATH_VECTOR_H
#define LMP_MATH_VECTOR_H
#include <math.h>
#include <string.h>
#define VECTOR_NULL {0, 0, 0}
#define SHAPE_NULL {0, 0, 0, 0, 0, 0}
#define FORM_NULL {0, 0, 0, 0, 0, 0}
#define MATRIX_NULL {VECTOR_NULL, VECTOR_NULL, VECTOR_NULL}
#define VECTOR4_NULL {0, 0, 0, 0}
#define QUATERNION_NULL {0, 0, 0, 0}
#define FORM4_NULL {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
#define FZERO 1e-15
#define fzero(x) (((x)>-FZERO) && ((x)<FZERO))
namespace
LAMMPS_NS
{
typedef
double
vector
[
3
];
// 0:x 1:y 2:z
typedef
int
lvector
[
3
];
typedef
double
shape
[
6
];
// 0:xx 1:yy 2:zz 3:zy 4:zx 5:yx
typedef
int
lshape
[
6
];
// xy=0 xz=0 yz=0;
typedef
double
form
[
6
];
// 0:xx 1:yy 2:zz 3:zy 4:zx 5:yx
typedef
int
lform
[
6
];
// xy=yx xz=zx yz=zy;
typedef
vector
matrix
[
3
];
// 00:xx 11:yy 22:zz 21:zy 20:zx 10:yx
typedef
lvector
lmatrix
[
3
];
typedef
double
vector4
[
4
];
// 4D vector
typedef
double
form4
[
10
];
// 0:00 1:11 2:22 3:33 4:32
// 5:31 6:30 7:21 8:20 9:10
// 01=10 02=20 03=30 etc
typedef
double
quaternion
[
4
];
// quaternion
// vector operators
inline
double
vec_dot
(
vector
&
a
,
vector
&
b
)
{
// a.b
return
a
[
0
]
*
b
[
0
]
+
a
[
1
]
*
b
[
1
]
+
a
[
2
]
*
b
[
2
];
}
inline
void
vec_null
(
vector
&
dest
)
{
memset
(
dest
,
0
,
sizeof
(
vector
));
}
inline
void
vec_neg
(
vector
&
dest
)
{
// -a
dest
[
0
]
=
-
dest
[
0
];
dest
[
1
]
=
-
dest
[
1
];
dest
[
2
]
=
-
dest
[
2
];
}
inline
void
vec_norm
(
vector
&
dest
)
{
// a/|a|
register
double
f
=
sqrt
(
vec_dot
(
dest
,
dest
));
dest
[
0
]
/=
f
;
dest
[
1
]
/=
f
;
dest
[
2
]
/=
f
;
}
inline
void
vec_add
(
vector
&
dest
,
vector
&
src
)
{
// a+b
dest
[
0
]
+=
src
[
0
];
dest
[
1
]
+=
src
[
1
];
dest
[
2
]
+=
src
[
2
];
}
inline
void
vec_subtr
(
vector
&
dest
,
vector
&
src
)
{
// a-b
dest
[
0
]
-=
src
[
0
];
dest
[
1
]
-=
src
[
1
];
dest
[
2
]
-=
src
[
2
];
}
inline
void
vec_mult
(
vector
&
dest
,
vector
&
src
)
{
// a*b
dest
[
0
]
*=
src
[
0
];
dest
[
1
]
*=
src
[
1
];
dest
[
2
]
*=
src
[
2
];
}
inline
void
vec_div
(
vector
&
dest
,
vector
&
src
)
{
// a/b
dest
[
0
]
/=
src
[
0
];
dest
[
1
]
/=
src
[
1
];
dest
[
2
]
/=
src
[
2
];
}
inline
void
vec_cross
(
vector
&
dest
,
vector
&
src
)
{
// a x b
vector
t
=
{
dest
[
1
]
*
src
[
2
]
-
dest
[
2
]
*
src
[
1
],
dest
[
2
]
*
src
[
0
]
-
dest
[
0
]
*
src
[
2
],
dest
[
0
]
*
src
[
1
]
-
dest
[
1
]
*
src
[
0
]};
memcpy
(
dest
,
t
,
sizeof
(
vector
));
}
inline
void
vec_scalar_mult
(
vector
&
dest
,
double
f
)
{
// f*a
dest
[
0
]
*=
f
;
dest
[
1
]
*=
f
;
dest
[
2
]
*=
f
;
}
inline
void
vec_to_lvec
(
lvector
&
dest
,
vector
&
src
)
{
dest
[
0
]
=
(
int
)
src
[
0
];
dest
[
1
]
=
(
int
)
src
[
1
];
dest
[
2
]
=
(
int
)
src
[
2
];
}
inline
void
lvec_to_vec
(
vector
&
dest
,
lvector
&
src
)
{
dest
[
0
]
=
(
double
)
src
[
0
];
dest
[
1
]
=
(
double
)
src
[
1
];
dest
[
2
]
=
(
double
)
src
[
2
];
}
// shape operators
inline
double
shape_det
(
shape
&
s
)
{
return
s
[
0
]
*
s
[
1
]
*
s
[
2
];
}
inline
void
shape_null
(
shape
&
dest
)
{
memset
(
dest
,
0
,
sizeof
(
shape
));
}
inline
void
shape_unit
(
shape
&
dest
)
{
memset
(
dest
,
0
,
sizeof
(
shape
));
dest
[
0
]
=
dest
[
1
]
=
dest
[
2
]
=
1.0
;
}
inline
void
shape_add
(
shape
&
dest
,
shape
&
src
)
{
// h_a+h_b
dest
[
0
]
+=
src
[
0
];
dest
[
1
]
+=
src
[
1
];
dest
[
2
]
+=
src
[
2
];
dest
[
3
]
+=
src
[
3
];
dest
[
4
]
+=
src
[
4
];
dest
[
5
]
+=
src
[
5
];
}
inline
void
shape_subtr
(
shape
&
dest
,
shape
&
src
)
{
// h_a-h_b
dest
[
0
]
-=
src
[
0
];
dest
[
1
]
-=
src
[
1
];
dest
[
2
]
-=
src
[
2
];
dest
[
3
]
-=
src
[
3
];
dest
[
4
]
-=
src
[
4
];
dest
[
5
]
-=
src
[
5
];
}
inline
void
shape_inv
(
shape
&
h_inv
,
shape
&
h
)
{
// h^-1
h_inv
[
0
]
=
1.0
/
h
[
0
];
h_inv
[
1
]
=
1.0
/
h
[
1
];
h_inv
[
2
]
=
1.0
/
h
[
2
];
h_inv
[
3
]
=
-
h
[
3
]
/
(
h
[
1
]
*
h
[
2
]);
h_inv
[
4
]
=
(
h
[
3
]
*
h
[
5
]
-
h
[
1
]
*
h
[
4
])
/
(
h
[
0
]
*
h
[
1
]
*
h
[
2
]);
h_inv
[
5
]
=
-
h
[
5
]
/
(
h
[
0
]
*
h
[
1
]);
}
inline
void
shape_dot
(
shape
&
dest
,
shape
&
src
)
{
// h_a.h_b
dest
[
3
]
=
dest
[
1
]
*
src
[
3
]
+
dest
[
3
]
*
src
[
2
];
dest
[
4
]
=
dest
[
0
]
*
src
[
4
]
+
dest
[
5
]
*
src
[
3
]
+
dest
[
4
]
*
src
[
2
];
dest
[
5
]
=
dest
[
0
]
*
src
[
5
]
+
dest
[
5
]
*
src
[
1
];
dest
[
0
]
*=
src
[
0
];
dest
[
1
]
*=
src
[
1
];
dest
[
2
]
*=
src
[
2
];
}
inline
void
shape_scalar_mult
(
shape
&
dest
,
double
f
)
{
// f*h
dest
[
0
]
*=
f
;
dest
[
1
]
*=
f
;
dest
[
2
]
*=
f
;
dest
[
3
]
*=
f
;
dest
[
4
]
*=
f
;
dest
[
5
]
*=
f
;
}
inline
void
shape_vec_dot
(
vector
&
dest
,
vector
&
src
,
shape
&
h
)
{
// h.a
dest
[
0
]
=
h
[
0
]
*
src
[
0
]
+
h
[
5
]
*
src
[
1
]
+
h
[
4
]
*
src
[
2
];
dest
[
1
]
=
h
[
1
]
*
src
[
1
]
+
h
[
3
]
*
src
[
2
];
dest
[
2
]
=
h
[
2
]
*
src
[
2
];
}
inline
void
vec_shape_dot
(
vector
&
dest
,
shape
&
h
,
vector
&
src
)
{
// a.h
dest
[
2
]
=
h
[
4
]
*
src
[
0
]
+
h
[
3
]
*
src
[
1
]
+
h
[
2
]
*
src
[
2
];
dest
[
1
]
=
h
[
5
]
*
src
[
0
]
+
h
[
1
]
*
src
[
1
];
dest
[
0
]
=
h
[
0
]
*
src
[
0
];
}
inline
void
shape_to_matrix
(
matrix
&
dest
,
shape
&
h
)
{
// m = h
dest
[
0
][
0
]
=
h
[
0
];
dest
[
1
][
0
]
=
h
[
5
];
dest
[
2
][
0
]
=
h
[
4
];
dest
[
0
][
1
]
=
0.0
;
dest
[
1
][
1
]
=
h
[
1
];
dest
[
2
][
1
]
=
h
[
3
];
dest
[
0
][
2
]
=
0.0
;
dest
[
1
][
2
]
=
0.0
;
dest
[
2
][
2
]
=
h
[
2
];
}
inline
void
shape_to_lshape
(
lshape
&
dest
,
shape
&
src
)
{
dest
[
0
]
=
(
long
)
src
[
0
];
dest
[
1
]
=
(
long
)
src
[
1
];
dest
[
2
]
=
(
long
)
src
[
2
];
dest
[
3
]
=
(
long
)
src
[
3
];
dest
[
4
]
=
(
long
)
src
[
4
];
dest
[
5
]
=
(
long
)
src
[
5
];
}
inline
void
lshape_to_shape
(
shape
&
dest
,
lshape
&
src
)
{
dest
[
0
]
=
(
double
)
src
[
0
];
dest
[
1
]
=
(
double
)
src
[
1
];
dest
[
2
]
=
(
double
)
src
[
2
];
dest
[
3
]
=
(
double
)
src
[
3
];
dest
[
4
]
=
(
double
)
src
[
4
];
dest
[
5
]
=
(
double
)
src
[
5
];}
// form operators
inline
double
form_det
(
form
&
m
)
{
// |m|
return
m
[
0
]
*
(
m
[
1
]
*
m
[
2
]
-
m
[
3
]
*
m
[
3
])
+
m
[
4
]
*
(
2.0
*
m
[
3
]
*
m
[
5
]
-
m
[
1
]
*
m
[
4
])
-
m
[
2
]
*
m
[
5
]
*
m
[
5
];
}
inline
void
form_null
(
form
&
dest
)
{
memset
(
dest
,
0
,
sizeof
(
form
));
}
inline
void
form_unit
(
form
&
dest
)
{
memset
(
dest
,
0
,
sizeof
(
form
));
dest
[
0
]
=
dest
[
1
]
=
dest
[
2
]
=
1.0
;
}
inline
void
form_add
(
form
&
dest
,
form
&
src
)
{
// m_a+m_b
dest
[
0
]
+=
src
[
0
];
dest
[
1
]
+=
src
[
1
];
dest
[
2
]
+=
src
[
2
];
dest
[
3
]
+=
src
[
3
];
dest
[
4
]
+=
src
[
4
];
dest
[
5
]
+=
src
[
5
];
}
inline
void
form_subtr
(
shape
&
dest
,
form
&
src
)
{
// m_a-m_b
dest
[
0
]
-=
src
[
0
];
dest
[
1
]
-=
src
[
1
];
dest
[
2
]
-=
src
[
2
];
dest
[
3
]
-=
src
[
3
];
dest
[
4
]
-=
src
[
4
];
dest
[
5
]
-=
src
[
5
];
}
inline
int
form_inv
(
form
&
m_inv
,
form
&
m
)
{
// m^-1
register
double
det
=
form_det
(
m
);
if
(
fzero
(
det
))
return
0
;
m_inv
[
0
]
=
(
m
[
1
]
*
m
[
2
]
-
m
[
3
]
*
m
[
3
])
/
det
;
m_inv
[
1
]
=
(
m
[
0
]
*
m
[
2
]
-
m
[
4
]
*
m
[
4
])
/
det
;
m_inv
[
2
]
=
(
m
[
0
]
*
m
[
1
]
-
m
[
5
]
*
m
[
5
])
/
det
;
m_inv
[
3
]
=
(
m
[
4
]
*
m
[
5
]
-
m
[
0
]
*
m
[
3
])
/
det
;
m_inv
[
4
]
=
(
m
[
3
]
*
m
[
5
]
-
m
[
1
]
*
m
[
4
])
/
det
;
m_inv
[
5
]
=
(
m
[
3
]
*
m
[
4
]
-
m
[
2
]
*
m
[
5
])
/
det
;
return
1
;
}
inline
void
form_dot
(
form
&
dest
,
form
&
src
)
{
// m_a.m_b
form
m
;
memcpy
(
m
,
dest
,
sizeof
(
form
));
dest
[
0
]
=
m
[
0
]
*
src
[
0
]
+
m
[
4
]
*
src
[
4
]
+
m
[
5
]
*
src
[
5
];
dest
[
1
]
=
m
[
1
]
*
src
[
1
]
+
m
[
3
]
*
src
[
3
]
+
m
[
5
]
*
src
[
5
];
dest
[
2
]
=
m
[
2
]
*
src
[
2
]
+
m
[
3
]
*
src
[
3
]
+
m
[
4
]
*
src
[
4
];
dest
[
3
]
=
m
[
3
]
*
src
[
2
]
+
m
[
1
]
*
src
[
3
]
+
m
[
5
]
*
src
[
4
];
dest
[
4
]
=
m
[
4
]
*
src
[
2
]
+
m
[
5
]
*
src
[
3
]
+
m
[
0
]
*
src
[
4
];
dest
[
5
]
=
m
[
5
]
*
src
[
1
]
+
m
[
4
]
*
src
[
3
]
+
m
[
0
]
*
src
[
5
];
}
inline
void
form_vec_dot
(
vector
&
dest
,
form
&
m
)
{
// m.a
vector
a
;
memcpy
(
a
,
dest
,
sizeof
(
vector
));
dest
[
0
]
=
m
[
0
]
*
a
[
0
]
+
m
[
5
]
*
a
[
1
]
+
m
[
4
]
*
a
[
2
];
dest
[
1
]
=
m
[
5
]
*
a
[
0
]
+
m
[
1
]
*
a
[
1
]
+
m
[
3
]
*
a
[
2
];
dest
[
2
]
=
m
[
4
]
*
a
[
0
]
+
m
[
3
]
*
a
[
1
]
+
m
[
2
]
*
a
[
2
];
}
inline
void
form_to_lform
(
lform
&
dest
,
form
&
src
)
{
dest
[
0
]
=
(
long
)
src
[
0
];
dest
[
1
]
=
(
long
)
src
[
1
];
dest
[
2
]
=
(
long
)
src
[
2
];
dest
[
3
]
=
(
long
)
src
[
3
];
dest
[
4
]
=
(
long
)
src
[
4
];
dest
[
5
]
=
(
long
)
src
[
5
];
}
inline
void
lform_to_form
(
form
&
dest
,
lform
&
src
)
{
dest
[
0
]
=
(
double
)
src
[
0
];
dest
[
1
]
=
(
double
)
src
[
1
];
dest
[
2
]
=
(
double
)
src
[
2
];
dest
[
3
]
=
(
double
)
src
[
3
];
dest
[
4
]
=
(
double
)
src
[
4
];
dest
[
5
]
=
(
double
)
src
[
5
];}
// matrix operators
inline
double
matrix_det
(
matrix
&
m
)
{
// |m|
vector
axb
;
memcpy
(
&
axb
,
m
[
0
],
sizeof
(
vector
));
vec_cross
(
axb
,
m
[
1
]);
return
vec_dot
(
axb
,
m
[
2
]);
}
inline
void
matrix_null
(
matrix
&
dest
)
{
memset
(
dest
,
0
,
sizeof
(
dest
));
}
inline
void
matrix_unit
(
matrix
&
dest
)
{
memset
(
dest
,
0
,
sizeof
(
dest
));
dest
[
0
][
0
]
=
dest
[
1
][
1
]
=
dest
[
2
][
2
]
=
1.0
;
}
inline
void
matrix_scalar_mult
(
matrix
&
dest
,
double
f
)
{
// f*m
vec_scalar_mult
(
dest
[
0
],
f
);
vec_scalar_mult
(
dest
[
1
],
f
);
vec_scalar_mult
(
dest
[
2
],
f
);
}
inline
void
matrix_trans
(
matrix
&
dest
)
{
// m^t
double
f
=
dest
[
0
][
1
];
dest
[
0
][
1
]
=
dest
[
1
][
0
];
dest
[
1
][
0
]
=
f
;
f
=
dest
[
0
][
2
];
dest
[
0
][
2
]
=
dest
[
2
][
0
];
dest
[
2
][
0
]
=
f
;
f
=
dest
[
1
][
2
];
dest
[
1
][
2
]
=
dest
[
2
][
1
];
dest
[
2
][
1
]
=
f
;
}
inline
int
matrix_inv
(
matrix
&
dest
,
matrix
&
src
)
{
// m^-1
double
f
=
matrix_det
(
src
);
if
(
fzero
(
f
))
return
0
;
// singular matrix
memcpy
(
dest
[
0
],
src
[
1
],
sizeof
(
vector
));
memcpy
(
dest
[
1
],
src
[
2
],
sizeof
(
vector
));
memcpy
(
dest
[
2
],
src
[
0
],
sizeof
(
vector
));
vec_cross
(
dest
[
0
],
src
[
2
]);
vec_cross
(
dest
[
1
],
src
[
0
]);
vec_cross
(
dest
[
2
],
src
[
1
]);
matrix_scalar_mult
(
dest
,
1.0
/
f
);
matrix_trans
(
dest
);
return
0
;
}
inline
void
matrix_vec_dot
(
vector
&
dest
,
vector
&
src
,
matrix
&
m
)
{
// m.a
dest
[
0
]
=
m
[
0
][
0
]
*
src
[
0
]
+
m
[
1
][
0
]
*
src
[
1
]
+
m
[
2
][
0
]
*
src
[
2
];
dest
[
1
]
=
m
[
0
][
1
]
*
src
[
0
]
+
m
[
1
][
1
]
*
src
[
1
]
+
m
[
2
][
1
]
*
src
[
2
];
dest
[
2
]
=
m
[
0
][
2
]
*
src
[
0
]
+
m
[
1
][
2
]
*
src
[
1
]
+
m
[
2
][
2
]
*
src
[
2
];
}
inline
void
matrix_to_shape
(
shape
&
dest
,
matrix
&
src
)
{
// h = m
dest
[
0
]
=
src
[
0
][
0
];
dest
[
1
]
=
src
[
1
][
1
];
dest
[
2
]
=
src
[
2
][
2
];
dest
[
3
]
=
src
[
2
][
1
];
dest
[
4
]
=
src
[
2
][
0
];
dest
[
5
]
=
src
[
1
][
0
];
}
inline
void
matrix_to_lmatrix
(
lmatrix
&
dest
,
matrix
&
src
)
{
vec_to_lvec
(
dest
[
0
],
src
[
0
]);
vec_to_lvec
(
dest
[
1
],
src
[
1
]);
vec_to_lvec
(
dest
[
2
],
src
[
2
]);
}
inline
void
lmatrix_to_matrix
(
matrix
&
dest
,
lmatrix
&
src
)
{
lvec_to_vec
(
dest
[
0
],
src
[
0
]);
lvec_to_vec
(
dest
[
1
],
src
[
1
]);
lvec_to_vec
(
dest
[
2
],
src
[
2
]);
}
// quaternion operators
inline
double
quat_dot
(
quaternion
&
p
,
quaternion
&
q
)
{
// p.q
return
p
[
0
]
*
q
[
0
]
+
p
[
1
]
*
q
[
1
]
+
p
[
2
]
*
q
[
2
]
+
p
[
3
]
*
q
[
3
];
}
inline
void
quat_norm
(
quaternion
&
q
)
{
// q = q/|q|
double
f
=
sqrt
(
q
[
0
]
*
q
[
0
]
+
q
[
1
]
*
q
[
1
]
+
q
[
2
]
*
q
[
2
]
+
q
[
3
]
*
q
[
3
]);
q
[
0
]
/=
f
;
q
[
1
]
/=
f
;
q
[
2
]
/=
f
;
q
[
3
]
/=
f
;
}
inline
void
quat_conj
(
quaternion
&
q
)
{
// q = conj(q)
q
[
1
]
=
-
q
[
1
];
q
[
2
]
=
-
q
[
2
];
q
[
3
]
=
-
q
[
3
];
}
inline
void
quat_mult
(
quaternion
&
dest
,
quaternion
&
src
)
{
// dest *= src
quaternion
q
;
memcpy
(
q
,
dest
,
sizeof
(
quaternion
));
dest
[
0
]
=
src
[
0
]
*
q
[
3
]
+
src
[
1
]
*
q
[
2
]
-
src
[
2
]
*
q
[
1
]
+
src
[
3
]
*
q
[
0
];
dest
[
1
]
=
-
src
[
0
]
*
q
[
2
]
+
src
[
1
]
*
q
[
3
]
+
src
[
2
]
*
q
[
0
]
+
src
[
3
]
*
q
[
1
];
dest
[
2
]
=
src
[
0
]
*
q
[
1
]
-
src
[
1
]
*
q
[
0
]
+
src
[
2
]
*
q
[
3
]
+
src
[
3
]
*
q
[
2
];
dest
[
3
]
=
-
src
[
0
]
*
q
[
0
]
-
src
[
1
]
*
q
[
1
]
-
src
[
2
]
*
q
[
2
]
+
src
[
3
]
*
q
[
3
];
}
inline
void
quat_div
(
quaternion
&
dest
,
quaternion
&
src
)
{
// dest /= src
quaternion
q
;
memcpy
(
q
,
dest
,
sizeof
(
quaternion
));
dest
[
0
]
=
src
[
0
]
*
q
[
3
]
-
src
[
1
]
*
q
[
2
]
+
src
[
2
]
*
q
[
1
]
-
src
[
3
]
*
q
[
0
];
dest
[
1
]
=
-
src
[
0
]
*
q
[
2
]
-
src
[
1
]
*
q
[
3
]
-
src
[
2
]
*
q
[
0
]
-
src
[
3
]
*
q
[
1
];
dest
[
2
]
=
src
[
0
]
*
q
[
1
]
+
src
[
1
]
*
q
[
0
]
-
src
[
2
]
*
q
[
3
]
-
src
[
3
]
*
q
[
2
];
dest
[
3
]
=
-
src
[
0
]
*
q
[
0
]
+
src
[
1
]
*
q
[
1
]
+
src
[
2
]
*
q
[
2
]
-
src
[
3
]
*
q
[
3
];
}
// dest = q*src*conj(q)
inline
void
quat_vec_rot
(
vector
&
dest
,
vector
&
src
,
quaternion
&
q
)
{
quaternion
aa
=
{
q
[
0
]
*
q
[
0
],
q
[
1
]
*
q
[
1
],
q
[
2
]
*
q
[
2
],
q
[
3
]
*
q
[
3
]};
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
]};
dest
[
0
]
=
(
aa
[
0
]
+
aa
[
1
]
-
aa
[
2
]
-
aa
[
3
])
*
src
[
0
]
+
((
ab
[
3
]
-
ab
[
2
])
*
src
[
1
]
+
(
ab
[
1
]
+
ab
[
4
])
*
src
[
2
])
*
2.0
;
dest
[
1
]
=
(
aa
[
0
]
-
aa
[
1
]
+
aa
[
2
]
-
aa
[
3
])
*
src
[
1
]
+
((
ab
[
2
]
+
ab
[
3
])
*
src
[
0
]
+
(
ab
[
5
]
-
ab
[
0
])
*
src
[
2
])
*
2.0
;
dest
[
2
]
=
(
aa
[
0
]
-
aa
[
1
]
-
aa
[
2
]
+
aa
[
3
])
*
src
[
2
]
+
((
ab
[
4
]
-
ab
[
1
])
*
src
[
0
]
+
(
ab
[
0
]
+
ab
[
5
])
*
src
[
1
])
*
2.0
;
}
// vector4 operators
inline
void
vec4_null
(
vector4
&
dest
)
{
memset
(
dest
,
0
,
sizeof
(
vector4
));
}
inline
double
vec4_dot
(
vector4
&
a
,
vector4
&
b
)
{
return
a
[
0
]
*
b
[
0
]
+
a
[
1
]
*
b
[
1
]
+
a
[
2
]
*
b
[
2
]
+
a
[
3
]
*
b
[
3
];
}
// form operators
inline
void
form4_null
(
form4
&
dest
)
{
memset
(
dest
,
0
,
sizeof
(
form4
));
}
inline
void
form4_unit
(
form4
&
dest
)
{
memset
(
dest
,
0
,
sizeof
(
form4
));
dest
[
0
]
=
dest
[
1
]
=
dest
[
2
]
=
dest
[
3
]
=
1.0
;
}
inline
double
form4_det
(
form4
&
m
)
{
register
double
f
=
m
[
6
]
*
m
[
7
]
-
m
[
5
]
*
m
[
8
];
return
m
[
0
]
*
(
m
[
1
]
*
(
m
[
2
]
*
m
[
3
]
-
m
[
4
]
*
m
[
4
])
+
m
[
5
]
*
(
2.0
*
m
[
4
]
*
m
[
7
]
-
m
[
2
]
*
m
[
5
])
-
m
[
3
]
*
m
[
7
]
*
m
[
7
])
+
f
*
f
+
-
m
[
1
]
*
(
m
[
2
]
*
m
[
6
]
*
m
[
6
]
+
m
[
8
]
*
(
m
[
3
]
*
m
[
8
]
-
2.0
*
m
[
4
]
*
m
[
6
]))
+
m
[
9
]
*
(
2.0
*
(
m
[
2
]
*
m
[
5
]
*
m
[
6
]
+
m
[
3
]
*
m
[
7
]
*
m
[
8
]
-
m
[
4
]
*
(
m
[
6
]
*
m
[
7
]
+
m
[
5
]
*
m
[
8
]))
+
m
[
9
]
*
(
m
[
4
]
*
m
[
4
]
-
m
[
2
]
*
m
[
3
]));
}
inline
int
form4_inv
(
form4
&
m_inv
,
form4
&
m
)
{
register
double
det
=
form4_det
(
m
);
if
(
fzero
(
det
))
return
0
;
m_inv
[
0
]
=
(
m
[
1
]
*
(
m
[
2
]
*
m
[
3
]
-
m
[
4
]
*
m
[
4
])
+
m
[
5
]
*
(
2.0
*
m
[
4
]
*
m
[
7
]
-
m
[
2
]
*
m
[
5
])
-
m
[
3
]
*
m
[
7
]
*
m
[
7
])
/
det
;
m_inv
[
1
]
=
(
m
[
0
]
*
(
m
[
2
]
*
m
[
3
]
-
m
[
4
]
*
m
[
4
])
+
m
[
6
]
*
(
2.0
*
m
[
4
]
*
m
[
8
]
-
m
[
2
]
*
m
[
6
])
-
m
[
3
]
*
m
[
8
]
*
m
[
8
])
/
det
;
m_inv
[
2
]
=
(
m
[
0
]
*
(
m
[
1
]
*
m
[
3
]
-
m
[
5
]
*
m
[
5
])
+
m
[
6
]
*
(
2.0
*
m
[
5
]
*
m
[
9
]
-
m
[
1
]
*
m
[
6
])
-
m
[
3
]
*
m
[
9
]
*
m
[
9
])
/
det
;
m_inv
[
3
]
=
(
m
[
0
]
*
(
m
[
1
]
*
m
[
2
]
-
m
[
7
]
*
m
[
7
])
+
m
[
8
]
*
(
2.0
*
m
[
7
]
*
m
[
9
]
-
m
[
1
]
*
m
[
8
])
-
m
[
2
]
*
m
[
9
]
*
m
[
9
])
/
det
;
m_inv
[
4
]
=
(
m
[
0
]
*
(
m
[
5
]
*
m
[
7
]
-
m
[
1
]
*
m
[
4
])
+
m
[
1
]
*
m
[
6
]
*
m
[
8
]
+
m
[
9
]
*
(
m
[
4
]
*
m
[
9
]
-
m
[
6
]
*
m
[
7
]
-
m
[
5
]
*
m
[
8
]))
/
det
;
m_inv
[
5
]
=
(
m
[
0
]
*
(
m
[
4
]
*
m
[
7
]
-
m
[
2
]
*
m
[
5
])
+
m
[
2
]
*
m
[
6
]
*
m
[
9
]
+
m
[
8
]
*
(
m
[
5
]
*
m
[
8
]
-
m
[
6
]
*
m
[
7
]
-
m
[
4
]
*
m
[
9
]))
/
det
;
m_inv
[
6
]
=
(
m
[
1
]
*
(
m
[
4
]
*
m
[
8
]
-
m
[
2
]
*
m
[
6
])
+
m
[
2
]
*
m
[
5
]
*
m
[
9
]
+
m
[
7
]
*
(
m
[
6
]
*
m
[
7
]
-
m
[
5
]
*
m
[
8
]
-
m
[
4
]
*
m
[
9
]))
/
det
;
m_inv
[
7
]
=
(
m
[
0
]
*
(
m
[
4
]
*
m
[
5
]
-
m
[
3
]
*
m
[
7
])
+
m
[
3
]
*
m
[
8
]
*
m
[
9
]
+
m
[
6
]
*
(
m
[
6
]
*
m
[
7
]
-
m
[
5
]
*
m
[
8
]
-
m
[
4
]
*
m
[
9
]))
/
det
;
m_inv
[
8
]
=
(
m
[
1
]
*
(
m
[
4
]
*
m
[
6
]
-
m
[
3
]
*
m
[
8
])
+
m
[
3
]
*
m
[
7
]
*
m
[
9
]
+
m
[
5
]
*
(
m
[
5
]
*
m
[
8
]
-
m
[
6
]
*
m
[
7
]
-
m
[
4
]
*
m
[
9
]))
/
det
;
m_inv
[
9
]
=
(
m
[
2
]
*
(
m
[
5
]
*
m
[
6
]
-
m
[
3
]
*
m
[
9
])
+
m
[
3
]
*
m
[
7
]
*
m
[
8
]
+
m
[
4
]
*
(
m
[
4
]
*
m
[
9
]
-
m
[
6
]
*
m
[
7
]
-
m
[
5
]
*
m
[
8
]))
/
det
;
return
1
;
}
inline
void
form4_vec4_dot
(
vector4
&
dest
,
form4
&
m
)
{
vector4
a
;
memcpy
(
a
,
dest
,
sizeof
(
vector4
));
dest
[
0
]
=
m
[
0
]
*
a
[
0
]
+
m
[
9
]
*
a
[
1
]
+
m
[
7
]
*
a
[
2
]
+
m
[
6
]
*
a
[
3
];
dest
[
1
]
=
m
[
9
]
*
a
[
0
]
+
m
[
1
]
*
a
[
1
]
+
m
[
6
]
*
a
[
2
]
+
m
[
5
]
*
a
[
3
];
dest
[
2
]
=
m
[
8
]
*
a
[
0
]
+
m
[
7
]
*
a
[
1
]
+
m
[
2
]
*
a
[
2
]
+
m
[
4
]
*
a
[
3
];
dest
[
3
]
=
m
[
6
]
*
a
[
0
]
+
m
[
5
]
*
a
[
1
]
+
m
[
4
]
*
a
[
2
]
+
m
[
3
]
*
a
[
3
];
}
// square regression: y = eqn[0] + eqn[1]*x + eqn[2]*x*x
inline
int
regress2
(
vector
&
eqn
,
int
order
,
double
*
x
,
double
*
y
,
int
n
)
{
form
xtx
=
FORM_NULL
,
xtx_inv
;
vector
xty
=
VECTOR_NULL
;
double
xn
,
xi
,
yi
;
int
i
;
vec_null
(
eqn
);
xtx
[
0
]
=
n
;
if
((
order
=
order
%
2
)
<
0
)
order
=
-
order
;
// max: quad regress
if
(
order
<
1
)
xtx
[
1
]
=
1.0
;
if
(
order
<
2
)
xtx
[
2
]
=
1.0
;
for
(
i
=
0
;
i
<
n
;
++
i
)
{
xty
[
0
]
+=
(
yi
=
y
[
i
]);
if
(
order
<
1
)
continue
;
xty
[
1
]
+=
yi
*
(
xi
=
xn
=
x
[
i
]);
xtx
[
5
]
+=
xn
;
xtx
[
1
]
+=
(
xn
*=
xi
);
if
(
order
<
2
)
continue
;
xty
[
2
]
+=
yi
*
xn
;
xtx
[
3
]
+=
(
xn
*=
xi
);
xtx
[
2
]
+=
xn
*
xi
;
}
if
(
order
>
1
)
xtx
[
4
]
=
xtx
[
2
];
if
(
!
form_inv
(
xtx_inv
,
xtx
))
return
0
;
memcpy
(
eqn
,
xty
,
sizeof
(
vector
));
form_vec_dot
(
eqn
,
xtx_inv
);
return
1
;
}
// cubic regression: y = eqn[0] + eqn[1]*x + eqn[2]*x*x + eqn[3]*x*x*x
inline
int
regress3
(
vector4
&
eqn
,
int
order
,
double
*
x
,
double
*
y
,
int
n
)
{
form4
xtx
=
FORM4_NULL
,
xtx_inv
;
vector4
xty
=
VECTOR4_NULL
;
double
xn
,
xi
,
yi
;
int
i
;
vec4_null
(
eqn
);
xtx
[
0
]
=
n
;
if
((
order
=
order
%
3
)
<
0
)
order
=
-
order
;
// max: cubic regress
if
(
order
<
1
)
xtx
[
1
]
=
1.0
;
if
(
order
<
2
)
xtx
[
2
]
=
1.0
;
if
(
order
<
3
)
xtx
[
3
]
=
1.0
;
for
(
i
=
0
;
i
<
n
;
++
i
)
{
xty
[
0
]
+=
(
yi
=
y
[
i
]);
if
(
order
<
1
)
continue
;
xty
[
1
]
+=
yi
*
(
xi
=
xn
=
x
[
i
]);
xtx
[
9
]
+=
xn
;
xtx
[
1
]
+=
(
xn
*=
xi
);
if
(
order
<
2
)
continue
;
xty
[
2
]
+=
yi
*
xn
;
xtx
[
7
]
+=
(
xn
*=
xi
);
xtx
[
2
]
+=
xn
*
xi
;
if
(
order
<
3
)
continue
;
xty
[
3
]
+=
yi
*
xn
;
xtx
[
4
]
+=
(
xn
*=
xi
*
xi
);
xtx
[
3
]
+=
xn
*
xi
;
}
if
(
order
>
1
)
xtx
[
8
]
=
xtx
[
1
];
if
(
order
>
2
)
{
xtx
[
6
]
=
xtx
[
7
];
xtx
[
5
]
=
xtx
[
2
];
}
if
(
!
form4_inv
(
xtx_inv
,
xtx
))
return
0
;
memcpy
(
eqn
,
xty
,
sizeof
(
vector4
));
form4_vec4_dot
(
eqn
,
xtx_inv
);
return
1
;
}
}
#endif
Event Timeline
Log In to Comment