480 lines
27 KiB
C++
480 lines
27 KiB
C++
// clang-format off
|
|
/* -*- c++ -*- ----------------------------------------------------------
|
|
LAMMPS - Large-scale Atomic/Molecular Massively Parallel Simulator
|
|
https://www.lammps.org/, Sandia National Laboratories
|
|
LAMMPS development team: developers@lammps.org
|
|
|
|
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: W. Michael Brown (Intel)
|
|
------------------------------------------------------------------------- */
|
|
|
|
#ifndef LMP_MATH_EXTRA_INTEL_H
|
|
#define LMP_MATH_EXTRA_INTEL_H
|
|
|
|
#define ME_quat_to_mat_trans(quat, mat) \
|
|
{ \
|
|
flt_t quat_w = quat.w; \
|
|
flt_t quat_i = quat.i; \
|
|
flt_t quat_j = quat.j; \
|
|
flt_t quat_k = quat.k; \
|
|
flt_t w2 = quat_w * quat_w; \
|
|
flt_t i2 = quat_i * quat_i; \
|
|
flt_t j2 = quat_j * quat_j; \
|
|
flt_t k2 = quat_k * quat_k; \
|
|
flt_t twoij = (flt_t)2.0 * quat_i * quat_j; \
|
|
flt_t twoik = (flt_t)2.0 * quat_i * quat_k; \
|
|
flt_t twojk = (flt_t)2.0 * quat_j * quat_k; \
|
|
flt_t twoiw = (flt_t)2.0 * quat_i * quat_w; \
|
|
flt_t twojw = (flt_t)2.0 * quat_j * quat_w; \
|
|
flt_t twokw = (flt_t)2.0 * quat_k * quat_w; \
|
|
\
|
|
mat##_0 = w2 + i2 - j2 - k2; \
|
|
mat##_3 = twoij - twokw; \
|
|
mat##_6 = twojw + twoik; \
|
|
\
|
|
mat##_1 = twoij + twokw; \
|
|
mat##_4 = w2 - i2 + j2 - k2; \
|
|
mat##_7 = twojk - twoiw; \
|
|
\
|
|
mat##_2 = twoik - twojw; \
|
|
mat##_5 = twojk + twoiw; \
|
|
mat##_8 = w2 - i2 - j2 + k2; \
|
|
}
|
|
|
|
/* ----------------------------------------------------------------------
|
|
diagonal matrix times a full matrix
|
|
------------------------------------------------------------------------- */
|
|
|
|
#define ME_diag_times3(d, m, ans) \
|
|
{ \
|
|
ans##_0 = d[0] * m##_0; \
|
|
ans##_1 = d[0] * m##_1; \
|
|
ans##_2 = d[0] * m##_2; \
|
|
ans##_3 = d[1] * m##_3; \
|
|
ans##_4 = d[1] * m##_4; \
|
|
ans##_5 = d[1] * m##_5; \
|
|
ans##_6 = d[2] * m##_6; \
|
|
ans##_7 = d[2] * m##_7; \
|
|
ans##_8 = d[2] * m##_8; \
|
|
}
|
|
|
|
#define ME_diag_times3a(d, m, ans) \
|
|
{ \
|
|
ans##_0 = d##_0 * m##_0; \
|
|
ans##_1 = d##_0 * m##_1; \
|
|
ans##_2 = d##_0 * m##_2; \
|
|
ans##_3 = d##_1 * m##_3; \
|
|
ans##_4 = d##_1 * m##_4; \
|
|
ans##_5 = d##_1 * m##_5; \
|
|
ans##_6 = d##_2 * m##_6; \
|
|
ans##_7 = d##_2 * m##_7; \
|
|
ans##_8 = d##_2 * m##_8; \
|
|
}
|
|
|
|
/* ----------------------------------------------------------------------
|
|
multiply the transpose of mat1 times mat2
|
|
------------------------------------------------------------------------- */
|
|
|
|
#define ME_transpose_times3(m1, m2, ans) \
|
|
{ \
|
|
ans##_0 = m1##_0*m2##_0 + m1##_3*m2##_3 + m1##_6*m2##_6; \
|
|
ans##_1 = m1##_0*m2##_1 + m1##_3*m2##_4 + m1##_6*m2##_7; \
|
|
ans##_2 = m1##_0*m2##_2 + m1##_3*m2##_5 + m1##_6*m2##_8; \
|
|
ans##_3 = m1##_1*m2##_0 + m1##_4*m2##_3 + m1##_7*m2##_6; \
|
|
ans##_4 = m1##_1*m2##_1 + m1##_4*m2##_4 + m1##_7*m2##_7; \
|
|
ans##_5 = m1##_1*m2##_2 + m1##_4*m2##_5 + m1##_7*m2##_8; \
|
|
ans##_6 = m1##_2*m2##_0 + m1##_5*m2##_3 + m1##_8*m2##_6; \
|
|
ans##_7 = m1##_2*m2##_1 + m1##_5*m2##_4 + m1##_8*m2##_7; \
|
|
ans##_8 = m1##_2*m2##_2 + m1##_5*m2##_5 + m1##_8*m2##_8; \
|
|
}
|
|
|
|
/* ----------------------------------------------------------------------
|
|
normalize a vector, return in ans
|
|
------------------------------------------------------------------------- */
|
|
|
|
#define ME_normalize3(v0, v1, v2, ans) \
|
|
{ \
|
|
flt_t scale = (flt_t)1.0 / std::sqrt(v0*v0+v1*v1+v2*v2); \
|
|
ans##_0 = v0 * scale; \
|
|
ans##_1 = v1 * scale; \
|
|
ans##_2 = v2 * scale; \
|
|
}
|
|
|
|
/* ----------------------------------------------------------------------
|
|
add two matrices
|
|
------------------------------------------------------------------------- */
|
|
|
|
#define ME_plus3(m1, m2, ans) \
|
|
{ \
|
|
ans##_0 = m1##_0 + m2##_0; \
|
|
ans##_1 = m1##_1 + m2##_1; \
|
|
ans##_2 = m1##_2 + m2##_2; \
|
|
ans##_3 = m1##_3 + m2##_3; \
|
|
ans##_4 = m1##_4 + m2##_4; \
|
|
ans##_5 = m1##_5 + m2##_5; \
|
|
ans##_6 = m1##_6 + m2##_6; \
|
|
ans##_7 = m1##_7 + m2##_7; \
|
|
ans##_8 = m1##_8 + m2##_8; \
|
|
}
|
|
|
|
/* ----------------------------------------------------------------------
|
|
dot product of 2 vectors
|
|
------------------------------------------------------------------------- */
|
|
|
|
#define ME_dot3(v1, v2) \
|
|
(v1##_0*v2##_0 + v1##_1 * v2##_1 + v1##_2 * v2##_2)
|
|
|
|
/* ----------------------------------------------------------------------
|
|
determinant of a matrix
|
|
------------------------------------------------------------------------- */
|
|
|
|
#define ME_det3(m) \
|
|
( m##_0 * m##_4 * m##_8 - m##_0 * m##_5 * m##_7 - \
|
|
m##_3 * m##_1 * m##_8 + m##_3 * m##_2 * m##_7 + \
|
|
m##_6 * m##_1 * m##_5 - m##_6 * m##_2 * m##_4 )
|
|
|
|
/* ----------------------------------------------------------------------
|
|
row vector times matrix
|
|
------------------------------------------------------------------------- */
|
|
|
|
#define ME_vecmat(v, m, ans) \
|
|
{ \
|
|
ans##_0 = v##_0 * m##_0 + v##_1 * m##_3 + v##_2 * m##_6; \
|
|
ans##_1 = v##_0 * m##_1 + v##_1 * m##_4 + v##_2 * m##_7; \
|
|
ans##_2 = v##_0 * m##_2 + v##_1 * m##_5 + v##_2 * m##_8; \
|
|
}
|
|
|
|
/* ----------------------------------------------------------------------
|
|
cross product of 2 vectors
|
|
------------------------------------------------------------------------- */
|
|
|
|
#define ME_cross3(v1, v2, 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; \
|
|
}
|
|
|
|
/* ----------------------------------------------------------------------
|
|
cross product of 2 vectors
|
|
------------------------------------------------------------------------- */
|
|
|
|
#define ME_mv0_cross3(m1, v2, ans) \
|
|
{ \
|
|
ans##_0 = m1##_1 * v2##_2 - m1##_2 * v2##_1; \
|
|
ans##_1 = m1##_2 * v2##_0 - m1##_0 * v2##_2; \
|
|
ans##_2 = m1##_0 * v2##_1 - m1##_1 * v2##_0; \
|
|
}
|
|
|
|
#define ME_mv1_cross3(m1, v2, ans) \
|
|
{ \
|
|
ans##_0 = m1##_4 * v2##_2 - m1##_5 * v2##_1; \
|
|
ans##_1 = m1##_5 * v2##_0 - m1##_3 * v2##_2; \
|
|
ans##_2 = m1##_3 * v2##_1 - m1##_4 * v2##_0; \
|
|
}
|
|
|
|
#define ME_mv2_cross3(m1, v2, ans) \
|
|
{ \
|
|
ans##_0 = m1##_7 * v2##_2 - m1##_8 * v2##_1; \
|
|
ans##_1 = m1##_8 * v2##_0 - m1##_6 * v2##_2; \
|
|
ans##_2 = m1##_6 * v2##_1 - m1##_7 * v2##_0; \
|
|
}
|
|
|
|
|
|
#define ME_compute_eta_torque(m1, m2, s1, ans) \
|
|
{ \
|
|
flt_t den = m1##_3*m1##_2*m1##_7-m1##_0*m1##_5*m1##_7- \
|
|
m1##_2*m1##_6*m1##_4+m1##_1*m1##_6*m1##_5- \
|
|
m1##_3*m1##_1*m1##_8+m1##_0*m1##_4*m1##_8; \
|
|
den = (flt_t)1.0 / den; \
|
|
\
|
|
ans##_0 = s1##_0*(m1##_5*m1##_1*m2##_2+(flt_t)2.0*m1##_4*m1##_8*m2##_0- \
|
|
m1##_4*m2##_2*m1##_2-(flt_t)2.0*m1##_5*m2##_0*m1##_7+ \
|
|
m2##_1*m1##_2*m1##_7-m2##_1*m1##_1*m1##_8- \
|
|
m1##_3*m1##_8*m2##_1+m1##_6*m1##_5*m2##_1+ \
|
|
m1##_3*m2##_2*m1##_7-m2##_2*m1##_6*m1##_4)*den; \
|
|
\
|
|
ans##_1 = s1##_0*(m1##_2*m2##_0*m1##_7-m1##_8*m2##_0*m1##_1+ \
|
|
(flt_t)2.0*m1##_0*m1##_8*m2##_1-m1##_0*m2##_2*m1##_5- \
|
|
(flt_t)2.0*m1##_6*m1##_2*m2##_1+m2##_2*m1##_3*m1##_2- \
|
|
m1##_8*m1##_3*m2##_0+m1##_6*m2##_0*m1##_5+ \
|
|
m1##_6*m2##_2*m1##_1-m2##_2*m1##_0*m1##_7)*den; \
|
|
\
|
|
ans##_2 = s1##_0*(m1##_1*m1##_5*m2##_0-m1##_2*m2##_0*m1##_4- \
|
|
m1##_0*m1##_5*m2##_1+m1##_3*m1##_2*m2##_1- \
|
|
m2##_1*m1##_0*m1##_7-m1##_6*m1##_4*m2##_0+ \
|
|
(flt_t)2.0*m1##_4*m1##_0*m2##_2- \
|
|
(flt_t)2.0*m1##_3*m2##_2*m1##_1+ \
|
|
m1##_3*m1##_7*m2##_0+m1##_6*m2##_1*m1##_1)*den; \
|
|
\
|
|
ans##_3 = s1##_1*(-m1##_4*m2##_5*m1##_2+(flt_t)2.0*m1##_4*m1##_8*m2##_3+ \
|
|
m1##_5*m1##_1*m2##_5-(flt_t)2.0*m1##_5*m2##_3*m1##_7+ \
|
|
m2##_4*m1##_2*m1##_7-m2##_4*m1##_1*m1##_8- \
|
|
m1##_3*m1##_8*m2##_4+m1##_6*m1##_5*m2##_4- \
|
|
m2##_5*m1##_6*m1##_4+m1##_3*m2##_5*m1##_7)*den; \
|
|
\
|
|
ans##_4 = s1##_1*(m1##_2*m2##_3*m1##_7-m1##_1*m1##_8*m2##_3+ \
|
|
(flt_t)2.0*m1##_8*m1##_0*m2##_4-m2##_5*m1##_0*m1##_5- \
|
|
(flt_t)2.0*m1##_6*m2##_4*m1##_2-m1##_3*m1##_8*m2##_3+ \
|
|
m1##_6*m1##_5*m2##_3+m1##_3*m2##_5*m1##_2- \
|
|
m1##_0*m2##_5*m1##_7+m2##_5*m1##_1*m1##_6)*den; \
|
|
\
|
|
ans##_5 = s1##_1*(m1##_1*m1##_5*m2##_3-m1##_2*m2##_3*m1##_4- \
|
|
m1##_0*m1##_5*m2##_4+m1##_3*m1##_2*m2##_4+ \
|
|
(flt_t)2.0*m1##_4*m1##_0*m2##_5-m1##_0*m2##_4*m1##_7+ \
|
|
m1##_1*m1##_6*m2##_4-m2##_3*m1##_6*m1##_4- \
|
|
(flt_t)2.0*m1##_3*m1##_1*m2##_5+m1##_3*m2##_3*m1##_7)* \
|
|
den; \
|
|
\
|
|
ans##_6 = s1##_2*(-m1##_4*m1##_2*m2##_8+m1##_1*m1##_5*m2##_8+ \
|
|
(flt_t)2.0*m1##_4*m2##_6*m1##_8-m1##_1*m2##_7*m1##_8+ \
|
|
m1##_2*m1##_7*m2##_7-(flt_t)2.0*m2##_6*m1##_7*m1##_5- \
|
|
m1##_3*m2##_7*m1##_8+m1##_5*m1##_6*m2##_7- \
|
|
m1##_4*m1##_6*m2##_8+m1##_7*m1##_3*m2##_8)*den; \
|
|
\
|
|
ans##_7 = s1##_2*-(m1##_1*m1##_8*m2##_6-m1##_2*m2##_6*m1##_7- \
|
|
(flt_t)2.0*m2##_7*m1##_0*m1##_8+m1##_5*m2##_8*m1##_0+ \
|
|
(flt_t)2.0*m2##_7*m1##_2*m1##_6+m1##_3*m2##_6*m1##_8- \
|
|
m1##_3*m1##_2*m2##_8-m1##_5*m1##_6*m2##_6+ \
|
|
m1##_0*m2##_8*m1##_7-m2##_8*m1##_1*m1##_6)*den; \
|
|
\
|
|
ans##_8 = s1##_2*(m1##_1*m1##_5*m2##_6-m1##_2*m2##_6*m1##_4- \
|
|
m1##_0*m1##_5*m2##_7+m1##_3*m1##_2*m2##_7- \
|
|
m1##_4*m1##_6*m2##_6-m1##_7*m2##_7*m1##_0+ \
|
|
(flt_t)2.0*m1##_4*m2##_8*m1##_0+m1##_7*m1##_3*m2##_6+ \
|
|
m1##_6*m1##_1*m2##_7-(flt_t)2.0*m2##_8*m1##_3*m1##_1)* \
|
|
den; \
|
|
}
|
|
|
|
#define ME_vcopy4(dst,src) \
|
|
dst##_0 = src##_0; \
|
|
dst##_1 = src##_1; \
|
|
dst##_2 = src##_2; \
|
|
dst##_3 = src##_3;
|
|
|
|
#define ME_mldivide3(m1, v_0, v_1, v_2, ans, error) \
|
|
{ \
|
|
flt_t aug_0, aug_1, aug_2, aug_3, aug_4, aug_5; \
|
|
flt_t aug_6, aug_7, aug_8, aug_9, aug_10, aug_11, t; \
|
|
\
|
|
aug_3 = v_0; \
|
|
aug_0 = m1##_0; \
|
|
aug_1 = m1##_1; \
|
|
aug_2 = m1##_2; \
|
|
aug_7 = v_1; \
|
|
aug_4 = m1##_3; \
|
|
aug_5 = m1##_4; \
|
|
aug_6 = m1##_5; \
|
|
aug_11 = v_2; \
|
|
aug_8 = m1##_6; \
|
|
aug_9 = m1##_7; \
|
|
aug_10 = m1##_8; \
|
|
\
|
|
if (fabs(aug_4) > fabs(aug_0)) { \
|
|
flt_t swapt; \
|
|
swapt = aug_0; aug_0 = aug_4; aug_4 = swapt; \
|
|
swapt = aug_1; aug_1 = aug_5; aug_5 = swapt; \
|
|
swapt = aug_2; aug_2 = aug_6; aug_6 = swapt; \
|
|
swapt = aug_3; aug_3 = aug_7; aug_7 = swapt; \
|
|
} \
|
|
if (fabs(aug_8) > fabs(aug_0)) { \
|
|
flt_t swapt; \
|
|
swapt = aug_0; aug_0 = aug_8; aug_8 = swapt; \
|
|
swapt = aug_1; aug_1 = aug_9; aug_9 = swapt; \
|
|
swapt = aug_2; aug_2 = aug_10; aug_10 = swapt; \
|
|
swapt = aug_3; aug_3 = aug_11; aug_11 = swapt; \
|
|
} \
|
|
\
|
|
if (aug_0 != (flt_t)0.0) { \
|
|
} else if (aug_4 != (flt_t)0.0) { \
|
|
flt_t swapt; \
|
|
swapt = aug_0; aug_0 = aug_4; aug_4 = swapt; \
|
|
swapt = aug_1; aug_1 = aug_5; aug_5 = swapt; \
|
|
swapt = aug_2; aug_2 = aug_6; aug_6 = swapt; \
|
|
swapt = aug_3; aug_3 = aug_7; aug_7 = swapt; \
|
|
} else if (aug_8 != (flt_t)0.0) { \
|
|
flt_t swapt; \
|
|
swapt = aug_0; aug_0 = aug_8; aug_8 = swapt; \
|
|
swapt = aug_1; aug_1 = aug_9; aug_9 = swapt; \
|
|
swapt = aug_2; aug_2 = aug_10; aug_10 = swapt; \
|
|
swapt = aug_3; aug_3 = aug_11; aug_11 = swapt; \
|
|
} else \
|
|
error = 1; \
|
|
\
|
|
t = aug_4 / aug_0; \
|
|
aug_5 -= t * aug_1; \
|
|
aug_6 -= t * aug_2; \
|
|
aug_7 -= t * aug_3; \
|
|
t = aug_8 / aug_0; \
|
|
aug_9 -= t * aug_1; \
|
|
aug_10 -= t * aug_2; \
|
|
aug_11 -= t * aug_3; \
|
|
\
|
|
if (fabs(aug_9) > fabs(aug_5)) { \
|
|
flt_t swapt; \
|
|
swapt = aug_4; aug_4 = aug_8; aug_8 = swapt; \
|
|
swapt = aug_5; aug_5 = aug_9; aug_9 = swapt; \
|
|
swapt = aug_6; aug_6 = aug_10; aug_10 = swapt; \
|
|
swapt = aug_7; aug_7 = aug_11; aug_11 = swapt; \
|
|
} \
|
|
\
|
|
if (aug_5 != (flt_t)0.0) { \
|
|
} else if (aug_9 != (flt_t)0.0) { \
|
|
flt_t swapt; \
|
|
swapt = aug_4; aug_4 = aug_8; aug_8 = swapt; \
|
|
swapt = aug_5; aug_5 = aug_9; aug_9 = swapt; \
|
|
swapt = aug_6; aug_6 = aug_10; aug_10 = swapt; \
|
|
swapt = aug_7; aug_7 = aug_11; aug_11 = swapt; \
|
|
} \
|
|
\
|
|
t = aug_9 / aug_5; \
|
|
aug_10 -= t * aug_6; \
|
|
aug_11 -= t * aug_7; \
|
|
\
|
|
if (aug_10 == (flt_t)0.0) \
|
|
error = 1; \
|
|
\
|
|
ans##_2 = aug_11/aug_10; \
|
|
t = (flt_t)0.0; \
|
|
t += aug_6 * ans##_2; \
|
|
ans##_1 = (aug_7-t) / aug_5; \
|
|
t = (flt_t)0.0; \
|
|
t += aug_1 * ans##_1; \
|
|
t += aug_2 * ans##_2; \
|
|
ans##_0 = (aug_3 - t) / aug_0; \
|
|
}
|
|
|
|
/* ----------------------------------------------------------------------
|
|
normalize a quaternion
|
|
------------------------------------------------------------------------- */
|
|
|
|
#define ME_qnormalize(q) \
|
|
{ \
|
|
double norm = 1.0 / \
|
|
std::sqrt(q##_w*q##_w + q##_i*q##_i + q##_j*q##_j + q##_k*q##_k); \
|
|
q##_w *= norm; \
|
|
q##_i *= norm; \
|
|
q##_j *= norm; \
|
|
q##_k *= norm; \
|
|
}
|
|
|
|
/* ----------------------------------------------------------------------
|
|
compute omega from angular momentum
|
|
w = omega = angular velocity in space frame
|
|
wbody = angular velocity in body frame
|
|
project space-frame angular momentum onto body axes
|
|
and divide by principal moments
|
|
------------------------------------------------------------------------- */
|
|
|
|
#define ME_mq_to_omega(m, quat, moments_0, moments_1, moments_2, w) \
|
|
{ \
|
|
double wbody_0, wbody_1, wbody_2; \
|
|
double rot_0, rot_1, rot_2, rot_3, rot_4, rot_5, rot_6, rot_7, rot_8; \
|
|
\
|
|
double w2 = quat##_w * quat##_w; \
|
|
double i2 = quat##_i * quat##_i; \
|
|
double j2 = quat##_j * quat##_j; \
|
|
double k2 = quat##_k * quat##_k; \
|
|
double twoij = 2.0 * quat##_i * quat##_j; \
|
|
double twoik = 2.0 * quat##_i * quat##_k; \
|
|
double twojk = 2.0 * quat##_j * quat##_k; \
|
|
double twoiw = 2.0 * quat##_i * quat##_w; \
|
|
double twojw = 2.0 * quat##_j * quat##_w; \
|
|
double twokw = 2.0 * quat##_k * quat##_w; \
|
|
\
|
|
rot##_0 = w2 + i2 - j2 - k2; \
|
|
rot##_1 = twoij - twokw; \
|
|
rot##_2 = twojw + twoik; \
|
|
\
|
|
rot##_3 = twoij + twokw; \
|
|
rot##_4 = w2 - i2 + j2 - k2; \
|
|
rot##_5 = twojk - twoiw; \
|
|
\
|
|
rot##_6 = twoik - twojw; \
|
|
rot##_7 = twojk + twoiw; \
|
|
rot##_8 = w2 - i2 - j2 + k2; \
|
|
\
|
|
wbody_0 = rot##_0*m##_0 + rot##_3*m##_1 + rot##_6*m##_2; \
|
|
wbody_1 = rot##_1*m##_0 + rot##_4*m##_1 + rot##_7*m##_2; \
|
|
wbody_2 = rot##_2*m##_0 + rot##_5*m##_1 + rot##_8*m##_2; \
|
|
\
|
|
wbody_0 *= moments_0; \
|
|
wbody_1 *= moments_1; \
|
|
wbody_2 *= moments_2; \
|
|
\
|
|
w##_0 = rot##_0*wbody_0 + rot##_1*wbody_1 + rot##_2*wbody_2; \
|
|
w##_1 = rot##_3*wbody_0 + rot##_4*wbody_1 + rot##_5*wbody_2; \
|
|
w##_2 = rot##_6*wbody_0 + rot##_7*wbody_1 + rot##_8*wbody_2; \
|
|
}
|
|
|
|
#define ME_omega_richardson(dtf,dtq,angmomin,quatin,torque,i0,i1,i2) \
|
|
{ \
|
|
angmomin[0] += dtf * torque[0]; \
|
|
double angmom_0 = angmomin[0]; \
|
|
angmomin[1] += dtf * torque[1]; \
|
|
double angmom_1 = angmomin[1]; \
|
|
angmomin[2] += dtf * torque[2]; \
|
|
double angmom_2 = angmomin[2]; \
|
|
\
|
|
double quat_w = quatin[0]; \
|
|
double quat_i = quatin[1]; \
|
|
double quat_j = quatin[2]; \
|
|
double quat_k = quatin[3]; \
|
|
\
|
|
double omega_0, omega_1, omega_2; \
|
|
ME_mq_to_omega(angmom,quat,i0,i1,i2,omega); \
|
|
\
|
|
double wq_0, wq_1, wq_2, wq_3; \
|
|
wq_0 = -omega_0*quat_i - omega_1*quat_j - omega_2*quat_k; \
|
|
wq_1 = quat_w*omega_0 + omega_1*quat_k - omega_2*quat_j; \
|
|
wq_2 = quat_w*omega_1 + omega_2*quat_i - omega_0*quat_k; \
|
|
wq_3 = quat_w*omega_2 + omega_0*quat_j - omega_1*quat_i; \
|
|
\
|
|
double qfull_w, qfull_i, qfull_j, qfull_k; \
|
|
qfull_w = quat_w + dtq * wq_0; \
|
|
qfull_i = quat_i + dtq * wq_1; \
|
|
qfull_j = quat_j + dtq * wq_2; \
|
|
qfull_k = quat_k + dtq * wq_3; \
|
|
ME_qnormalize(qfull); \
|
|
\
|
|
double qhalf_w, qhalf_i, qhalf_j, qhalf_k; \
|
|
qhalf_w = quat_w + 0.5*dtq * wq_0; \
|
|
qhalf_i = quat_i + 0.5*dtq * wq_1; \
|
|
qhalf_j = quat_j + 0.5*dtq * wq_2; \
|
|
qhalf_k = quat_k + 0.5*dtq * wq_3; \
|
|
ME_qnormalize(qhalf); \
|
|
\
|
|
ME_mq_to_omega(angmom,qhalf,i0,i1,i2,omega); \
|
|
wq_0 = -omega_0*qhalf_i - omega_1*qhalf_j - omega_2*qhalf_k; \
|
|
wq_1 = qhalf_w*omega_0 + omega_1*qhalf_k - omega_2*qhalf_j; \
|
|
wq_2 = qhalf_w*omega_1 + omega_2*qhalf_i - omega_0*qhalf_k; \
|
|
wq_3 = qhalf_w*omega_2 + omega_0*qhalf_j - omega_1*qhalf_i; \
|
|
\
|
|
qhalf_w += 0.5*dtq * wq_0; \
|
|
qhalf_i += 0.5*dtq * wq_1; \
|
|
qhalf_j += 0.5*dtq * wq_2; \
|
|
qhalf_k += 0.5*dtq * wq_3; \
|
|
ME_qnormalize(qhalf); \
|
|
\
|
|
quat_w = 2.0*qhalf_w - qfull_w; \
|
|
quat_i = 2.0*qhalf_i - qfull_i; \
|
|
quat_j = 2.0*qhalf_j - qfull_j; \
|
|
quat_k = 2.0*qhalf_k - qfull_k; \
|
|
ME_qnormalize(quat); \
|
|
\
|
|
quatin[0] = quat_w; \
|
|
quatin[1] = quat_i; \
|
|
quatin[2] = quat_j; \
|
|
quatin[3] = quat_k; \
|
|
}
|
|
|
|
#endif
|