forked from lijiext/lammps
git-svn-id: svn://svn.icms.temple.edu/lammps-ro/trunk@10051 f3b2605a-c512-4ea7-a41b-209d697bcdaa
This commit is contained in:
sjplimp
parent
5f25e2a828
commit
eec6088ce7
|
|
@ -1,72 +0,0 @@
|
|||
/* ----------------------------------------------------------------------
|
||||
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_COMPLEX_H
|
||||
#define LMP_MATH_COMPLEX_H
|
||||
|
||||
#define COMPLEX_NULL {0, 0}
|
||||
|
||||
namespace LAMMPS_NS {
|
||||
|
||||
typedef struct complex {
|
||||
double re, im; } complex;
|
||||
|
||||
}
|
||||
|
||||
#define C_MULT(d, x, y) { \
|
||||
d.re = x.re*y.re-x.im*y.im; \
|
||||
d.im = x.re*y.im+x.im*y.re; }
|
||||
|
||||
#define C_RMULT(d, x, y) { \
|
||||
register complex t = x; \
|
||||
d.re = t.re*y.re-t.im*y.im; \
|
||||
d.im = t.re*y.im+t.im*y.re; }
|
||||
|
||||
#define C_CRMULT(d, x, y) { \
|
||||
register complex t = x; \
|
||||
d.re = t.re*y.re-t.im*y.im; \
|
||||
d.im = -t.re*y.im-t.im*y.re; }
|
||||
|
||||
#define C_SMULT(d, x, y) { \
|
||||
d.re = x.re*y; \
|
||||
d.im = x.im*y; }
|
||||
|
||||
#define C_ADD(d, x, y) { \
|
||||
d.re = x.re+y.re; \
|
||||
d.im = x.im+y.im; }
|
||||
|
||||
#define C_SUBTR(d, x, y) { \
|
||||
d.re = x.re-y.re; \
|
||||
d.im = x.im-y.im; }
|
||||
|
||||
#define C_CONJ(d, x) { \
|
||||
d.re = x.re; \
|
||||
d.im = -x.im; }
|
||||
|
||||
#define C_SET(d, x, y) { \
|
||||
d.re = x; \
|
||||
d.im = y; }
|
||||
|
||||
#define C_ANGLE(d, angle) { \
|
||||
register double a = angle; \
|
||||
d.re = cos(a); \
|
||||
d.im = sin(a); }
|
||||
|
||||
#define C_COPY(d, x) { \
|
||||
memcpy(&d, &x, sizeof(complex)); }
|
||||
|
||||
#endif
|
||||
|
|
@ -1,448 +0,0 @@
|
|||
/* ----------------------------------------------------------------------
|
||||
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
|
||||
Loading…
Reference in New Issue