void
applyTransforms(struct trans *tlist, struct GENode *glist) {
struct trans *ptl = tlist;
struct GENode *pgl = NULL;
struct transMatrix m, aux;
struct homoCoord start;
struct homoCoord end;
while(NULL != ptl) {
pgl = glist;
while(NULL != pgl) {
switch(pgl->el.type)
{
case LINE:
initHomoVector(&start, pgl->el.data.line.st.x, pgl->el.data.line.st.y);
initHomoVector(&end, pgl->el.data.line.en.x, pgl->el.data.line.en.y);
break;
default:
break;
}
switch(ptl->tType) {
case TRANSLATION:
initTranslation(&m, ptl->data.t.tx, ptl->data.t.ty);
break;
case SCALING:
initScale(&m, ptl->data.s.sx, ptl->data.s.sy);
if (!(ptl->data.s.px == 0 && ptl->data.s.py == 0))
{
initTranslation(&aux, -1*ptl->data.s.px, -1*ptl->data.s.py);
matrixProduct(&m, m, aux);
initTranslation(&aux, ptl->data.s.px, ptl->data.s.py);
matrixProduct(&m, m, aux);
}
break;
case ROTATION:
initRotation(&m, ptl->data.r.u);
if (!(ptl->data.s.px == 0 && ptl->data.s.py == 0))
{
initTranslation(&aux, -1*ptl->data.r.px, -1*ptl->data.r.py);
matrixProduct(&m, m, aux);
initTranslation(&aux, ptl->data.s.px, ptl->data.s.py);
matrixProduct(&m, m, aux);
}
break;
default:
break;
}
matrixVectorProduct(&start, m, start);
matrixVectorProduct(&end, m, end);
twoDCoord(&pgl->el.data.line.st, start);
twoDCoord(&pgl->el.data.line.en, end);
pgl = pgl->next;
}
ptl = ptl->next;
}
}
void KinematicMotion::addRotation(const double *xAxisNew, const double *yAxisNew,
const double *zAxisNew, bool normalized) {
double e0[3];
double e1[3];
double e2[3];
copyVector(xAxisNew, e0);
copyVector(yAxisNew, e1);
copyVector(zAxisNew, e2);
if (!normalized) {
normalizeVector(e0);
normalizeVector(e1);
normalizeVector(e2);
}
double newRotation[9];
for (int i = 0; i < 3; i++) {
newRotation[i * 3 + 0] = e0[i];
newRotation[i * 3 + 1] = e1[i];
newRotation[i * 3 + 2] = e2[i];
}
//R'*(R*x + t) = R'*R*x + R'*t
matrixProduct(newRotation, rotationMatrix);
matrixVectorProduct(newRotation, translationVector);
}
void KinematicMotion::addRotation(const double *vec0, const double *vec1, bool normalized) { // not unique, but shortest path
// rotation is defined in the CURRENT coordinates system
double e0[3];
copyVector(vec0, e0);
double e1[3];
copyVector(vec1, e1);
if (!normalized) {
normalizeVector(e0);
normalizeVector(e1);
}
// algorithm form Non-linear Modeling and Analysis of Solids and Structures (Steen Krenk) P54
double e2[3];
e2[0] = e0[0] + e1[0];
e2[1] = e0[1] + e1[1];
e2[2] = e0[2] + e1[2];
double e2_square = vectorProduct(e2, e2);
double newRotation[9];
newRotation[0] = 1.0 + 2.0 * e1[0] * e0[0] - 2.0 / e2_square * e2[0] * e2[0];
newRotation[1] = 0.0 + 2.0 * e1[0] * e0[1] - 2.0 / e2_square * e2[0] * e2[1];
newRotation[2] = 0.0 + 2.0 * e1[0] * e0[2] - 2.0 / e2_square * e2[0] * e2[2];
newRotation[3] = 0.0 + 2.0 * e1[1] * e0[0] - 2.0 / e2_square * e2[1] * e2[0];
newRotation[4] = 1.0 + 2.0 * e1[1] * e0[1] - 2.0 / e2_square * e2[1] * e2[1];
newRotation[5] = 0.0 + 2.0 * e1[1] * e0[2] - 2.0 / e2_square * e2[1] * e2[2];
newRotation[6] = 0.0 + 2.0 * e1[2] * e0[0] - 2.0 / e2_square * e2[2] * e2[0];
newRotation[7] = 0.0 + 2.0 * e1[2] * e0[1] - 2.0 / e2_square * e2[2] * e2[1];
newRotation[8] = 1.0 + 2.0 * e1[2] * e0[2] - 2.0 / e2_square * e2[2] * e2[2];
//R'*(R*x + t) = R'*R*x + R'*t
matrixProduct(newRotation, rotationMatrix);
matrixVectorProduct(newRotation, translationVector);
}
void KinematicMotion::addRotation(const double *axis, bool normalized, double angle) {
// rotation is defined in the CURRENT coordinates system
double u[3];
copyVector(axis, u);
if (!normalized) {
normalizeVector(u);
}
// algorithm from http://en.wikipedia.org/wiki/Rotation_matrix
double c = cos(angle);
double s = sin(angle);
double newRotation[9];
newRotation[0] = c + u[0] * u[0] * (1.0 - c);
newRotation[1] = u[0] * u[1] * (1.0 - c) - u[2] * s;
newRotation[2] = u[0] * u[2] * (1.0 - c) + u[1] * s;
newRotation[3] = u[1] * u[0] * (1.0 - c) + u[2] * s;
newRotation[4] = c + u[1] * u[1] * (1.0 - c);
newRotation[5] = u[1] * u[2] * (1.0 - c) - u[0] * s;
newRotation[6] = u[2] * u[0] * (1.0 - c) - u[1] * s;
newRotation[7] = u[2] * u[1] * (1.0 - c) + u[0] * s;
newRotation[8] = c + u[2] * u[2] * (1.0 - c);
//R'*(R*x + t) = R'*R*x + R'*t
matrixProduct(newRotation, rotationMatrix);
matrixVectorProduct(newRotation, translationVector);
}
PyObject *scriptVertexRotatePointBack(PyObject *self, PyObject *args) {
PyObject *pyCoords;
GLdouble coords[figureData.dim], coords2[figureData.dim];
if (!PyArg_ParseTuple(args, "O", &pyCoords))
return NULL;
if (!coordsFromPython(pyCoords, coords))
return NULL;
matrixProduct(coords, figureRotMatrix, coords2, 1, figureData.dim, figureData.dim);
return coordsToPython(coords2);
}
int main(){
double a[N][N];
double x[N];
double b[N];
int n,i,c,j,k;
for(n=0; n<N; n++){
for(i=0; i<N; i++){
a[n][i] = (n+1);
}
}
for(c=0; c<N; c++){
x[c] = 1.0;
}
matrixProduct(a, x, N);
return 0;
}
void KinematicMotion::checkRotationCorrectness() const {
double M[9];
double M_inv[9];
copyMatrix(rotationMatrix, M);
copyMatrix(rotationMatrix, M_inv);
matrixTanspose(M_inv);
matrixProduct(M_inv, M); // M = M_inv * M
const double TOL = 1E-10;
//printMatrix(M);
assert(fabs(M[0 * 3 + 0] - 1.0) < TOL);
assert(fabs(M[0 * 3 + 1] - 0.0) < TOL);
assert(fabs(M[0 * 3 + 2] - 0.0) < TOL);
assert(fabs(M[1 * 3 + 0] - 0.0) < TOL);
assert(fabs(M[1 * 3 + 1] - 1.0) < TOL);
assert(fabs(M[1 * 3 + 2] - 0.0) < TOL);
assert(fabs(M[2 * 3 + 0] - 0.0) < TOL);
assert(fabs(M[2 * 3 + 1] - 0.0) < TOL);
assert(fabs(M[2 * 3 + 2] - 1.0) < TOL);
}
void CovCond(double condNumber, const V & direction, M & covMatrix,
M & precMatrix)
{
//std::cout << "Entering CovCond()"
// << std::endl;
V v1(direction);
//std::cout << "In CovCond(), v1 contents are:"
// << std::endl
// << v1
// << std::endl;
V v2(direction,condNumber,1.0); // MATLAB linspace
v2.cwInvert();
v2.sort();
//std::cout << "In CovCond(), v2 contents are:"
// << std::endl
// << v2
// << std::endl;
double v1Norm2 = v1.norm2();
if (v1[0] >=0) v1[0] += v1Norm2;
else v1[0] -= v1Norm2;
double v1Norm2Sq = v1.norm2Sq();
M Z(direction,1.0);
Z -= (2./v1Norm2Sq) * matrixProduct(v1,v1);
//std::cout << "In CovCond(), Z contents are:"
// << std::endl
// << Z
// << std::endl;
M Zt(Z.transpose());
covMatrix = Z * leftDiagScaling(v2, Zt);
precMatrix = Z * leftDiagScaling(1./v2,Zt);
//std::cout << "Leaving CovCond()"
// << std::endl;
}
void solveSip(const uqFullEnvironmentClass& env)
{
if ((env.subDisplayFile()) && (env.displayVerbosity() >= 2)) {
*env.subDisplayFile() << "Entering solveSip()..."
<< std::endl;
}
////////////////////////////////////////////////////////
// Step 1 of 5: Instantiate the parameter space
////////////////////////////////////////////////////////
unsigned int p = 2;
uqVectorSpaceClass<uqGslVectorClass,uqGslMatrixClass> paramSpace(env, "param_", p, NULL);
uqGslVectorClass aVec(paramSpace.zeroVector());
aVec[0] = 2.;
aVec[1] = 5.;
uqGslVectorClass xGiven(paramSpace.zeroVector());
xGiven[0] = -1.;
xGiven[1] = 7.;
////////////////////////////////////////////////////////
// Step 2 of 5: Instantiate the parameter domain
////////////////////////////////////////////////////////
//uqGslVectorClass paramMins (paramSpace.zeroVector());
//uqGslVectorClass paramMaxs (paramSpace.zeroVector());
//paramMins [0] = -1.e+16;
//paramMaxs [0] = 1.e+16;
//paramMins [1] = -1.e+16;
//paramMaxs [1] = 1.e+16;
//uqBoxSubsetClass<uqGslVectorClass,uqGslMatrixClass> paramDomain("param_",paramSpace,paramMins,paramMaxs);
uqVectorSetClass<uqGslVectorClass,uqGslMatrixClass>* paramDomain = ¶mSpace;
////////////////////////////////////////////////////////
// Step 3 of 5: Instantiate the likelihood function object
////////////////////////////////////////////////////////
unsigned int n = 5;
uqVectorSpaceClass<uqGslVectorClass,uqGslMatrixClass> dataSpace(env, "data_", n, NULL);
uqGslVectorClass yMeanVec(dataSpace.zeroVector());
double tmp = scalarProduct(aVec,xGiven);
for (unsigned int i = 0; i < n; ++i) {
yMeanVec[i] = tmp;
}
double sigmaEps = 2.1;
uqGslMatrixClass yCovMat(dataSpace.zeroVector());
tmp = sigmaEps*sigmaEps;
for (unsigned int i = 0; i < n; ++i) {
yCovMat(i,i) = tmp;
}
uqGslVectorClass ySamples(dataSpace.zeroVector());
uqGaussianVectorRVClass<uqGslVectorClass,uqGslMatrixClass> yRv("y_", dataSpace, yMeanVec, yCovMat);
yRv.realizer().realization(ySamples);
double ySampleMean = 0.;
for (unsigned int i = 0; i < n; ++i) {
ySampleMean += ySamples[i];
}
ySampleMean /= ((double) n);
struct likelihoodDataStruct likelihoodData;
likelihoodData.aVec = &aVec;
likelihoodData.sigmaEps = sigmaEps;
likelihoodData.ySamples = &ySamples;
uqGenericScalarFunctionClass<uqGslVectorClass,uqGslMatrixClass>
likelihoodFunctionObj("like_",
*paramDomain,
likelihoodRoutine,
(void *) &likelihoodData,
true); // routine computes [ln(function)]
////////////////////////////////////////////////////////
// Step 4 of 5: Instantiate the inverse problem
////////////////////////////////////////////////////////
uqGslVectorClass xPriorMeanVec(paramSpace.zeroVector());
xPriorMeanVec[0] = 0.;
xPriorMeanVec[1] = 0.;
uqGslMatrixClass sigma0Mat(paramSpace.zeroVector());
sigma0Mat(0,0) = 1.e-3;
sigma0Mat(0,1) = 0.;
sigma0Mat(1,0) = 0.;
sigma0Mat(1,1) = 1.e-3;
uqGslMatrixClass sigma0MatInverse(paramSpace.zeroVector());
sigma0MatInverse = sigma0Mat.inverse();
uqGaussianVectorRVClass<uqGslVectorClass,uqGslMatrixClass> priorRv("prior_", *paramDomain, xPriorMeanVec, sigma0MatInverse);
uqGenericVectorRVClass <uqGslVectorClass,uqGslMatrixClass> postRv ("post_", paramSpace);
uqStatisticalInverseProblemClass<uqGslVectorClass,uqGslMatrixClass> sip("sip_", NULL, priorRv, likelihoodFunctionObj, postRv);
if ((env.subDisplayFile()) && (env.displayVerbosity() >= 2)) {
*env.subDisplayFile() << "In solveSip():"
<< "\n p = " << p
<< "\n xGiven = " << xGiven
<< "\n sigma0Mat = " << sigma0Mat
<< "\n sigma0MatInverse = " << sigma0MatInverse
<< "\n aVec = " << aVec
<< "\n n = " << n
<< "\n sigmaEps = " << sigmaEps
<< "\n yMeanVec = " << yMeanVec
<< "\n yCovMat = " << yCovMat
<< "\n ySamples = " << ySamples
<< "\n ySampleMean = " << ySampleMean
<< std::endl;
}
uqGslMatrixClass sigmaMatInverse(paramSpace.zeroVector());
sigmaMatInverse = matrixProduct(aVec,aVec);
sigmaMatInverse *= (((double) n)/sigmaEps/sigmaEps);
sigmaMatInverse += sigma0Mat;
uqGslMatrixClass sigmaMat(paramSpace.zeroVector());
sigmaMat = sigmaMatInverse.inverse();
uqGslVectorClass muVec(paramSpace.zeroVector());
muVec = sigmaMat * aVec;
muVec *= (((double) n) * ySampleMean)/sigmaEps/sigmaEps;
if ((env.subDisplayFile()) && (env.displayVerbosity() >= 2)) {
*env.subDisplayFile() << "In solveSip():"
<< "\n muVec = " << muVec
<< "\n sigmaMat = " << sigmaMat
<< "\n sigmaMatInverse = " << sigmaMatInverse
<< std::endl;
}
////////////////////////////////////////////////////////
// Step 5 of 5: Solve the inverse problem
////////////////////////////////////////////////////////
uqGslVectorClass initialValues(paramSpace.zeroVector());
initialValues[0] = 25.;
initialValues[1] = 25.;
uqGslMatrixClass proposalCovMat(paramSpace.zeroVector());
proposalCovMat(0,0) = 10.;
proposalCovMat(0,1) = 0.;
proposalCovMat(1,0) = 0.;
proposalCovMat(1,1) = 10.;
sip.solveWithBayesMetropolisHastings(NULL,initialValues,&proposalCovMat);
if ((env.subDisplayFile()) && (env.displayVerbosity() >= 2)) {
*env.subDisplayFile() << "Leaving solveSip()"
<< std::endl;
}
return;
}
/********************************************************************
* This function adds only the crystalline atoms to the superCell
* the amorphous ones are handled by makeAmorphous, and makeSpecial
********************************************************************/
void makeSuperCell() {
int g,p,iatom,ix,iy,iz,atomCount = 0;
// atom *atomPtr;
// atomPtr = (atom *)malloc(sizeof(atom));
static double *axCell,*byCell,*czCell=NULL;
static double **Mm = NULL, **Mminv = NULL, **Mrot = NULL,**Mr=NULL,**Mr2=NULL;
static double **a = NULL,**b= NULL;
double maxLength,dx,dy,dz,d,dxs,dys,dzs;
atom newAtom;
// double xpos,ypos,zpos;
int nxmin,nxmax,nymin,nymax,nzmin,nzmax;
/* claculate maximum length in supercell box, which is naturally
* the room diagonal:
*/
maxLength = vectLength(&(superCell.ax));
/* maxLength = sqrt(superCell.ax*superCell.ax+
superCell.by*superCell.by+
superCell.cz*superCell.cz);
*/
if (Mm == NULL) {
Mm = double2D(3,3,"Mm");
Mminv = double2D(3,3,"Mminv");
Mrot = double2D(3,3,"Mrot");
Mr = double2D(3,3,"Mr");
Mr2 = double2D(3,3,"Mr");
axCell=Mm[0]; byCell=Mm[1]; czCell=Mm[2];
a = double2D(1,3,"a");
b = double2D(1,3,"b");
}
atomCount = superCell.natoms;
for (g=0;g<nGrains;g++) {
/********************************************************
* if this grain is a crystalline one ...
*/
if (grains[g].amorphFlag == 0) {
dx = grains[g].shiftx/superCell.ax;
dy = grains[g].shifty/superCell.by;
dz = grains[g].shiftz/superCell.cz;
/* find the rotated unit cell vectors .. */
makeCellVect(grains+g, axCell, byCell, czCell);
// showMatrix(Mm,3,3,"M");
///////////////////////////////////////////////////////////////////
memset(Mrot[0],0,3*3*sizeof(double));
Mrot[0][0] = 1.0; Mrot[1][1] = 1.0; Mrot[2][2] = 1.0;
memcpy(Mr2[0],Mrot[0],3*3*sizeof(double));
memset(Mr[0],0,3*3*sizeof(double));
Mr[0][0] = 1.0; Mr[1][1] = cos(grains[g].tiltx); Mr[1][2] = sin(grains[g].tiltx);
Mr[2][1] = -sin(grains[g].tiltx); Mr[2][2] = cos(grains[g].tiltx);
matrixProduct(Mrot,3,3,Mr,3,3,Mr2);
memcpy(Mrot[0],Mr2[0],3*3*sizeof(double));
// showMatrix(Mrot,3,3,"Mrotx");
memset(Mr[0],0,3*3*sizeof(double));
Mr[1][1] = 1.0; Mr[0][0] = cos(grains[g].tilty); Mr[0][2] = -sin(grains[g].tilty);
Mr[2][0] = sin(grains[g].tilty); Mr[2][2] = cos(grains[g].tilty);
matrixProduct(Mrot,3,3,Mr,3,3,Mr2);
memcpy(Mrot[0],Mr2[0],3*3*sizeof(double));
// showMatrix(Mrot,3,3,"Mrotxy");
memset(Mr[0],0,3*3*sizeof(double));
Mr[2][2] = 1.0; Mr[0][0] = cos(grains[g].tiltz); Mr[0][1] = sin(grains[g].tiltz);
Mr[1][0] = -sin(grains[g].tiltz); Mr[1][1] = cos(grains[g].tiltz);
matrixProduct(Mrot,3,3,Mr,3,3,Mr2);
memcpy(Mrot[0],Mr2[0],3*3*sizeof(double));
// showMatrix(Mrot,3,3,"Mrotxyz");
///////////////////////////////////////////////////////////////////
/*
rotateVect(axCell,axCell,grains[g].tiltx,grains[g].tilty,grains[g].tiltz);
rotateVect(byCell,byCell,grains[g].tiltx,grains[g].tilty,grains[g].tiltz);
rotateVect(czCell,czCell,grains[g].tiltx,grains[g].tilty,grains[g].tiltz);
*/
inverse_3x3(Mminv[0],Mm[0]);
matrixProduct(Mm,3,3,Mrot,3,3,Mr2);
memcpy(Mm[0],Mr2[0],3*3*sizeof(double));
// showMatrix(Mm,3,3,"M");
inverse_3x3(Mr2[0],Mm[0]);
/* find out how far we will have to go in units of unit cell vectors.
* when creating the supercell by checking the number of unit cell vectors
* necessary to reach every corner of the supercell box.
*/
memset(a[0],0,3*sizeof(double));
matrixProduct(a,1,3,Mr2,3,3,b);
// showMatrix(Mm,3,3,"M");
// showMatrix(Mminv,3,3,"M");
nxmin = nxmax = (int)floor(b[0][0]-dx);
nymin = nymax = (int)floor(b[0][1]-dy);
nzmin = nzmax = (int)floor(b[0][2]-dz);
for (ix=0;ix<=1;ix++) for (iy=0;iy<=1;iy++) for (iz=0;iz<=1;iz++) {
a[0][0]=ix*superCell.ax; a[0][1]=iy*superCell.by; a[0][2]=iz*superCell.cz;
matrixProduct(a,1,3,Mr2,3,3,b);
// showMatrix(b,1,3,"b");
if (nxmin > (int)floor(b[0][0]-dx)) nxmin=(int)floor(b[0][0]-dx);
if (nxmax < (int)ceil( b[0][0]-dx)) nxmax=(int)ceil( b[0][0]-dx);
if (nymin > (int)floor(b[0][1]-dy)) nymin=(int)floor(b[0][1]-dy);
if (nymax < (int)ceil( b[0][1]-dy)) nymax=(int)ceil( b[0][1]-dy);
if (nzmin > (int)floor(b[0][2]-dz)) nzmin=(int)floor(b[0][2]-dz);
if (nzmax < (int)ceil( b[0][2]-dz)) nzmax=(int)ceil( b[0][2]-dz);
}
// nxmin--;nxmax++;nymin--;nymax++;nzmin--;nzmax++;
superCell.atoms = (atom *)realloc(superCell.atoms,(superCell.natoms+1+
(nxmax-nxmin)*(nymax-nymin)*
(nzmax-nzmin)*grains[g].natoms)*
sizeof(atom));
// showMatrix(Mm,3,3,"Mm");
// showMatrix(Mminv,3,3,"Mminv");
printf("Grain %d: range: (%d..%d, %d..%d, %d..%d)\n",
g,nxmin,nxmax,nymin,nymax,nzmin,nzmax);
dx = grains[g].shiftx;
dy = grains[g].shifty;
dz = grains[g].shiftz;
for (iatom=0;iatom<grains[g].natoms;iatom++) {
memcpy(&newAtom,&(grains[g].unitCell[iatom]),sizeof(atom));
// We need to convert the cartesian coordinates of this atom
// to fractional ones:
b[0][0] = newAtom.x;
b[0][1] = newAtom.y;
b[0][2] = newAtom.z;
matrixProduct(b,1,3,Mminv,3,3,a);
newAtom.x = a[0][0]; newAtom.y = a[0][1]; newAtom.z = a[0][2];
//printf("%2d: %d (%g,%g,%g) (%g,%g,%g)\n",iatom,newAtom.Znum,
// b[0][0],b[0][1],b[0][2],a[0][0],a[0][1],a[0][2]);
for (ix=nxmin;ix<=nxmax;ix++) {
for (iy=nymin;iy<=nymax;iy++) {
for (iz=nzmin;iz<=nzmax;iz++) {
/* atom position in reduced coordinates: */
// a[0][0] = ix+newAtom.x; a[0][1] = iy+newAtom.y; a[0][2] = iz+newAtom.z;
a[0][0] = newAtom.x+ix; a[0][1] = newAtom.y+iy; a[0][2] = newAtom.z+iz;
matrixProduct(a,1,3,Mm,3,3,b);
/*
b[0][0] = a[0][0]*Mm[0][0]+a[0][1]*Mm[0][1]+a[0][2]*Mm[0][2];
b[0][1] = a[0][0]*Mm[1][0]+a[0][1]*Mm[1][1]+a[0][2]*Mm[1][2];
b[0][2] = a[0][0]*Mm[2][0]+a[0][1]*Mm[2][1]+a[0][2]*Mm[2][2];
*/
/* // same as matrixProduct:
b[0][0] = a[0][0]*Mm[0][0]+a[0][1]*Mm[1][0]+a[0][2]*Mm[2][0];
b[0][1] = a[0][0]*Mm[0][1]+a[0][1]*Mm[1][1]+a[0][2]*Mm[2][1];
b[0][2] = a[0][0]*Mm[0][2]+a[0][1]*Mm[1][2]+a[0][2]*Mm[2][2];
*/
superCell.atoms[atomCount].x = b[0][0]+dx;
superCell.atoms[atomCount].y = b[0][1]+dy;
superCell.atoms[atomCount].z = b[0][2]+dz;
if ((superCell.atoms[atomCount].x >= 0) &&
(superCell.atoms[atomCount].x < superCell.ax) &&
(superCell.atoms[atomCount].y >= 0) &&
(superCell.atoms[atomCount].y < superCell.by) &&
(superCell.atoms[atomCount].z >= 0) &&
(superCell.atoms[atomCount].z < superCell.cz)) {
// If this is a sphere:
if (grains[g].sphereRadius > 0) {
dxs = superCell.atoms[atomCount].x - grains[g].sphereX;
dys = superCell.atoms[atomCount].y - grains[g].sphereY;
dzs = superCell.atoms[atomCount].z - grains[g].sphereZ;
if (dxs*dxs+dys*dys+dzs*dzs < grains[g].sphereRadius*grains[g].sphereRadius) {
superCell.atoms[atomCount].dw = newAtom.dw;
superCell.atoms[atomCount].occ = newAtom.occ;
superCell.atoms[atomCount].q = newAtom.q;
superCell.atoms[atomCount].Znum = newAtom.Znum;
atomCount++;
}
}
// If this is a straight-edged grain
else {
for (p=0;p<grains[g].nplanes;p++) {
/*
printf("hello %d (%g %g %g)\n",g,
superCell.atoms[atomCount].x,superCell.atoms[atomCount].y,
superCell.atoms[atomCount].z);
*/
d = findLambda(grains[g].planes+p,&(superCell.atoms[atomCount].z),-1);
/*
printf("%3d lambda: %g (%g %g %g), (%g %g %g), %d\n",atomCount,d,
newAtom.x,newAtom.y,newAtom.z,
superCell.atoms[atomCount].x,superCell.atoms[atomCount].y,
superCell.atoms[atomCount].z,grains[g].nplanes);
*/
if (d < 0)
break;
}
/* if all the previous test have been successful, this atom is IN,
* which means that we also need to copy the other data elements
* for this atom.
*/
if (p == grains[g].nplanes) {
superCell.atoms[atomCount].q = newAtom.q;
superCell.atoms[atomCount].dw = newAtom.dw;
superCell.atoms[atomCount].occ = newAtom.occ;
superCell.atoms[atomCount].Znum = newAtom.Znum;
atomCount++;
}
} // if this is a sphere or not ...
}
} /* iz ... */
} /* iy ... */
} /* ix ... */
} /* iatom ... */
superCell.natoms = atomCount;
} /* end of if !amorph,i.e. crystalline */
} /* g=0..nGrains .. */
/*
atomPtr->x = 0.5; atomPtr->y = 0.2; atomPtr->z = 0.7;
findLambda(grains[0].planes,&(atomPtr->z),1);
*/
}
void KinematicMotion::addRotation(const double *_rotationMatrix) {
//R'*(R*x + t) = R'*R*x + R'*t
matrixProduct(_rotationMatrix, rotationMatrix);
matrixVectorProduct(_rotationMatrix, translationVector);
}
