void mat_init(Matrix& m, int height, int width)
{
m.width = width;
m.height = height;
size_t size = m.height * m.width * sizeof(float);
m.elements = (float*)malloc(size);
mat_fill(m);
}
MATRIX mat_creat( int row, int col, int type )
{
MATRIX A;
if ((A =_mat_creat( row, col )) != NULL)
{
return (mat_fill(A, type));
}
else
{
return (NULL);
}
}
/*
*-----------------------------------------------------------------------------
* funct: mat_inv
* desct: find inverse of a matrix
* given: a = square matrix a
* retrn: square matrix Inverse(A)
* NULL = fails, singular matrix, or malloc() fails
* 1 = success
*-----------------------------------------------------------------------------
*/
MATRIX mat_inv(MATRIX a, MATRIX C)
{
MATRIX A, B, P;
int i, n;
n = MatCol(a);
if ( ( A = mat_creat( n, n, UNDEFINED ) ) == NULL )
return (NULL);
mat_copy(a,A);
if ( ( B = mat_creat( n, 1, UNDEFINED ) ) == NULL )
return (NULL);
if ( ( P = mat_creat( n, 1, UNDEFINED ) ) == NULL )
return (NULL);
// if dimensions of C is wrong
if ( MatRow(a) != MatRow(C) || MatCol(a) != MatCol(C) ) {
printf("mat_inv error: incompatible output matrix size\n");
_exit(-1);
// if dimensions of C is correct
} else {
/*
* - LU-decomposition -
* also check for singular matrix
*/
if (mat_lu(A, P) == -1) {
mat_free(A);
mat_free(B);
mat_free(C);
mat_free(P);
printf("mat_inv error: failed to invert\n");
return (NULL);
}
for (i=0; i<n; i++) {
mat_fill(B, ZERO_MATRIX);
B[i][0] = 1.0;
mat_backsubs1( A, B, C, P, i );
}
}
mat_free(A);
mat_free(B);
mat_free(P);
if (C==NULL) {
printf("mat_inv error: failed to invert\n");
return(NULL);
} else {
return (C);
}
}
//--------------------------------------------------------------------------
// Function to create a multilayer system replicating a standard stack
// structure cs::num_multilayers times
//
// |-----------------------| 1.0 |-----------------------| L2 M2
// | Material 2 | |-----------------------|
// |-----------------------| 0.6 x2 | | L2 M2
// | | --> |-----------------------| L1 M1
// | Material 1 | |-----------------------|
// | | | | L1 M2
// |-----------------------| 0.0 |-----------------------|
//
// The multilayers code divides the total system height into n multiples
// of the defined material heights.
//--------------------------------------------------------------------------
void generate_multilayers(std::vector<cs::catom_t>& catom_array){
// Load number of multilayer repeats to temporary constant
const int num_layers = cs::num_multilayers;
// Determine fractional system height for each layer
const double fractional_system_height = cs::system_dimensions[2]/double(num_layers);
// Unroll min, max and fill for performance
std::vector<double> mat_min(mp::num_materials);
std::vector<double> mat_max(mp::num_materials);
std::vector<bool> mat_fill(mp::num_materials);
for(int mat=0;mat<mp::num_materials;mat++){
mat_min[mat]=mp::material[mat].min;
mat_max[mat]=mp::material[mat].max;
// alloys generally are not defined by height, and so have max = 0.0
if(mat_max[mat]<0.0000001) mat_max[mat]=-0.1;
mat_fill[mat]=mp::material[mat].fill;
}
// Assign materials to generated atoms
for(unsigned int atom=0;atom<catom_array.size();atom++){
// loop over multilayers
for(int multi=0; multi < num_layers; ++multi){
const double multilayer_num = double(multi);
for(int mat=0;mat<mp::num_materials;mat++){
// determine mimimum and maximum heigh for this layer
const double mat_min_z = (mat_min[mat] + multilayer_num)*fractional_system_height;
const double mat_max_z = (mat_max[mat] + multilayer_num)*fractional_system_height;
const double cz=catom_array[atom].z;
// if in range then allocate to material
if((cz>=mat_min_z) && (cz<mat_max_z) && (mat_fill[mat]==false)){
catom_array[atom].material=mat;
catom_array[atom].include=true;
// Optionally recategorize heigh magnetization by layer in multilayer
if(cs::multilayer_height_category) catom_array[atom].lh_category = multi;
}
}
}
}
return;
}
// Main get_nav filter function
void get_nav(struct sensordata *sensorData_ptr, struct nav *navData_ptr, struct control *controlData_ptr){
double tnow, imu_dt;
double dq[4], quat_new[4];
// compute time-elapsed 'dt'
// This compute the navigation state at the DAQ's Time Stamp
tnow = sensorData_ptr->imuData_ptr->time;
imu_dt = tnow - tprev;
tprev = tnow;
// ================== Time Update ===================
// Temporary storage in Matrix form
quat[0] = navData_ptr->quat[0];
quat[1] = navData_ptr->quat[1];
quat[2] = navData_ptr->quat[2];
quat[3] = navData_ptr->quat[3];
a_temp31[0][0] = navData_ptr->vn; a_temp31[1][0] = navData_ptr->ve;
a_temp31[2][0] = navData_ptr->vd;
b_temp31[0][0] = navData_ptr->lat; b_temp31[1][0] = navData_ptr->lon;
b_temp31[2][0] = navData_ptr->alt;
// AHRS Transformations
C_N2B = quat2dcm(quat, C_N2B);
C_B2N = mat_tran(C_N2B, C_B2N);
// Attitude Update
// ... Calculate Navigation Rate
nr = navrate(a_temp31,b_temp31,nr);
dq[0] = 1;
dq[1] = 0.5*om_ib[0][0]*imu_dt;
dq[2] = 0.5*om_ib[1][0]*imu_dt;
dq[3] = 0.5*om_ib[2][0]*imu_dt;
qmult(quat,dq,quat_new);
quat[0] = quat_new[0]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
quat[1] = quat_new[1]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
quat[2] = quat_new[2]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
quat[3] = quat_new[3]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
if(quat[0] < 0) {
// Avoid quaternion flips sign
quat[0] = -quat[0];
quat[1] = -quat[1];
quat[2] = -quat[2];
quat[3] = -quat[3];
}
navData_ptr->quat[0] = quat[0];
navData_ptr->quat[1] = quat[1];
navData_ptr->quat[2] = quat[2];
navData_ptr->quat[3] = quat[3];
quat2eul(navData_ptr->quat,&(navData_ptr->phi),&(navData_ptr->the),&(navData_ptr->psi));
// Velocity Update
dx = mat_mul(C_B2N,f_b,dx);
dx = mat_add(dx,grav,dx);
navData_ptr->vn += imu_dt*dx[0][0];
navData_ptr->ve += imu_dt*dx[1][0];
navData_ptr->vd += imu_dt*dx[2][0];
// Position Update
dx = llarate(a_temp31,b_temp31,dx);
navData_ptr->lat += imu_dt*dx[0][0];
navData_ptr->lon += imu_dt*dx[1][0];
navData_ptr->alt += imu_dt*dx[2][0];
// JACOBIAN
F = mat_fill(F, ZERO_MATRIX);
// ... pos2gs
F[0][3] = 1.0; F[1][4] = 1.0; F[2][5] = 1.0;
// ... gs2pos
F[5][2] = -2*g/EARTH_RADIUS;
// ... gs2att
temp33 = sk(f_b,temp33);
atemp33 = mat_mul(C_B2N,temp33,atemp33);
F[3][6] = -2.0*atemp33[0][0]; F[3][7] = -2.0*atemp33[0][1]; F[3][8] = -2.0*atemp33[0][2];
F[4][6] = -2.0*atemp33[1][0]; F[4][7] = -2.0*atemp33[1][1]; F[4][8] = -2.0*atemp33[1][2];
F[5][6] = -2.0*atemp33[2][0]; F[5][7] = -2.0*atemp33[2][1]; F[5][8] = -2.0*atemp33[2][2];
// ... gs2acc
F[3][9] = -C_B2N[0][0]; F[3][10] = -C_B2N[0][1]; F[3][11] = -C_B2N[0][2];
F[4][9] = -C_B2N[1][0]; F[4][10] = -C_B2N[1][1]; F[4][11] = -C_B2N[1][2];
F[5][9] = -C_B2N[2][0]; F[5][10] = -C_B2N[2][1]; F[5][11] = -C_B2N[2][2];
// ... att2att
temp33 = sk(om_ib,temp33);
F[6][6] = -temp33[0][0]; F[6][7] = -temp33[0][1]; F[6][8] = -temp33[0][2];
F[7][6] = -temp33[1][0]; F[7][7] = -temp33[1][1]; F[7][8] = -temp33[1][2];
F[8][6] = -temp33[2][0]; F[8][7] = -temp33[2][1]; F[8][8] = -temp33[2][2];
// ... att2gyr
F[6][12] = -0.5;
F[7][13] = -0.5;
F[8][14] = -0.5;
// ... Accel Markov Bias
F[9][9] = -1.0/TAU_A; F[10][10] = -1.0/TAU_A; F[11][11] = -1.0/TAU_A;
F[12][12] = -1.0/TAU_G; F[13][13] = -1.0/TAU_G; F[14][14] = -1.0/TAU_G;
//fprintf(stderr,"Jacobian Created\n");
// State Transition Matrix: PHI = I15 + F*dt;
temp1515 = mat_scalMul(F,imu_dt,temp1515);
PHI = mat_add(I15,temp1515,PHI);
// Process Noise
G = mat_fill(G, ZERO_MATRIX);
G[3][0] = -C_B2N[0][0]; G[3][1] = -C_B2N[0][1]; G[3][2] = -C_B2N[0][2];
G[4][0] = -C_B2N[1][0]; G[4][1] = -C_B2N[1][1]; G[4][2] = -C_B2N[1][2];
G[5][0] = -C_B2N[2][0]; G[5][1] = -C_B2N[2][1]; G[5][2] = -C_B2N[2][2];
G[6][3] = -0.5;
G[7][4] = -0.5;
G[8][5] = -0.5;
G[9][6] = 1.0; G[10][7] = 1.0; G[11][8] = 1.0;
G[12][9] = 1.0; G[13][10] = 1.0; G[14][11] = 1.0;
//fprintf(stderr,"Process Noise Matrix G is created\n");
// Discrete Process Noise
temp1512 = mat_mul(G,Rw,temp1512);
temp1515 = mat_transmul(temp1512,G,temp1515); // Qw = G*Rw*G'
Qw = mat_scalMul(temp1515,imu_dt,Qw); // Qw = dt*G*Rw*G'
Q = mat_mul(PHI,Qw,Q); // Q = (I+F*dt)*Qw
temp1515 = mat_tran(Q,temp1515);
Q = mat_add(Q,temp1515,Q);
Q = mat_scalMul(Q,0.5,Q); // Q = 0.5*(Q+Q')
//fprintf(stderr,"Discrete Process Noise is created\n");
// Covariance Time Update
temp1515 = mat_mul(PHI,P,temp1515);
P = mat_transmul(temp1515,PHI,P); // P = PHI*P*PHI'
P = mat_add(P,Q,P); // P = PHI*P*PHI' + Q
temp1515 = mat_tran(P, temp1515);
P = mat_add(P,temp1515,P);
P = mat_scalMul(P,0.5,P); // P = 0.5*(P+P')
//fprintf(stderr,"Covariance Updated through Time Update\n");
navData_ptr->Pp[0] = P[0][0]; navData_ptr->Pp[1] = P[1][1]; navData_ptr->Pp[2] = P[2][2];
navData_ptr->Pv[0] = P[3][3]; navData_ptr->Pv[1] = P[4][4]; navData_ptr->Pv[2] = P[5][5];
navData_ptr->Pa[0] = P[6][6]; navData_ptr->Pa[1] = P[7][7]; navData_ptr->Pa[2] = P[8][8];
navData_ptr->Pab[0] = P[9][9]; navData_ptr->Pab[1] = P[10][10]; navData_ptr->Pab[2] = P[11][11];
navData_ptr->Pgb[0] = P[12][12]; navData_ptr->Pgb[1] = P[13][13]; navData_ptr->Pgb[2] = P[14][14];
navData_ptr->err_type = TU_only;
//fprintf(stderr,"Time Update Done\n");
// ================== DONE TU ===================
if(sensorData_ptr->gpsData_ptr->newData){
// ================== GPS Update ===================
sensorData_ptr->gpsData_ptr->newData = 0; // Reset the flag
// Position, converted to NED
a_temp31[0][0] = navData_ptr->lat;
a_temp31[1][0] = navData_ptr->lon; a_temp31[2][0] = navData_ptr->alt;
pos_ins_ecef = lla2ecef(a_temp31,pos_ins_ecef);
a_temp31[2][0] = 0.0;
//pos_ref = lla2ecef(a_temp31,pos_ref);
pos_ref = mat_copy(a_temp31,pos_ref);
pos_ins_ned = ecef2ned(pos_ins_ecef,pos_ins_ned,pos_ref);
pos_gps[0][0] = sensorData_ptr->gpsData_ptr->lat*D2R;
pos_gps[1][0] = sensorData_ptr->gpsData_ptr->lon*D2R;
pos_gps[2][0] = sensorData_ptr->gpsData_ptr->alt;
pos_gps_ecef = lla2ecef(pos_gps,pos_gps_ecef);
pos_gps_ned = ecef2ned(pos_gps_ecef,pos_gps_ned,pos_ref);
// Create Measurement: y
y[0][0] = pos_gps_ned[0][0] - pos_ins_ned[0][0];
y[1][0] = pos_gps_ned[1][0] - pos_ins_ned[1][0];
y[2][0] = pos_gps_ned[2][0] - pos_ins_ned[2][0];
y[3][0] = sensorData_ptr->gpsData_ptr->vn - navData_ptr->vn;
y[4][0] = sensorData_ptr->gpsData_ptr->ve - navData_ptr->ve;
y[5][0] = sensorData_ptr->gpsData_ptr->vd - navData_ptr->vd;
//fprintf(stderr,"Measurement Matrix, y, created\n");
// Kalman Gain
temp615 = mat_mul(H,P,temp615);
temp66 = mat_transmul(temp615,H,temp66);
atemp66 = mat_add(temp66,R,atemp66);
temp66 = mat_inv(atemp66,temp66); // temp66 = inv(H*P*H'+R)
//fprintf(stderr,"inv(H*P*H'+R) Computed\n");
temp156 = mat_transmul(P,H,temp156); // P*H'
//fprintf(stderr,"P*H' Computed\n");
K = mat_mul(temp156,temp66,K); // K = P*H'*inv(H*P*H'+R)
//fprintf(stderr,"Kalman Gain Computed\n");
// Covariance Update
temp1515 = mat_mul(K,H,temp1515);
ImKH = mat_sub(I15,temp1515,ImKH); // ImKH = I - K*H
temp615 = mat_transmul(R,K,temp615);
KRKt = mat_mul(K,temp615,KRKt); // KRKt = K*R*K'
temp1515 = mat_transmul(P,ImKH,temp1515);
P = mat_mul(ImKH,temp1515,P); // ImKH*P*ImKH'
temp1515 = mat_add(P,KRKt,temp1515);
P = mat_copy(temp1515,P); // P = ImKH*P*ImKH' + KRKt
//fprintf(stderr,"Covariance Updated through GPS Update\n");
navData_ptr->Pp[0] = P[0][0]; navData_ptr->Pp[1] = P[1][1]; navData_ptr->Pp[2] = P[2][2];
navData_ptr->Pv[0] = P[3][3]; navData_ptr->Pv[1] = P[4][4]; navData_ptr->Pv[2] = P[5][5];
navData_ptr->Pa[0] = P[6][6]; navData_ptr->Pa[1] = P[7][7]; navData_ptr->Pa[2] = P[8][8];
navData_ptr->Pab[0] = P[9][9]; navData_ptr->Pab[1] = P[10][10]; navData_ptr->Pab[2] = P[11][11];
navData_ptr->Pgb[0] = P[12][12]; navData_ptr->Pgb[1] = P[13][13]; navData_ptr->Pgb[2] = P[14][14];
// State Update
x = mat_mul(K,y,x);
denom = (1.0 - (ECC2 * sin(navData_ptr->lat) * sin(navData_ptr->lat)));
denom = sqrt(denom*denom);
Re = EARTH_RADIUS / sqrt(denom);
Rn = EARTH_RADIUS*(1-ECC2) / denom*sqrt(denom);
navData_ptr->alt = navData_ptr->alt - x[2][0];
navData_ptr->lat = navData_ptr->lat + x[0][0]/(Re + navData_ptr->alt);
navData_ptr->lon = navData_ptr->lon + x[1][0]/(Rn + navData_ptr->alt)/cos(navData_ptr->lat);
navData_ptr->vn = navData_ptr->vn + x[3][0];
navData_ptr->ve = navData_ptr->ve + x[4][0];
navData_ptr->vd = navData_ptr->vd + x[5][0];
quat[0] = navData_ptr->quat[0];
quat[1] = navData_ptr->quat[1];
quat[2] = navData_ptr->quat[2];
quat[3] = navData_ptr->quat[3];
// Attitude correction
dq[0] = 1.0;
dq[1] = x[6][0];
dq[2] = x[7][0];
dq[3] = x[8][0];
qmult(quat,dq,quat_new);
quat[0] = quat_new[0]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
quat[1] = quat_new[1]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
quat[2] = quat_new[2]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
quat[3] = quat_new[3]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
navData_ptr->quat[0] = quat[0];
navData_ptr->quat[1] = quat[1];
navData_ptr->quat[2] = quat[2];
navData_ptr->quat[3] = quat[3];
quat2eul(navData_ptr->quat,&(navData_ptr->phi),&(navData_ptr->the),&(navData_ptr->psi));
navData_ptr->ab[0] = navData_ptr->ab[0] + x[9][0];
navData_ptr->ab[1] = navData_ptr->ab[1] + x[10][0];
navData_ptr->ab[2] = navData_ptr->ab[2] + x[11][0];
navData_ptr->gb[0] = navData_ptr->gb[0] + x[12][0];
navData_ptr->gb[1] = navData_ptr->gb[1] + x[13][0];
navData_ptr->gb[2] = navData_ptr->gb[2] + x[14][0];
navData_ptr->err_type = gps_aided;
//fprintf(stderr,"Measurement Update Done\n");
}
// Remove current estimated biases from rate gyro and accels
sensorData_ptr->imuData_ptr->p -= navData_ptr->gb[0];
sensorData_ptr->imuData_ptr->q -= navData_ptr->gb[1];
sensorData_ptr->imuData_ptr->r -= navData_ptr->gb[2];
sensorData_ptr->imuData_ptr->ax -= navData_ptr->ab[0];
sensorData_ptr->imuData_ptr->ay -= navData_ptr->ab[1];
sensorData_ptr->imuData_ptr->az -= navData_ptr->ab[2];
// Get the new Specific forces and Rotation Rate,
// use in the next time update
f_b[0][0] = sensorData_ptr->imuData_ptr->ax;
f_b[1][0] = sensorData_ptr->imuData_ptr->ay;
f_b[2][0] = sensorData_ptr->imuData_ptr->az;
om_ib[0][0] = sensorData_ptr->imuData_ptr->p;
om_ib[1][0] = sensorData_ptr->imuData_ptr->q;
om_ib[2][0] = sensorData_ptr->imuData_ptr->r;
}
/*
*-----------------------------------------------------------------------------
* funct: mat_inv
* desct: find inverse of a matrix
* given: a = square matrix a, and C, return for inv(a)
* retrn: square matrix Inverse(A), C
* NULL = fails, singular matrix
*-----------------------------------------------------------------------------
*/
MATRIX mat_inv( MATRIX a , MATRIX C)
{
MATRIX A, B, P;
int i, n;
#ifdef CONFORM_CHECK
if(MatCol(a)!=MatCol(C))
{
error("\nUnconformable matrices in routine mat_inv(): Col(A)!=Col(B)\n");
}
if(MatRow(a)!=MatRow(C))
{
error("\nUnconformable matrices in routine mat_inv(): Row(A)!=Row(B)\n");
}
#endif
n = MatCol(a);
A = mat_creat(MatRow(a), MatCol(a), UNDEFINED);
A = mat_copy(a, A);
B = mat_creat( n, 1, UNDEFINED );
P = mat_creat( n, 1, UNDEFINED );
/*
* - LU-decomposition -
* also check for singular matrix
*/
if (mat_lu(A, P) == -1)
{
mat_free(A);
mat_free(B);
mat_free(P);
return (NULL);
}
else
{
/* Bug??? was still mat_backsubs1 even when singular??? */
for (i=0; i<n; i++)
{
mat_fill(B, ZERO_MATRIX);
B[i][0] = 1.0;
mat_backsubs1( A, B, C, P, i );
}
mat_free(A);
mat_free(B);
mat_free(P);
return (C);
}
}
int create_crystal_structure(std::vector<cs::catom_t> & catom_array){
//----------------------------------------------------------
// check calling of routine if error checking is activated
//----------------------------------------------------------
if(err::check==true){std::cout << "cs::create_crystal_structure has been called" << std::endl;}
int min_bounds[3];
int max_bounds[3];
#ifdef MPICF
if(vmpi::mpi_mode==0){
min_bounds[0] = int(vmpi::min_dimensions[0]/unit_cell.dimensions[0]);
min_bounds[1] = int(vmpi::min_dimensions[1]/unit_cell.dimensions[1]);
min_bounds[2] = int(vmpi::min_dimensions[2]/unit_cell.dimensions[2]);
max_bounds[0] = vmath::iceil(vmpi::max_dimensions[0]/unit_cell.dimensions[0]);
max_bounds[1] = vmath::iceil(vmpi::max_dimensions[1]/unit_cell.dimensions[1]);
max_bounds[2] = vmath::iceil(vmpi::max_dimensions[2]/unit_cell.dimensions[2]);
}
else{
min_bounds[0]=0;
min_bounds[1]=0;
min_bounds[2]=0;
max_bounds[0]=cs::total_num_unit_cells[0];
max_bounds[1]=cs::total_num_unit_cells[1];
max_bounds[2]=cs::total_num_unit_cells[2];
}
#else
min_bounds[0]=0;
min_bounds[1]=0;
min_bounds[2]=0;
max_bounds[0]=cs::total_num_unit_cells[0];
max_bounds[1]=cs::total_num_unit_cells[1];
max_bounds[2]=cs::total_num_unit_cells[2];
#endif
cs::local_num_unit_cells[0]=max_bounds[0]-min_bounds[0];
cs::local_num_unit_cells[1]=max_bounds[1]-min_bounds[1];
cs::local_num_unit_cells[2]=max_bounds[2]-min_bounds[2];
int num_atoms=cs::local_num_unit_cells[0]*cs::local_num_unit_cells[1]*cs::local_num_unit_cells[2]*unit_cell.atom.size();
// set catom_array size
catom_array.reserve(num_atoms);
// Initialise atoms number
int atom=0;
// find maximum height lh_category
unsigned int maxlh=0;
for(unsigned int uca=0;uca<unit_cell.atom.size();uca++) if(unit_cell.atom[uca].hc > maxlh) maxlh = unit_cell.atom[uca].hc;
maxlh+=1;
const double cff = 1.e-9; // Small numerical correction for atoms exactly on the borderline between processors
// Duplicate unit cell
for(int z=min_bounds[2];z<max_bounds[2];z++){
for(int y=min_bounds[1];y<max_bounds[1];y++){
for(int x=min_bounds[0];x<max_bounds[0];x++){
// need to change this to accept non-orthogonal lattices
// Loop over atoms in unit cell
for(unsigned int uca=0;uca<unit_cell.atom.size();uca++){
double cx = (double(x)+unit_cell.atom[uca].x)*unit_cell.dimensions[0]+cff;
double cy = (double(y)+unit_cell.atom[uca].y)*unit_cell.dimensions[1]+cff;
double cz = (double(z)+unit_cell.atom[uca].z)*unit_cell.dimensions[2]+cff;
#ifdef MPICF
if(vmpi::mpi_mode==0){
// only generate atoms within allowed dimensions
if( (cx>=vmpi::min_dimensions[0] && cx<vmpi::max_dimensions[0]) &&
(cy>=vmpi::min_dimensions[1] && cy<vmpi::max_dimensions[1]) &&
(cz>=vmpi::min_dimensions[2] && cz<vmpi::max_dimensions[2])){
#endif
if((cx<cs::system_dimensions[0]) && (cy<cs::system_dimensions[1]) && (cz<cs::system_dimensions[2])){
catom_array.push_back(cs::catom_t());
catom_array[atom].x=cx;
catom_array[atom].y=cy;
catom_array[atom].z=cz;
//std::cout << atom << "\t" << cx << "\t" << cy <<"\t" << cz << std::endl;
catom_array[atom].material=unit_cell.atom[uca].mat;
catom_array[atom].uc_id=uca;
catom_array[atom].lh_category=unit_cell.atom[uca].hc+z*maxlh;
catom_array[atom].uc_category=unit_cell.atom[uca].lc;
catom_array[atom].scx=x;
catom_array[atom].scy=y;
catom_array[atom].scz=z;
atom++;
}
#ifdef MPICF
}
}
else{
if((cx<cs::system_dimensions[0]) && (cy<cs::system_dimensions[1]) && (cz<cs::system_dimensions[2])){
catom_array.push_back(cs::catom_t());
catom_array[atom].x=cx;
catom_array[atom].y=cy;
catom_array[atom].z=cz;
catom_array[atom].material=unit_cell.atom[uca].mat;
catom_array[atom].uc_id=uca;
catom_array[atom].lh_category=unit_cell.atom[uca].hc+z*maxlh;
catom_array[atom].uc_category=unit_cell.atom[uca].lc;
catom_array[atom].scx=x;
catom_array[atom].scy=y;
catom_array[atom].scz=z;
catom_array[atom].include=false; // assume no atoms until classification complete
atom++;
}
}
#endif
}
}
}
}
// Check to see if actual and expected number of atoms agree, if not trim the excess
if(atom!=num_atoms){
std::vector<cs::catom_t> tmp_catom_array(num_atoms);
tmp_catom_array=catom_array;
catom_array.resize(atom);
for(int a=0;a<atom;a++){
catom_array[a]=tmp_catom_array[a];
}
tmp_catom_array.resize(0);
}
// If z-height material selection is enabled then do so
if(cs::SelectMaterialByZHeight==true){
// Check for interfacial roughness and call custom material assignment routine
if(cs::interfacial_roughness==true) cs::roughness(catom_array);
// Check for multilayer system and if required generate multilayers
else if(cs::multilayers) cs::generate_multilayers(catom_array);
// Otherwise perform normal assignement of materials
else{
// determine z-bounds for materials
std::vector<double> mat_min(mp::num_materials);
std::vector<double> mat_max(mp::num_materials);
std::vector<bool> mat_fill(mp::num_materials);
// Unroll min, max and fill for performance
for(int mat=0;mat<mp::num_materials;mat++){
mat_min[mat]=mp::material[mat].min*cs::system_dimensions[2];
mat_max[mat]=mp::material[mat].max*cs::system_dimensions[2];
// alloys generally are not defined by height, and so have max = 0.0
if(mat_max[mat]<0.0000001) mat_max[mat]=-0.1;
mat_fill[mat]=mp::material[mat].fill;
}
// Assign materials to generated atoms
for(unsigned int atom=0;atom<catom_array.size();atom++){
for(int mat=0;mat<mp::num_materials;mat++){
const double cz=catom_array[atom].z;
if((cz>=mat_min[mat]) && (cz<mat_max[mat]) && (mat_fill[mat]==false)){
catom_array[atom].material=mat;
catom_array[atom].include=true;
}
}
}
}
// Delete unneeded atoms
clear_atoms(catom_array);
}
// Check to see if any atoms have been generated
if(atom==0){
terminaltextcolor(RED);
std::cout << "Error - no atoms have been generated, increase system dimensions!" << std::endl;
terminaltextcolor(WHITE);
zlog << zTs() << "Error: No atoms have been generated. Increase system dimensions." << std::endl;
err::vexit();
}
// Now unselect all atoms by default for particle shape cutting
for(unsigned int atom=0;atom<catom_array.size();atom++){
catom_array[atom].include=false;
}
return EXIT_SUCCESS;
}
