forked from lijiext/lammps
git-svn-id: svn://svn.icms.temple.edu/lammps-ro/trunk@5971 f3b2605a-c512-4ea7-a41b-209d697bcdaa
This commit is contained in:
sjplimp
parent
3e3ae00afa
commit
8cd382fef6
|
|
@ -16,8 +16,9 @@
|
|||
------------------------------------------------------------------------- */
|
||||
|
||||
#include "math.h"
|
||||
#include "math_extra.h"
|
||||
#include "string.h"
|
||||
#include "fix_nvt_sllod.h"
|
||||
#include "math_extra.h"
|
||||
#include "atom.h"
|
||||
#include "domain.h"
|
||||
#include "group.h"
|
||||
|
|
|
|||
513
src/math_extra.h
513
src/math_extra.h
|
|
@ -19,16 +19,20 @@
|
|||
#define LMP_MATH_EXTRA_H
|
||||
|
||||
#include "math.h"
|
||||
#include "stdio.h"
|
||||
#include "string.h"
|
||||
#include "error.h"
|
||||
|
||||
// short methods are inlined, others are in math_extra.cpp
|
||||
|
||||
namespace MathExtra {
|
||||
|
||||
// 3 vector operations
|
||||
|
||||
inline void norm3(double *v);
|
||||
inline void normalize3(const double *v, double *ans);
|
||||
inline void snormalize3(const double, const double *v, double *ans);
|
||||
inline void add3(const double *v1, const double *v2, double *ans);
|
||||
inline void sub3(const double *v1, const double *v2, double *ans);
|
||||
inline double len3(const double *v);
|
||||
inline double lensq3(const double *v);
|
||||
inline double dot3(const double *v1, const double *v2);
|
||||
inline void cross3(const double *v1, const double *v2, double *ans);
|
||||
|
||||
|
|
@ -57,36 +61,75 @@ namespace MathExtra {
|
|||
inline void row_times3(const double *v, const double m[3][3],
|
||||
double *ans);
|
||||
inline void scalar_times3(const double f, double m[3][3]);
|
||||
inline void mldivide3(const double mat[3][3], const double *vec,
|
||||
double *ans, LAMMPS_NS::Error *error);
|
||||
inline void write3(const double mat[3][3]);
|
||||
|
||||
|
||||
void write3(const double mat[3][3]);
|
||||
int mldivide3(const double mat[3][3], const double *vec, double *ans);
|
||||
int jacobi(double **matrix, double *evalues, double **evectors);
|
||||
void rotate(double **matrix, int i, int j, int k, int l,
|
||||
double s, double tau);
|
||||
|
||||
// shape matrix operations
|
||||
// upper-triangular 3x3 matrix stored in Voigt notation as 6-vector
|
||||
|
||||
inline void multiply_shape_shape(const double *one, const double *two,
|
||||
double *ans);
|
||||
// quaternion operations
|
||||
|
||||
inline void normalize4(double *quat);
|
||||
inline void quat_to_mat(const double *quat, double mat[3][3]);
|
||||
inline void quat_to_mat_trans(const double *quat, double mat[3][3]);
|
||||
inline void axisangle_to_quat(const double *v, const double angle,
|
||||
double *quat);
|
||||
inline void multiply_quat_quat(const double *one, const double *two,
|
||||
double *ans);
|
||||
inline void multiply_vec_quat(const double *one, const double *two,
|
||||
double *ans);
|
||||
inline void vecquat(double *a, double *b, double *c);
|
||||
inline void quatvec(double *a, double *b, double *c);
|
||||
inline void quatquat(double *a, double *b, double *c);
|
||||
inline void invquatvec(double *a, double *b, double *c);
|
||||
inline void qconjugate(double *q, double *qc);
|
||||
inline void qnormalize(double *q);
|
||||
inline void matvec_rows(double *x, double *y, double *z,
|
||||
double *b, double *c);
|
||||
inline void matvec_cols(double *x, double *y, double *z,
|
||||
double *b, double *c);
|
||||
|
||||
void omega_from_angmom(double *m, double *ex, double *ey, double *ez,
|
||||
double *idiag, double *w);
|
||||
void angmom_from_omega(double *w, double *ex, double *ey, double *ez,
|
||||
double *idiag, double *m);
|
||||
void exyz_to_q(double *ex, double *ey, double *ez, double *q);
|
||||
void q_to_exyz(double *q, double *ex, double *ey, double *ez);
|
||||
void quat_to_mat(const double *quat, double mat[3][3]);
|
||||
void quat_to_mat_trans(const double *quat, double mat[3][3]);
|
||||
|
||||
// rotation operations
|
||||
|
||||
inline void rotation_generator_x(const double m[3][3], double ans[3][3]);
|
||||
inline void rotation_generator_y(const double m[3][3], double ans[3][3]);
|
||||
inline void rotation_generator_z(const double m[3][3], double ans[3][3]);
|
||||
|
||||
// shape matrix operations
|
||||
|
||||
inline void multiply_shape_shape(const double *one, const double *two,
|
||||
double *ans);
|
||||
// moment of inertia operations
|
||||
|
||||
void inertia_ellipsoid(double *shape, double *quat, double mass,
|
||||
double *inertia);
|
||||
void inertia_triangle(double *v0, double *v1, double *v2,
|
||||
double mass, double *inertia);
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
normalize a vector
|
||||
normalize a vector in place
|
||||
------------------------------------------------------------------------- */
|
||||
|
||||
void MathExtra::norm3(double *v)
|
||||
{
|
||||
double scale = 1.0/sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]);
|
||||
v[0] *= scale;
|
||||
v[1] *= scale;
|
||||
v[2] *= scale;
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
normalize a vector, return in ans
|
||||
------------------------------------------------------------------------- */
|
||||
|
||||
void MathExtra::normalize3(const double *v, double *ans)
|
||||
|
|
@ -109,13 +152,53 @@ void MathExtra::snormalize3(const double length, const double *v, double *ans)
|
|||
ans[2] = v[2]*scale;
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
ans = v1 + v2
|
||||
------------------------------------------------------------------------- */
|
||||
|
||||
void MathExtra::add3(const double *v1, const double *v2, double *ans)
|
||||
{
|
||||
ans[0] = v1[0] + v2[0];
|
||||
ans[1] = v1[1] + v2[1];
|
||||
ans[2] = v1[2] + v2[2];
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
ans = v1 - v2
|
||||
------------------------------------------------------------------------- */
|
||||
|
||||
void MathExtra::sub3(const double *v1, const double *v2, double *ans)
|
||||
{
|
||||
ans[0] = v1[0] - v2[0];
|
||||
ans[1] = v1[1] - v2[1];
|
||||
ans[2] = v1[2] - v2[2];
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
length of vector v
|
||||
------------------------------------------------------------------------- */
|
||||
|
||||
double MathExtra::len3(const double *v)
|
||||
{
|
||||
return sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
squared length of vector v, or dot product of v with itself
|
||||
------------------------------------------------------------------------- */
|
||||
|
||||
double MathExtra::lensq3(const double *v)
|
||||
{
|
||||
return v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
dot product of 2 vectors
|
||||
------------------------------------------------------------------------- */
|
||||
|
||||
double MathExtra::dot3(const double *v1, const double *v2)
|
||||
{
|
||||
return v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2];
|
||||
return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
|
|
@ -124,9 +207,9 @@ double MathExtra::dot3(const double *v1, const double *v2)
|
|||
|
||||
void MathExtra::cross3(const double *v1, const double *v2, double *ans)
|
||||
{
|
||||
ans[0] = v1[1]*v2[2]-v1[2]*v2[1];
|
||||
ans[1] = v1[2]*v2[0]-v1[0]*v2[2];
|
||||
ans[2] = v1[0]*v2[1]-v1[1]*v2[0];
|
||||
ans[0] = v1[1]*v2[2] - v1[2]*v2[1];
|
||||
ans[1] = v1[2]*v2[0] - v1[0]*v2[2];
|
||||
ans[2] = v1[0]*v2[1] - v1[1]*v2[0];
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
|
|
@ -135,7 +218,8 @@ void MathExtra::cross3(const double *v1, const double *v2, double *ans)
|
|||
|
||||
double MathExtra::det3(const double m[3][3])
|
||||
{
|
||||
double ans = m[0][0]*m[1][1]*m[2][2] - m[0][0]*m[1][2]*m[2][1] -
|
||||
double ans =
|
||||
m[0][0]*m[1][1]*m[2][2] - m[0][0]*m[1][2]*m[2][1] -
|
||||
m[1][0]*m[0][1]*m[2][2] + m[1][0]*m[0][2]*m[2][1] +
|
||||
m[2][0]*m[0][1]*m[1][2] - m[2][0]*m[0][2]*m[1][1];
|
||||
return ans;
|
||||
|
|
@ -166,15 +250,15 @@ void MathExtra::diag_times3(const double *d, const double m[3][3],
|
|||
void MathExtra::plus3(const double m[3][3], const double m2[3][3],
|
||||
double ans[3][3])
|
||||
{
|
||||
ans[0][0] = m[0][0]+m2[0][0];
|
||||
ans[0][1] = m[0][1]+m2[0][1];
|
||||
ans[0][2] = m[0][2]+m2[0][2];
|
||||
ans[1][0] = m[1][0]+m2[1][0];
|
||||
ans[1][1] = m[1][1]+m2[1][1];
|
||||
ans[1][2] = m[1][2]+m2[1][2];
|
||||
ans[2][0] = m[2][0]+m2[2][0];
|
||||
ans[2][1] = m[2][1]+m2[2][1];
|
||||
ans[2][2] = m[2][2]+m2[2][2];
|
||||
ans[0][0] = m[0][0] + m2[0][0];
|
||||
ans[0][1] = m[0][1] + m2[0][1];
|
||||
ans[0][2] = m[0][2] + m2[0][2];
|
||||
ans[1][0] = m[1][0] + m2[1][0];
|
||||
ans[1][1] = m[1][1] + m2[1][1];
|
||||
ans[1][2] = m[1][2] + m2[1][2];
|
||||
ans[2][0] = m[2][0] + m2[2][0];
|
||||
ans[2][1] = m[2][1] + m2[2][1];
|
||||
ans[2][2] = m[2][2] + m2[2][2];
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
|
|
@ -184,15 +268,15 @@ void MathExtra::plus3(const double m[3][3], const double m2[3][3],
|
|||
void MathExtra::times3(const double m[3][3], const double m2[3][3],
|
||||
double ans[3][3])
|
||||
{
|
||||
ans[0][0] = m[0][0]*m2[0][0]+m[0][1]*m2[1][0]+m[0][2]*m2[2][0];
|
||||
ans[0][1] = m[0][0]*m2[0][1]+m[0][1]*m2[1][1]+m[0][2]*m2[2][1];
|
||||
ans[0][2] = m[0][0]*m2[0][2]+m[0][1]*m2[1][2]+m[0][2]*m2[2][2];
|
||||
ans[1][0] = m[1][0]*m2[0][0]+m[1][1]*m2[1][0]+m[1][2]*m2[2][0];
|
||||
ans[1][1] = m[1][0]*m2[0][1]+m[1][1]*m2[1][1]+m[1][2]*m2[2][1];
|
||||
ans[1][2] = m[1][0]*m2[0][2]+m[1][1]*m2[1][2]+m[1][2]*m2[2][2];
|
||||
ans[2][0] = m[2][0]*m2[0][0]+m[2][1]*m2[1][0]+m[2][2]*m2[2][0];
|
||||
ans[2][1] = m[2][0]*m2[0][1]+m[2][1]*m2[1][1]+m[2][2]*m2[2][1];
|
||||
ans[2][2] = m[2][0]*m2[0][2]+m[2][1]*m2[1][2]+m[2][2]*m2[2][2];
|
||||
ans[0][0] = m[0][0]*m2[0][0] + m[0][1]*m2[1][0] + m[0][2]*m2[2][0];
|
||||
ans[0][1] = m[0][0]*m2[0][1] + m[0][1]*m2[1][1] + m[0][2]*m2[2][1];
|
||||
ans[0][2] = m[0][0]*m2[0][2] + m[0][1]*m2[1][2] + m[0][2]*m2[2][2];
|
||||
ans[1][0] = m[1][0]*m2[0][0] + m[1][1]*m2[1][0] + m[1][2]*m2[2][0];
|
||||
ans[1][1] = m[1][0]*m2[0][1] + m[1][1]*m2[1][1] + m[1][2]*m2[2][1];
|
||||
ans[1][2] = m[1][0]*m2[0][2] + m[1][1]*m2[1][2] + m[1][2]*m2[2][2];
|
||||
ans[2][0] = m[2][0]*m2[0][0] + m[2][1]*m2[1][0] + m[2][2]*m2[2][0];
|
||||
ans[2][1] = m[2][0]*m2[0][1] + m[2][1]*m2[1][1] + m[2][2]*m2[2][1];
|
||||
ans[2][2] = m[2][0]*m2[0][2] + m[2][1]*m2[1][2] + m[2][2]*m2[2][2];
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
|
|
@ -202,15 +286,15 @@ void MathExtra::times3(const double m[3][3], const double m2[3][3],
|
|||
void MathExtra::transpose_times3(const double m[3][3], const double m2[3][3],
|
||||
double ans[3][3])
|
||||
{
|
||||
ans[0][0] = m[0][0]*m2[0][0]+m[1][0]*m2[1][0]+m[2][0]*m2[2][0];
|
||||
ans[0][1] = m[0][0]*m2[0][1]+m[1][0]*m2[1][1]+m[2][0]*m2[2][1];
|
||||
ans[0][2] = m[0][0]*m2[0][2]+m[1][0]*m2[1][2]+m[2][0]*m2[2][2];
|
||||
ans[1][0] = m[0][1]*m2[0][0]+m[1][1]*m2[1][0]+m[2][1]*m2[2][0];
|
||||
ans[1][1] = m[0][1]*m2[0][1]+m[1][1]*m2[1][1]+m[2][1]*m2[2][1];
|
||||
ans[1][2] = m[0][1]*m2[0][2]+m[1][1]*m2[1][2]+m[2][1]*m2[2][2];
|
||||
ans[2][0] = m[0][2]*m2[0][0]+m[1][2]*m2[1][0]+m[2][2]*m2[2][0];
|
||||
ans[2][1] = m[0][2]*m2[0][1]+m[1][2]*m2[1][1]+m[2][2]*m2[2][1];
|
||||
ans[2][2] = m[0][2]*m2[0][2]+m[1][2]*m2[1][2]+m[2][2]*m2[2][2];
|
||||
ans[0][0] = m[0][0]*m2[0][0] + m[1][0]*m2[1][0] + m[2][0]*m2[2][0];
|
||||
ans[0][1] = m[0][0]*m2[0][1] + m[1][0]*m2[1][1] + m[2][0]*m2[2][1];
|
||||
ans[0][2] = m[0][0]*m2[0][2] + m[1][0]*m2[1][2] + m[2][0]*m2[2][2];
|
||||
ans[1][0] = m[0][1]*m2[0][0] + m[1][1]*m2[1][0] + m[2][1]*m2[2][0];
|
||||
ans[1][1] = m[0][1]*m2[0][1] + m[1][1]*m2[1][1] + m[2][1]*m2[2][1];
|
||||
ans[1][2] = m[0][1]*m2[0][2] + m[1][1]*m2[1][2] + m[2][1]*m2[2][2];
|
||||
ans[2][0] = m[0][2]*m2[0][0] + m[1][2]*m2[1][0] + m[2][2]*m2[2][0];
|
||||
ans[2][1] = m[0][2]*m2[0][1] + m[1][2]*m2[1][1] + m[2][2]*m2[2][1];
|
||||
ans[2][2] = m[0][2]*m2[0][2] + m[1][2]*m2[1][2] + m[2][2]*m2[2][2];
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
|
|
@ -220,20 +304,20 @@ void MathExtra::transpose_times3(const double m[3][3], const double m2[3][3],
|
|||
void MathExtra::times3_transpose(const double m[3][3], const double m2[3][3],
|
||||
double ans[3][3])
|
||||
{
|
||||
ans[0][0] = m[0][0]*m2[0][0]+m[0][1]*m2[0][1]+m[0][2]*m2[0][2];
|
||||
ans[0][1] = m[0][0]*m2[1][0]+m[0][1]*m2[1][1]+m[0][2]*m2[1][2];
|
||||
ans[0][2] = m[0][0]*m2[2][0]+m[0][1]*m2[2][1]+m[0][2]*m2[2][2];
|
||||
ans[1][0] = m[1][0]*m2[0][0]+m[1][1]*m2[0][1]+m[1][2]*m2[0][2];
|
||||
ans[1][1] = m[1][0]*m2[1][0]+m[1][1]*m2[1][1]+m[1][2]*m2[1][2];
|
||||
ans[1][2] = m[1][0]*m2[2][0]+m[1][1]*m2[2][1]+m[1][2]*m2[2][2];
|
||||
ans[2][0] = m[2][0]*m2[0][0]+m[2][1]*m2[0][1]+m[2][2]*m2[0][2];
|
||||
ans[2][1] = m[2][0]*m2[1][0]+m[2][1]*m2[1][1]+m[2][2]*m2[1][2];
|
||||
ans[2][2] = m[2][0]*m2[2][0]+m[2][1]*m2[2][1]+m[2][2]*m2[2][2];
|
||||
ans[0][0] = m[0][0]*m2[0][0] + m[0][1]*m2[0][1] + m[0][2]*m2[0][2];
|
||||
ans[0][1] = m[0][0]*m2[1][0] + m[0][1]*m2[1][1] + m[0][2]*m2[1][2];
|
||||
ans[0][2] = m[0][0]*m2[2][0] + m[0][1]*m2[2][1] + m[0][2]*m2[2][2];
|
||||
ans[1][0] = m[1][0]*m2[0][0] + m[1][1]*m2[0][1] + m[1][2]*m2[0][2];
|
||||
ans[1][1] = m[1][0]*m2[1][0] + m[1][1]*m2[1][1] + m[1][2]*m2[1][2];
|
||||
ans[1][2] = m[1][0]*m2[2][0] + m[1][1]*m2[2][1] + m[1][2]*m2[2][2];
|
||||
ans[2][0] = m[2][0]*m2[0][0] + m[2][1]*m2[0][1] + m[2][2]*m2[0][2];
|
||||
ans[2][1] = m[2][0]*m2[1][0] + m[2][1]*m2[1][1] + m[2][2]*m2[1][2];
|
||||
ans[2][2] = m[2][0]*m2[2][0] + m[2][1]*m2[2][1] + m[2][2]*m2[2][2];
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
invert a matrix
|
||||
does NOT checks for singular or badly scaled matrix
|
||||
does NOT check for singular or badly scaled matrix
|
||||
------------------------------------------------------------------------- */
|
||||
|
||||
void MathExtra::invert3(const double m[3][3], double ans[3][3])
|
||||
|
|
@ -260,9 +344,9 @@ void MathExtra::invert3(const double m[3][3], double ans[3][3])
|
|||
void MathExtra::times_column3(const double m[3][3], const double *v,
|
||||
double *ans)
|
||||
{
|
||||
ans[0] = m[0][0]*v[0]+m[0][1]*v[1]+m[0][2]*v[2];
|
||||
ans[1] = m[1][0]*v[0]+m[1][1]*v[1]+m[1][2]*v[2];
|
||||
ans[2] = m[2][0]*v[0]+m[2][1]*v[1]+m[2][2]*v[2];
|
||||
ans[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
|
||||
ans[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
|
||||
ans[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
|
|
@ -272,9 +356,9 @@ void MathExtra::times_column3(const double m[3][3], const double *v,
|
|||
void MathExtra::transpose_times_column3(const double m[3][3], const double *v,
|
||||
double *ans)
|
||||
{
|
||||
ans[0] = m[0][0]*v[0]+m[1][0]*v[1]+m[2][0]*v[2];
|
||||
ans[1] = m[0][1]*v[0]+m[1][1]*v[1]+m[2][1]*v[2];
|
||||
ans[2] = m[0][2]*v[0]+m[1][2]*v[1]+m[2][2]*v[2];
|
||||
ans[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
|
||||
ans[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
|
||||
ans[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
|
|
@ -302,9 +386,9 @@ void MathExtra::transpose_times_diag3(const double m[3][3],
|
|||
void MathExtra::row_times3(const double *v, const double m[3][3],
|
||||
double *ans)
|
||||
{
|
||||
ans[0] = m[0][0]*v[0]+v[1]*m[1][0]+v[2]*m[2][0];
|
||||
ans[1] = v[0]*m[0][1]+m[1][1]*v[1]+v[2]*m[2][1];
|
||||
ans[2] = v[0]*m[0][2]+v[1]*m[1][2]+m[2][2]*v[2];
|
||||
ans[0] = m[0][0]*v[0] + v[1]*m[1][0] + v[2]*m[2][0];
|
||||
ans[1] = v[0]*m[0][1] + m[1][1]*v[1] + v[2]*m[2][1];
|
||||
ans[2] = v[0]*m[0][2] + v[1]*m[1][2] + m[2][2]*v[2];
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
|
|
@ -325,72 +409,19 @@ inline void MathExtra::scalar_times3(const double f, double m[3][3])
|
|||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
solve Ax = b or M ans = v
|
||||
use gaussian elimination & partial pivoting on matrix
|
||||
multiply 2 shape matrices to yield a 3rd shape matrix
|
||||
upper-triangular 3x3 matrices stored in Voigt notation as 6-vectors
|
||||
------------------------------------------------------------------------- */
|
||||
|
||||
void MathExtra::mldivide3(const double m[3][3], const double *v, double *ans,
|
||||
LAMMPS_NS::Error *error)
|
||||
void MathExtra::multiply_shape_shape(const double *one, const double *two,
|
||||
double *ans)
|
||||
{
|
||||
// create augmented matrix for pivoting
|
||||
|
||||
double aug[3][4];
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
aug[i][3] = v[i];
|
||||
for (unsigned j = 0; j < 3; j++) aug[i][j] = m[i][j];
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < 2; i++) {
|
||||
unsigned p = i;
|
||||
for (unsigned j = i+1; j < 3; j++) {
|
||||
if (fabs(aug[j][i]) > fabs(aug[i][i])) {
|
||||
double tempv[4];
|
||||
memcpy(tempv,aug[i],4*sizeof(double));
|
||||
memcpy(aug[i],aug[j],4*sizeof(double));
|
||||
memcpy(aug[j],tempv,4*sizeof(double));
|
||||
}
|
||||
}
|
||||
|
||||
while (aug[p][i] == 0.0 && p < 3) p++;
|
||||
|
||||
if (p == 3) error->all("Bad matrix inversion in MathExtra::mldivide3");
|
||||
else
|
||||
if (p != i) {
|
||||
double tempv[4];
|
||||
memcpy(tempv,aug[i],4*sizeof(double));
|
||||
memcpy(aug[i],aug[p],4*sizeof(double));
|
||||
memcpy(aug[p],tempv,4*sizeof(double));
|
||||
}
|
||||
|
||||
for (unsigned j = i+1; j < 3; j++) {
|
||||
double m = aug[j][i]/aug[i][i];
|
||||
for (unsigned k=i+1; k<4; k++) aug[j][k]-=m*aug[i][k];
|
||||
}
|
||||
}
|
||||
|
||||
if (aug[2][2] == 0.0)
|
||||
error->all("Bad matrix inversion in MathExtra::mldivide3");
|
||||
|
||||
// back substitution
|
||||
|
||||
ans[2] = aug[2][3]/aug[2][2];
|
||||
for (int i = 1; i >= 0; i--) {
|
||||
double sumax = 0.0;
|
||||
for (unsigned j = i+1; j < 3; j++) sumax += aug[i][j]*ans[j];
|
||||
ans[i] = (aug[i][3]-sumax) / aug[i][i];
|
||||
}
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
output a matrix
|
||||
------------------------------------------------------------------------- */
|
||||
|
||||
void MathExtra::write3(const double mat[3][3])
|
||||
{
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
for (unsigned j = 0; j < 3; j++) printf("%g ",mat[i][j]);
|
||||
printf("\n");
|
||||
}
|
||||
ans[0] = one[0]*two[0];
|
||||
ans[1] = one[1]*two[1];
|
||||
ans[2] = one[2]*two[2];
|
||||
ans[3] = one[1]*two[3] + one[3]*two[2];
|
||||
ans[4] = one[0]*two[4] + one[5]*two[3] + one[4]*two[2];
|
||||
ans[5] = one[0]*two[5] + one[5]*two[1];
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
|
|
@ -407,68 +438,6 @@ void MathExtra::normalize4(double *quat)
|
|||
quat[3] *= scale;
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
compute rotation matrix from quaternion
|
||||
quat = [w i j k]
|
||||
------------------------------------------------------------------------- */
|
||||
|
||||
void MathExtra::quat_to_mat(const double *quat, double mat[3][3])
|
||||
{
|
||||
double w2 = quat[0]*quat[0];
|
||||
double i2 = quat[1]*quat[1];
|
||||
double j2 = quat[2]*quat[2];
|
||||
double k2 = quat[3]*quat[3];
|
||||
double twoij = 2.0*quat[1]*quat[2];
|
||||
double twoik = 2.0*quat[1]*quat[3];
|
||||
double twojk = 2.0*quat[2]*quat[3];
|
||||
double twoiw = 2.0*quat[1]*quat[0];
|
||||
double twojw = 2.0*quat[2]*quat[0];
|
||||
double twokw = 2.0*quat[3]*quat[0];
|
||||
|
||||
mat[0][0] = w2+i2-j2-k2;
|
||||
mat[0][1] = twoij-twokw;
|
||||
mat[0][2] = twojw+twoik;
|
||||
|
||||
mat[1][0] = twoij+twokw;
|
||||
mat[1][1] = w2-i2+j2-k2;
|
||||
mat[1][2] = twojk-twoiw;
|
||||
|
||||
mat[2][0] = twoik-twojw;
|
||||
mat[2][1] = twojk+twoiw;
|
||||
mat[2][2] = w2-i2-j2+k2;
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
compute rotation matrix from quaternion conjugate
|
||||
quat = [w i j k]
|
||||
------------------------------------------------------------------------- */
|
||||
|
||||
void MathExtra::quat_to_mat_trans(const double *quat, double mat[3][3])
|
||||
{
|
||||
double w2 = quat[0]*quat[0];
|
||||
double i2 = quat[1]*quat[1];
|
||||
double j2 = quat[2]*quat[2];
|
||||
double k2 = quat[3]*quat[3];
|
||||
double twoij = 2.0*quat[1]*quat[2];
|
||||
double twoik = 2.0*quat[1]*quat[3];
|
||||
double twojk = 2.0*quat[2]*quat[3];
|
||||
double twoiw = 2.0*quat[1]*quat[0];
|
||||
double twojw = 2.0*quat[2]*quat[0];
|
||||
double twokw = 2.0*quat[3]*quat[0];
|
||||
|
||||
mat[0][0] = w2+i2-j2-k2;
|
||||
mat[1][0] = twoij-twokw;
|
||||
mat[2][0] = twojw+twoik;
|
||||
|
||||
mat[0][1] = twoij+twokw;
|
||||
mat[1][1] = w2-i2+j2-k2;
|
||||
mat[2][1] = twojk-twoiw;
|
||||
|
||||
mat[0][2] = twoik-twojw;
|
||||
mat[1][2] = twojk+twoiw;
|
||||
mat[2][2] = w2-i2-j2+k2;
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
compute quaternion from axis-angle rotation
|
||||
v MUST be a unit vector
|
||||
|
|
@ -515,70 +484,154 @@ void MathExtra::multiply_vec_quat(const double *one, const double *two,
|
|||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
multiply 2 shape matrices
|
||||
upper-triangular 3x3, stored as 6-vector in Voigt notation
|
||||
vector-quaternion multiply: c = a*b, where a = (0,a)
|
||||
------------------------------------------------------------------------- */
|
||||
|
||||
void MathExtra::multiply_shape_shape(const double *one, const double *two,
|
||||
double *ans)
|
||||
void MathExtra::vecquat(double *a, double *b, double *c)
|
||||
{
|
||||
ans[0] = one[0]*two[0];
|
||||
ans[1] = one[1]*two[1];
|
||||
ans[2] = one[2]*two[2];
|
||||
ans[3] = one[1]*two[3] + one[3]*two[2];
|
||||
ans[4] = one[0]*two[4] + one[5]*two[3] + one[4]*two[2];
|
||||
ans[5] = one[0]*two[5] + one[5]*two[1];
|
||||
c[0] = -a[0]*b[1] - a[1]*b[2] - a[2]*b[3];
|
||||
c[1] = b[0]*a[0] + a[1]*b[3] - a[2]*b[2];
|
||||
c[2] = b[0]*a[1] + a[2]*b[1] - a[0]*b[3];
|
||||
c[3] = b[0]*a[2] + a[0]*b[2] - a[1]*b[1];
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
Apply principal rotation generator about x to rotation matrix m
|
||||
quaternion-vector multiply: c = a*b, where b = (0,b)
|
||||
------------------------------------------------------------------------- */
|
||||
|
||||
void MathExtra::quatvec(double *a, double *b, double *c)
|
||||
{
|
||||
c[0] = -a[1]*b[0] - a[2]*b[1] - a[3]*b[2];
|
||||
c[1] = a[0]*b[0] + a[2]*b[2] - a[3]*b[1];
|
||||
c[2] = a[0]*b[1] + a[3]*b[0] - a[1]*b[2];
|
||||
c[3] = a[0]*b[2] + a[1]*b[1] - a[2]*b[0];
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
quaternion-quaternion multiply: c = a*b
|
||||
------------------------------------------------------------------------- */
|
||||
|
||||
void MathExtra::quatquat(double *a, double *b, double *c)
|
||||
{
|
||||
c[0] = a[0]*b[0] - a[1]*b[1] - a[2]*b[2] - a[3]*b[3];
|
||||
c[1] = a[0]*b[1] + b[0]*a[1] + a[2]*b[3] - a[3]*b[2];
|
||||
c[2] = a[0]*b[2] + b[0]*a[2] + a[3]*b[1] - a[1]*b[3];
|
||||
c[3] = a[0]*b[3] + b[0]*a[3] + a[1]*b[2] - a[2]*b[1];
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
quaternion multiply: c = inv(a)*b
|
||||
a is a quaternion
|
||||
b is a four component vector
|
||||
c is a three component vector
|
||||
------------------------------------------------------------------------- */
|
||||
|
||||
void MathExtra::invquatvec(double *a, double *b, double *c)
|
||||
{
|
||||
c[0] = -a[1]*b[0] + a[0]*b[1] + a[3]*b[2] - a[2]*b[3];
|
||||
c[1] = -a[2]*b[0] - a[3]*b[1] + a[0]*b[2] + a[1]*b[3];
|
||||
c[2] = -a[3]*b[0] + a[2]*b[1] - a[1]*b[2] + a[0]*b[3];
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
conjugate of a quaternion: qc = conjugate of q
|
||||
assume q is of unit length
|
||||
------------------------------------------------------------------------- */
|
||||
|
||||
void MathExtra::qconjugate(double *q, double *qc)
|
||||
{
|
||||
qc[0] = q[0];
|
||||
qc[1] = -q[1];
|
||||
qc[2] = -q[2];
|
||||
qc[3] = -q[3];
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
normalize a quaternion
|
||||
------------------------------------------------------------------------- */
|
||||
|
||||
void MathExtra::qnormalize(double *q)
|
||||
{
|
||||
double norm = 1.0 / sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
|
||||
q[0] *= norm;
|
||||
q[1] *= norm;
|
||||
q[2] *= norm;
|
||||
q[3] *= norm;
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
matvec_rows: c = Ab, where rows of A are x, y, z
|
||||
------------------------------------------------------------------------- */
|
||||
|
||||
void MathExtra::matvec_rows(double *x, double *y, double *z,
|
||||
double *b, double *c)
|
||||
{
|
||||
c[0] = x[0]*b[0] + x[1]*b[1] + x[2]*b[2];
|
||||
c[1] = y[0]*b[0] + y[1]*b[1] + y[2]*b[2];
|
||||
c[2] = z[0]*b[0] + z[1]*b[1] + z[2]*b[2];
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
matvec_cols: c = Ab, where columns of A are x, y, z
|
||||
------------------------------------------------------------------------- */
|
||||
|
||||
void MathExtra::matvec_cols(double *x, double *y, double *z,
|
||||
double *b, double *c)
|
||||
{
|
||||
c[0] = x[0]*b[0] + y[0]*b[1] + z[0]*b[2];
|
||||
c[1] = x[1]*b[0] + y[1]*b[1] + z[1]*b[2];
|
||||
c[2] = x[2]*b[0] + y[2]*b[1] + z[2]*b[2];
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
apply principal rotation generator about x to rotation matrix m
|
||||
------------------------------------------------------------------------- */
|
||||
|
||||
void MathExtra::rotation_generator_x(const double m[3][3], double ans[3][3])
|
||||
{
|
||||
ans[0][0]=0;
|
||||
ans[0][1]=-m[0][2];
|
||||
ans[0][2]=m[0][1];
|
||||
ans[1][0]=0;
|
||||
ans[1][1]=-m[1][2];
|
||||
ans[1][2]=m[1][1];
|
||||
ans[2][0]=0;
|
||||
ans[2][1]=-m[2][2];
|
||||
ans[2][2]=m[2][1];
|
||||
ans[0][0] = 0.0;
|
||||
ans[0][1] = -m[0][2];
|
||||
ans[0][2] = m[0][1];
|
||||
ans[1][0] = 0.0;
|
||||
ans[1][1] = -m[1][2];
|
||||
ans[1][2] = m[1][1];
|
||||
ans[2][0] = 0.0;
|
||||
ans[2][1] = -m[2][2];
|
||||
ans[2][2] = m[2][1];
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
Apply principal rotation generator about y to rotation matrix m
|
||||
apply principal rotation generator about y to rotation matrix m
|
||||
------------------------------------------------------------------------- */
|
||||
|
||||
void MathExtra::rotation_generator_y(const double m[3][3], double ans[3][3])
|
||||
{
|
||||
ans[0][0]=m[0][2];
|
||||
ans[0][1]=0;
|
||||
ans[0][2]=-m[0][0];
|
||||
ans[1][0]=m[1][2];
|
||||
ans[1][1]=0;
|
||||
ans[1][2]=-m[1][0];
|
||||
ans[2][0]=m[2][2];
|
||||
ans[2][1]=0;
|
||||
ans[2][2]=-m[2][0];
|
||||
ans[0][0] = m[0][2];
|
||||
ans[0][1] = 0.0;
|
||||
ans[0][2] = -m[0][0];
|
||||
ans[1][0] = m[1][2];
|
||||
ans[1][1] = 0.0;
|
||||
ans[1][2] = -m[1][0];
|
||||
ans[2][0] = m[2][2];
|
||||
ans[2][1] = 0.0;
|
||||
ans[2][2] = -m[2][0];
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
Apply principal rotation generator about z to rotation matrix m
|
||||
apply principal rotation generator about z to rotation matrix m
|
||||
------------------------------------------------------------------------- */
|
||||
|
||||
void MathExtra::rotation_generator_z(const double m[3][3], double ans[3][3])
|
||||
{
|
||||
ans[0][0]=-m[0][1];
|
||||
ans[0][1]=m[0][0];
|
||||
ans[0][2]=0;
|
||||
ans[1][0]=-m[1][1];
|
||||
ans[1][1]=m[1][0];
|
||||
ans[1][2]=0;
|
||||
ans[2][0]=-m[2][1];
|
||||
ans[2][1]=m[2][0];
|
||||
ans[2][2]=0;
|
||||
ans[0][0] = -m[0][1];
|
||||
ans[0][1] = m[0][0];
|
||||
ans[0][2] = 0.0;
|
||||
ans[1][0] = -m[1][1];
|
||||
ans[1][1] = m[1][0];
|
||||
ans[1][2] = 0.0;
|
||||
ans[2][0] = -m[2][1];
|
||||
ans[2][1] = m[2][0];
|
||||
ans[2][2] = 0.0;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
Loading…
Reference in New Issue