ICODE-ADC/lammps
4
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git-svn-id: svn://svn.icms.temple.edu/lammps-ro/trunk@5978 f3b2605a-c512-4ea7-a41b-209d697bcdaa

Browse Source
This commit is contained in:
sjplimp
2011-04-19 20:57:30 +00:00
parent 99ed989715
commit 105ebaac24
9 changed files with 114 additions and 239 deletions
Download Patch File Download Diff File Expand all files Collapse all files

76
src/math_extra.cpp
View File

@ -174,6 +174,58 @@ void rotate(double matrix[3][3], int i, int j, int k, int l,
matrix[k][l] = h+s*(g-h*tau);
}
/* ----------------------------------------------------------------------
Richardson iteration to update quaternion from angular momentum
return new normalized quaternion q
also returns
------------------------------------------------------------------------- */
void richardson(double *q, double *m, double *w, double *moments, double dtq)
{
// full update from dq/dt = 1/2 w q
double wq[4];
MathExtra::vecquat(w,q,wq);
double qfull[4];
qfull[0] = q[0] + dtq * wq[0];
qfull[1] = q[1] + dtq * wq[1];
qfull[2] = q[2] + dtq * wq[2];
qfull[3] = q[3] + dtq * wq[3];
MathExtra::qnormalize(qfull);
// 1st half update from dq/dt = 1/2 w q
double qhalf[4];
qhalf[0] = q[0] + 0.5*dtq * wq[0];
qhalf[1] = q[1] + 0.5*dtq * wq[1];
qhalf[2] = q[2] + 0.5*dtq * wq[2];
qhalf[3] = q[3] + 0.5*dtq * wq[3];
MathExtra::qnormalize(qhalf);
// re-compute omega at 1/2 step from m at 1/2 step and q at 1/2 step
// recompute wq
MathExtra::mq_to_omega(m,qhalf,moments,w);
MathExtra::vecquat(w,qhalf,wq);
// 2nd half update from dq/dt = 1/2 w q
qhalf[0] += 0.5*dtq * wq[0];
qhalf[1] += 0.5*dtq * wq[1];
qhalf[2] += 0.5*dtq * wq[2];
qhalf[3] += 0.5*dtq * wq[3];
MathExtra::qnormalize(qhalf);
// corrected Richardson update
q[0] = 2.0*qhalf[0] - qfull[0];
q[1] = 2.0*qhalf[1] - qfull[1];
q[2] = 2.0*qhalf[2] - qfull[2];
q[3] = 2.0*qhalf[3] - qfull[3];
MathExtra::qnormalize(q);
}
/* ----------------------------------------------------------------------
compute rotation matrix from quaternion
quat = [w i j k]
@ -264,6 +316,30 @@ void angmom_to_omega(double *m, double *ex, double *ey, double *ez,
w[2] = wbody[0]*ex[2] + wbody[1]*ey[2] + wbody[2]*ez[2];
}
/* ----------------------------------------------------------------------
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
------------------------------------------------------------------------- */
void mq_to_omega(double *m, double *q, double *moments, double *w)
{
double wbody[3];
double rot[3][3];
MathExtra::quat_to_mat(q,rot);
MathExtra::transpose_times_column3(rot,m,wbody);
if (moments[0] == 0.0) wbody[0] = 0.0;
else wbody[0] /= moments[0];
if (moments[1] == 0.0) wbody[1] = 0.0;
else wbody[1] /= moments[1];
if (moments[2] == 0.0) wbody[2] = 0.0;
else wbody[2] /= moments[2];
MathExtra::times_column3(rot,wbody,w);
}
/* ----------------------------------------------------------------------
compute angular momentum from omega, both in space frame
only know Idiag so need to do M = Iw in body frame