Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F82540178
math_extra.cpp
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
Thu, Sep 12, 00:15
Size
21 KB
Mime Type
text/x-c
Expires
Sat, Sep 14, 00:15 (1 d, 23 h)
Engine
blob
Format
Raw Data
Handle
20712915
Attached To
rLAMMPS lammps
math_extra.cpp
View Options
/* ----------------------------------------------------------------------
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: Mike Brown (SNL)
------------------------------------------------------------------------- */
#include <stdio.h>
#include <string.h>
#include "math_extra.h"
#define MAXJACOBI 50
namespace MathExtra {
/* ----------------------------------------------------------------------
output a matrix
------------------------------------------------------------------------- */
void 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");
}
}
/* ----------------------------------------------------------------------
solve Ax = b or M ans = v
use gaussian elimination & partial pivoting on matrix
------------------------------------------------------------------------- */
int mldivide3(const double m[3][3], const double *v, 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 (p < 3 && aug[p][i] == 0.0) p++;
if (p == 3) return 1;
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) return 1;
// 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];
}
return 0;
}
/* ----------------------------------------------------------------------
compute evalues and evectors of 3x3 real symmetric matrix
based on Jacobi rotations
adapted from Numerical Recipes jacobi() function
------------------------------------------------------------------------- */
int jacobi(double matrix[3][3], double *evalues, double evectors[3][3])
{
int i,j,k;
double tresh,theta,tau,t,sm,s,h,g,c,b[3],z[3];
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) evectors[i][j] = 0.0;
evectors[i][i] = 1.0;
}
for (i = 0; i < 3; i++) {
b[i] = evalues[i] = matrix[i][i];
z[i] = 0.0;
}
for (int iter = 1; iter <= MAXJACOBI; iter++) {
sm = 0.0;
for (i = 0; i < 2; i++)
for (j = i+1; j < 3; j++)
sm += fabs(matrix[i][j]);
if (sm == 0.0) return 0;
if (iter < 4) tresh = 0.2*sm/(3*3);
else tresh = 0.0;
for (i = 0; i < 2; i++) {
for (j = i+1; j < 3; j++) {
g = 100.0*fabs(matrix[i][j]);
if (iter > 4 && fabs(evalues[i])+g == fabs(evalues[i])
&& fabs(evalues[j])+g == fabs(evalues[j]))
matrix[i][j] = 0.0;
else if (fabs(matrix[i][j]) > tresh) {
h = evalues[j]-evalues[i];
if (fabs(h)+g == fabs(h)) t = (matrix[i][j])/h;
else {
theta = 0.5*h/(matrix[i][j]);
t = 1.0/(fabs(theta)+sqrt(1.0+theta*theta));
if (theta < 0.0) t = -t;
}
c = 1.0/sqrt(1.0+t*t);
s = t*c;
tau = s/(1.0+c);
h = t*matrix[i][j];
z[i] -= h;
z[j] += h;
evalues[i] -= h;
evalues[j] += h;
matrix[i][j] = 0.0;
for (k = 0; k < i; k++) rotate(matrix,k,i,k,j,s,tau);
for (k = i+1; k < j; k++) rotate(matrix,i,k,k,j,s,tau);
for (k = j+1; k < 3; k++) rotate(matrix,i,k,j,k,s,tau);
for (k = 0; k < 3; k++) rotate(evectors,k,i,k,j,s,tau);
}
}
}
for (i = 0; i < 3; i++) {
evalues[i] = b[i] += z[i];
z[i] = 0.0;
}
}
return 1;
}
/* ----------------------------------------------------------------------
perform a single Jacobi rotation
------------------------------------------------------------------------- */
void rotate(double matrix[3][3], int i, int j, int k, int l,
double s, double tau)
{
double g = matrix[i][j];
double h = matrix[k][l];
matrix[i][j] = g-s*(h+g*tau);
matrix[k][l] = h+s*(g-h*tau);
}
/* ----------------------------------------------------------------------
Richardson iteration to update quaternion from angular momentum
return new normalized quaternion q
also returns updated omega at 1/2 step
------------------------------------------------------------------------- */
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);
}
/* ----------------------------------------------------------------------
apply evolution operators to quat, quat momentum
Miller et al., J Chem Phys. 116, 8649-8659 (2002)
------------------------------------------------------------------------- */
void no_squish_rotate(int k, double *p, double *q, double *inertia,
double dt)
{
double phi,c_phi,s_phi,kp[4],kq[4];
// apply permuation operator on p and q, get kp and kq
if (k == 1) {
kq[0] = -q[1]; kp[0] = -p[1];
kq[1] = q[0]; kp[1] = p[0];
kq[2] = q[3]; kp[2] = p[3];
kq[3] = -q[2]; kp[3] = -p[2];
} else if (k == 2) {
kq[0] = -q[2]; kp[0] = -p[2];
kq[1] = -q[3]; kp[1] = -p[3];
kq[2] = q[0]; kp[2] = p[0];
kq[3] = q[1]; kp[3] = p[1];
} else if (k == 3) {
kq[0] = -q[3]; kp[0] = -p[3];
kq[1] = q[2]; kp[1] = p[2];
kq[2] = -q[1]; kp[2] = -p[1];
kq[3] = q[0]; kp[3] = p[0];
}
// obtain phi, cosines and sines
phi = p[0]*kq[0] + p[1]*kq[1] + p[2]*kq[2] + p[3]*kq[3];
if (fabs(inertia[k-1]) < 1e-6) phi *= 0.0;
else phi /= 4.0 * inertia[k-1];
c_phi = cos(dt * phi);
s_phi = sin(dt * phi);
// advance p and q
p[0] = c_phi*p[0] + s_phi*kp[0];
p[1] = c_phi*p[1] + s_phi*kp[1];
p[2] = c_phi*p[2] + s_phi*kp[2];
p[3] = c_phi*p[3] + s_phi*kp[3];
q[0] = c_phi*q[0] + s_phi*kq[0];
q[1] = c_phi*q[1] + s_phi*kq[1];
q[2] = c_phi*q[2] + s_phi*kq[2];
q[3] = c_phi*q[3] + s_phi*kq[3];
}
/* ----------------------------------------------------------------------
compute omega from angular momentum, both in space frame
only know Idiag so need to do M = Iw in body frame
ex,ey,ez are column vectors of rotation matrix P
Mbody = P_transpose Mspace
wbody = Mbody / Idiag
wspace = P wbody
set wbody component to 0.0 if inertia component is 0.0
otherwise body can spin easily around that axis
------------------------------------------------------------------------- */
void angmom_to_omega(double *m, double *ex, double *ey, double *ez,
double *idiag, double *w)
{
double wbody[3];
if (idiag[0] == 0.0) wbody[0] = 0.0;
else wbody[0] = (m[0]*ex[0] + m[1]*ex[1] + m[2]*ex[2]) / idiag[0];
if (idiag[1] == 0.0) wbody[1] = 0.0;
else wbody[1] = (m[0]*ey[0] + m[1]*ey[1] + m[2]*ey[2]) / idiag[1];
if (idiag[2] == 0.0) wbody[2] = 0.0;
else wbody[2] = (m[0]*ez[0] + m[1]*ez[1] + m[2]*ez[2]) / idiag[2];
w[0] = wbody[0]*ex[0] + wbody[1]*ey[0] + wbody[2]*ez[0];
w[1] = wbody[0]*ex[1] + wbody[1]*ey[1] + wbody[2]*ez[1];
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_matvec(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::matvec(rot,wbody,w);
}
/* ----------------------------------------------------------------------
compute angular momentum from omega, both in space frame
only know Idiag so need to do M = Iw in body frame
ex,ey,ez are column vectors of rotation matrix P
wbody = P_transpose wspace
Mbody = Idiag wbody
Mspace = P Mbody
------------------------------------------------------------------------- */
void omega_to_angmom(double *w, double *ex, double *ey, double *ez,
double *idiag, double *m)
{
double mbody[3];
mbody[0] = (w[0]*ex[0] + w[1]*ex[1] + w[2]*ex[2]) * idiag[0];
mbody[1] = (w[0]*ey[0] + w[1]*ey[1] + w[2]*ey[2]) * idiag[1];
mbody[2] = (w[0]*ez[0] + w[1]*ez[1] + w[2]*ez[2]) * idiag[2];
m[0] = mbody[0]*ex[0] + mbody[1]*ey[0] + mbody[2]*ez[0];
m[1] = mbody[0]*ex[1] + mbody[1]*ey[1] + mbody[2]*ez[1];
m[2] = mbody[0]*ex[2] + mbody[1]*ey[2] + mbody[2]*ez[2];
}
/* ----------------------------------------------------------------------
create unit quaternion from space-frame ex,ey,ez
ex,ey,ez are columns of a rotation matrix
------------------------------------------------------------------------- */
void exyz_to_q(double *ex, double *ey, double *ez, double *q)
{
// squares of quaternion components
double q0sq = 0.25 * (ex[0] + ey[1] + ez[2] + 1.0);
double q1sq = q0sq - 0.5 * (ey[1] + ez[2]);
double q2sq = q0sq - 0.5 * (ex[0] + ez[2]);
double q3sq = q0sq - 0.5 * (ex[0] + ey[1]);
// some component must be greater than 1/4 since they sum to 1
// compute other components from it
if (q0sq >= 0.25) {
q[0] = sqrt(q0sq);
q[1] = (ey[2] - ez[1]) / (4.0*q[0]);
q[2] = (ez[0] - ex[2]) / (4.0*q[0]);
q[3] = (ex[1] - ey[0]) / (4.0*q[0]);
} else if (q1sq >= 0.25) {
q[1] = sqrt(q1sq);
q[0] = (ey[2] - ez[1]) / (4.0*q[1]);
q[2] = (ey[0] + ex[1]) / (4.0*q[1]);
q[3] = (ex[2] + ez[0]) / (4.0*q[1]);
} else if (q2sq >= 0.25) {
q[2] = sqrt(q2sq);
q[0] = (ez[0] - ex[2]) / (4.0*q[2]);
q[1] = (ey[0] + ex[1]) / (4.0*q[2]);
q[3] = (ez[1] + ey[2]) / (4.0*q[2]);
} else if (q3sq >= 0.25) {
q[3] = sqrt(q3sq);
q[0] = (ex[1] - ey[0]) / (4.0*q[3]);
q[1] = (ez[0] + ex[2]) / (4.0*q[3]);
q[2] = (ez[1] + ey[2]) / (4.0*q[3]);
}
qnormalize(q);
}
/* ----------------------------------------------------------------------
compute space-frame ex,ey,ez from current quaternion q
ex,ey,ez = space-frame coords of 1st,2nd,3rd principal axis
operation is ex = q' d q = Q d, where d is (1,0,0) = 1st axis in body frame
------------------------------------------------------------------------- */
void q_to_exyz(double *q, double *ex, double *ey, double *ez)
{
ex[0] = q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3];
ex[1] = 2.0 * (q[1]*q[2] + q[0]*q[3]);
ex[2] = 2.0 * (q[1]*q[3] - q[0]*q[2]);
ey[0] = 2.0 * (q[1]*q[2] - q[0]*q[3]);
ey[1] = q[0]*q[0] - q[1]*q[1] + q[2]*q[2] - q[3]*q[3];
ey[2] = 2.0 * (q[2]*q[3] + q[0]*q[1]);
ez[0] = 2.0 * (q[1]*q[3] + q[0]*q[2]);
ez[1] = 2.0 * (q[2]*q[3] - q[0]*q[1]);
ez[2] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
}
/* ----------------------------------------------------------------------
compute rotation matrix from quaternion
quat = [w i j k]
------------------------------------------------------------------------- */
void 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 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 space-frame inertia tensor of an ellipsoid
radii = 3 radii of ellipsoid
quat = orientiation quaternion of ellipsoid
return symmetric inertia tensor as 6-vector in Voigt notation
------------------------------------------------------------------------- */
void inertia_ellipsoid(double *radii, double *quat, double mass,
double *inertia)
{
double p[3][3],ptrans[3][3],itemp[3][3],tensor[3][3];
double idiag[3];
quat_to_mat(quat,p);
quat_to_mat_trans(quat,ptrans);
idiag[0] = 0.2*mass * (radii[1]*radii[1] + radii[2]*radii[2]);
idiag[1] = 0.2*mass * (radii[0]*radii[0] + radii[2]*radii[2]);
idiag[2] = 0.2*mass * (radii[0]*radii[0] + radii[1]*radii[1]);
diag_times3(idiag,ptrans,itemp);
times3(p,itemp,tensor);
inertia[0] = tensor[0][0];
inertia[1] = tensor[1][1];
inertia[2] = tensor[2][2];
inertia[3] = tensor[1][2];
inertia[4] = tensor[0][2];
inertia[5] = tensor[0][1];
}
/* ----------------------------------------------------------------------
compute space-frame inertia tensor of a line segment in 2d
length = length of line
theta = orientiation of line
return symmetric inertia tensor as 6-vector in Voigt notation
------------------------------------------------------------------------- */
void inertia_line(double length, double theta, double mass, double *inertia)
{
double p[3][3],ptrans[3][3],itemp[3][3],tensor[3][3];
double q[4],idiag[3];
q[0] = cos(0.5*theta);
q[1] = q[2] = 0.0;
q[3] = sin(0.5*theta);
MathExtra::quat_to_mat(q,p);
MathExtra::quat_to_mat_trans(q,ptrans);
idiag[0] = 0.0;
idiag[1] = 1.0/12.0 * mass * length*length;
idiag[2] = 1.0/12.0 * mass * length*length;
MathExtra::diag_times3(idiag,ptrans,itemp);
MathExtra::times3(p,itemp,tensor);
inertia[0] = tensor[0][0];
inertia[1] = tensor[1][1];
inertia[2] = tensor[2][2];
inertia[3] = tensor[1][2];
inertia[4] = tensor[0][2];
inertia[5] = tensor[0][1];
}
/* ----------------------------------------------------------------------
compute space-frame inertia tensor of a triangle
v0,v1,v2 = 3 vertices of triangle
from http://en.wikipedia.org/wiki/Inertia_tensor_of_triangle
inertia tensor = a/24 (v0^2 + v1^2 + v2^2 + (v0+v1+v2)^2) I - a Vt S V
a = 2*area of tri = |(v1-v0) x (v2-v0)|
I = 3x3 identity matrix
V = 3x3 matrix with v0,v1,v2 as rows
Vt = 3x3 matrix with v0,v1,v2 as columns
S = 1/24 [2 1 1]
[1 2 1]
[1 1 2]
return symmetric inertia tensor as 6-vector in Voigt notation
------------------------------------------------------------------------- */
void inertia_triangle(double *v0, double *v1, double *v2,
double mass, double *inertia)
{
double s[3][3] = {{2.0, 1.0, 1.0}, {1.0, 2.0, 1.0}, {1.0, 1.0, 2.0}};
double v[3][3],sv[3][3],vtsv[3][3];
double vvv[3],v1mv0[3],v2mv0[3],normal[3];
v[0][0] = v0[0]; v[0][1] = v0[1]; v[0][2] = v0[2];
v[1][0] = v1[0]; v[1][1] = v1[1]; v[1][2] = v1[2];
v[2][0] = v2[0]; v[2][1] = v2[1]; v[2][2] = v2[2];
times3(s,v,sv);
transpose_times3(v,sv,vtsv);
double sum = lensq3(v0) + lensq3(v1) + lensq3(v2);
vvv[0] = v0[0] + v1[0] + v2[0];
vvv[1] = v0[1] + v1[1] + v2[1];
vvv[2] = v0[2] + v1[2] + v2[2];
sum += lensq3(vvv);
sub3(v1,v0,v1mv0);
sub3(v2,v0,v2mv0);
cross3(v1mv0,v2mv0,normal);
double a = len3(normal);
double inv24 = mass/24.0;
inertia[0] = inv24*a*(sum-vtsv[0][0]);
inertia[1] = inv24*a*(sum-vtsv[1][1]);
inertia[2] = inv24*a*(sum-vtsv[2][2]);
inertia[3] = -inv24*a*vtsv[1][2];
inertia[4] = -inv24*a*vtsv[0][2];
inertia[5] = -inv24*a*vtsv[0][1];
}
/* ----------------------------------------------------------------------
compute space-frame inertia tensor of a triangle
idiag = previously computed diagonal inertia tensor
quat = orientiation quaternion of triangle
return symmetric inertia tensor as 6-vector in Voigt notation
------------------------------------------------------------------------- */
void inertia_triangle(double *idiag, double *quat, double mass,
double *inertia)
{
double p[3][3],ptrans[3][3],itemp[3][3],tensor[3][3];
quat_to_mat(quat,p);
quat_to_mat_trans(quat,ptrans);
diag_times3(idiag,ptrans,itemp);
times3(p,itemp,tensor);
inertia[0] = tensor[0][0];
inertia[1] = tensor[1][1];
inertia[2] = tensor[2][2];
inertia[3] = tensor[1][2];
inertia[4] = tensor[0][2];
inertia[5] = tensor[0][1];
}
/* ----------------------------------------------------------------------
Build rotation matrix for a small angle rotation around the X axis
------------------------------------------------------------------------- */
void BuildRxMatrix(double R[3][3], const double angle)
{
const double angleSq = angle * angle;
const double cosAngle = (1.0 - angleSq * 0.25) / (1.0 + angleSq * 0.25);
const double sinAngle = angle / (1.0 + angleSq * 0.25);
R[0][0] = 1.0; R[0][1] = 0.0; R[0][2] = 0.0;
R[1][0] = 0.0; R[1][1] = cosAngle; R[1][2] = -sinAngle;
R[2][0] = 0.0; R[2][1] = sinAngle; R[2][2] = cosAngle;
}
/* ----------------------------------------------------------------------
Build rotation matrix for a small angle rotation around the Y axis
------------------------------------------------------------------------- */
void BuildRyMatrix(double R[3][3], const double angle)
{
const double angleSq = angle * angle;
const double cosAngle = (1.0 - angleSq * 0.25) / (1.0 + angleSq * 0.25);
const double sinAngle = angle / (1.0 + angleSq * 0.25);
R[0][0] = cosAngle; R[0][1] = 0.0; R[0][2] = sinAngle;
R[1][0] = 0.0; R[1][1] = 1.0; R[1][2] = 0.0;
R[2][0] = -sinAngle; R[2][1] = 0.0; R[2][2] = cosAngle;
}
/* ----------------------------------------------------------------------
Build rotation matrix for a small angle rotation around the Y axis
------------------------------------------------------------------------- */
void BuildRzMatrix(double R[3][3], const double angle)
{
const double angleSq = angle * angle;
const double cosAngle = (1.0 - angleSq * 0.25) / (1.0 + angleSq * 0.25);
const double sinAngle = angle / (1.0 + angleSq * 0.25);
R[0][0] = cosAngle; R[0][1] = -sinAngle; R[0][2] = 0.0;
R[1][0] = sinAngle; R[1][1] = cosAngle; R[1][2] = 0.0;
R[2][0] = 0.0; R[2][1] = 0.0; R[2][2] = 1.0;
}
/* ---------------------------------------------------------------------- */
}
Event Timeline
Log In to Comment