//Torque Matrix T = T1 = T2
// N is the number of beads in each large sphere,
// ie. the dimension of shellsphs
int Tmat(double *T, double *ShellSphs,int N){
int i;
double *lc_eps;
lc_eps = (double*)calloc(27,sizeof(double));
LCepsilon(lc_eps);
for(i = 0; i < N; i++){
int i3 = i*3;
double r[3];
r[0] = ShellSphs[i3];
r[1] = ShellSphs[i3+1];
r[2] = ShellSphs[i3+2];
double *rmat;
rmat = (double*)calloc(9,sizeof(double));
rotmat(&r,rmat,lc_eps);
int kx,ky;
for(kx=0;kx<3;kx++){
for( ky =0; ky<3; ky++){
T[ (i3+kx)*6 + ky ] = (kx==ky ? 1:0);
T[ (i3+kx)*6 + (3+ky)]= rmat[3*kx+ky];
// Q[ kx*3*N + (i3+ ky)] = (kx==ky? 1:0);
// Q[(3+kx)*3*N +(i3 +ky)] = -rmat[3*kx +ky];
}
}
free(rmat);
}
free(lc_eps);
return 0;
}
Matrix CoordinateSystem::getRotFromCompass(CompassData& compassData, GravityData& gravityData) {
Vector compass_vector(3);
compass_vector(0) = compassData.x();
compass_vector(1) = compassData.y();
compass_vector(2) = compassData.z();
Vector gravity_vector(3);
gravity_vector(0) = gravityData.x();
gravity_vector(1) = gravityData.y();
gravity_vector(2) = gravityData.z();
Vector East(3);
cross_prod(compass_vector, gravity_vector, East); // East = cross(compass_vector, gravity_vector)
double normEast = norm_2(East);
if (normEast < 0)
return IdentityMatrix(3);
East /= normEast; // normalization
gravity_vector /= norm_2(gravity_vector);
Vector North(3);
cross_prod(gravity_vector, East, North); // North = cross(gravity_vector, East)
Matrix rotmat(3, 3);
row(rotmat, 0) = East;
row(rotmat, 1) = North;
row(rotmat, 2) = gravity_vector;
return Matrix(rotmat);
}
int main( void ) {
// init
time_t seed = time(NULL);
printf("seed: %ld\n", seed);
srand((unsigned int)seed); // might break in 2038
aa_test_ulimit();
for( size_t i = 0; i < 1000; i++ ) {
/* Random Data */
static const size_t k=2;
double E[2][7], S[2][8], T[2][12], dx[2][6];
for( size_t j = 0; j < k; j ++ ) {
rand_tf(E[j], S[j], T[j]);
aa_vrand(6,dx[j]);
}
//printf("%d\n",i);
/* Run Tests */
rotvec(E[0]);
euler(dx[0]);
euler1(dx[0]);
eulerzyx(E[0]);
chain(E,S,T);
quat(E);
duqu();
rel_q();
rel_d();
slerp();
theta2quat();
rotmat(E[0]);
tfmat();
tfmat_inv(T[0]);
mzlook(dx[0]+0, dx[0]+3, dx[1]+0);
integrate(E[0], S[0], T[0], dx[0]);
tf_conj(E, S);
qdiff(E,dx);
}
return 0;
}
// code for Matrix Q can be deleted.
//Matrix Q that converts forces on individual bead to total force/torque
int Qmatrix(double *Q, double *ShellSphs, int N ){
int i;
for(i=0; i< N; i++){
int i3 = i*3;
double r[3];
r[0] = ShellSphs[i3];
r[1] = ShellSphs[i3+1];
r[2] = ShellSphs[i3+2];
double *rmat;
rmat = (double*)calloc(9,sizeof(double));
rotmat(&r, rmat);
int kx,ky;
for(kx=0; kx < 3; kx++){
for(ky = 0; ky < 3; ky++){
Q[ kx*3*N + (i3+ ky)] = (kx==ky? 1:0);
Q[(3+kx)*3*N +(i3 +ky)] = -rmat[3*kx +ky];
}
}
}
return 0;
}
