/** Execute the algorithm.
*/
void CalculateUMatrix::exec()
{
double a=this->getProperty("a");
double b=this->getProperty("b");
double c=this->getProperty("c");
double alpha=this->getProperty("alpha");
double beta=this->getProperty("beta");
double gamma=this->getProperty("gamma");
OrientedLattice o(a,b,c,alpha,beta,gamma);
Matrix<double> B=o.getB();
double H,K,L;
PeaksWorkspace_sptr ws;
ws = boost::dynamic_pointer_cast<PeaksWorkspace>(AnalysisDataService::Instance().retrieve(this->getProperty("PeaksWorkspace")) );
if (!ws) throw std::runtime_error("Problems reading the peaks workspace");
Matrix<double> Hi(4,4),Si(4,4),HS(4,4),zero(4,4);
for (int i=0;i<ws->getNumberPeaks();i++)
{
Peak p=ws->getPeaks()[i];
H=p.getH();
K=p.getK();
L=p.getL();
if(H*H+K*K+L*L>0)
{
V3D Qhkl=B*V3D(H,K,L);
Hi[0][0]=0.; Hi[0][1]=-Qhkl.X(); Hi[0][2]=-Qhkl.Y(); Hi[0][3]=-Qhkl.Z();
Hi[1][0]=Qhkl.X(); Hi[1][1]=0.; Hi[1][2]=Qhkl.Z(); Hi[1][3]=-Qhkl.Y();
Hi[2][0]=Qhkl.Y(); Hi[2][1]=-Qhkl.Z(); Hi[2][2]=0.; Hi[2][3]=Qhkl.X();
Hi[3][0]=Qhkl.Z(); Hi[3][1]=Qhkl.Y(); Hi[3][2]=-Qhkl.X(); Hi[3][3]=0.;
V3D Qgon=p.getQSampleFrame();
Si[0][0]=0.; Si[0][1]=-Qgon.X(); Si[0][2]=-Qgon.Y(); Si[0][3]=-Qgon.Z();
Si[1][0]=Qgon.X(); Si[1][1]=0.; Si[1][2]=-Qgon.Z(); Si[1][3]=Qgon.Y();
Si[2][0]=Qgon.Y(); Si[2][1]=Qgon.Z(); Si[2][2]=0.; Si[2][3]=-Qgon.X();
Si[3][0]=Qgon.Z(); Si[3][1]=-Qgon.Y(); Si[3][2]=Qgon.X(); Si[3][3]=0.;
HS+=(Hi*Si);
}
}
//check if HS is 0
if (HS==zero) throw std::invalid_argument("The peaks workspace is not indexed or something really bad happened");
Matrix<double> Eval;
Matrix<double> Diag;
HS.Diagonalise(Eval,Diag);
Eval.sortEigen(Diag);
Mantid::Kernel::Quat qR(Eval[0][0],Eval[1][0],Eval[2][0],Eval[3][0]);//the first column corresponds to the highest eigenvalue
DblMatrix U(qR.getRotation());
o.setU(U);
ws->mutableSample().setOrientedLattice(new OrientedLattice(o));
}
Eigen::Quaterniond getQuaternionForAngularChange(double time) {
const double wn = sqrt(x*x + y*y + z*z);
const double ang = wn * time / 2;
const double swn = sin(ang);
const double cwn = cos(ang);
const double wxn = (wn != 0.0) ? x/wn : 0.0;
const double wyn = (wn != 0.0) ? y/wn : 0.0;
const double wzn = (wn != 0.0) ? z/wn : 0.0;
Eigen::Quaterniond qR(cwn, swn*wxn, swn*wyn, swn*wzn);
return qR;
};
/** Execute the algorithm.
*/
void CalculateUMatrix::exec()
{
double a=this->getProperty("a");
double b=this->getProperty("b");
double c=this->getProperty("c");
double alpha=this->getProperty("alpha");
double beta=this->getProperty("beta");
double gamma=this->getProperty("gamma");
OrientedLattice o(a,b,c,alpha,beta,gamma);
Matrix<double> B=o.getB();
double H,K,L;
PeaksWorkspace_sptr ws;
ws = AnalysisDataService::Instance().retrieveWS<PeaksWorkspace>(this->getProperty("PeaksWorkspace") );
if (!ws) throw std::runtime_error("Problems reading the peaks workspace");
size_t nIndexedpeaks=0;
bool found2nc=false;
V3D old(0,0,0);
Matrix<double> Hi(4,4),Si(4,4),HS(4,4),zero(4,4);
for (int i=0;i<ws->getNumberPeaks();i++)
{
Peak p=ws->getPeaks()[i];
H=p.getH();
K=p.getK();
L=p.getL();
if(H*H+K*K+L*L>0)
{
nIndexedpeaks++;
if (!found2nc)
{
if (nIndexedpeaks==1)
{
old=V3D(H,K,L);
}
else
{
if (!old.coLinear(V3D(0,0,0),V3D(H,K,L))) found2nc=true;
}
}
V3D Qhkl=B*V3D(H,K,L);
Hi[0][0]=0.; Hi[0][1]=-Qhkl.X(); Hi[0][2]=-Qhkl.Y(); Hi[0][3]=-Qhkl.Z();
Hi[1][0]=Qhkl.X(); Hi[1][1]=0.; Hi[1][2]=Qhkl.Z(); Hi[1][3]=-Qhkl.Y();
Hi[2][0]=Qhkl.Y(); Hi[2][1]=-Qhkl.Z(); Hi[2][2]=0.; Hi[2][3]=Qhkl.X();
Hi[3][0]=Qhkl.Z(); Hi[3][1]=Qhkl.Y(); Hi[3][2]=-Qhkl.X(); Hi[3][3]=0.;
V3D Qgon=p.getQSampleFrame();
Si[0][0]=0.; Si[0][1]=-Qgon.X(); Si[0][2]=-Qgon.Y(); Si[0][3]=-Qgon.Z();
Si[1][0]=Qgon.X(); Si[1][1]=0.; Si[1][2]=-Qgon.Z(); Si[1][3]=Qgon.Y();
Si[2][0]=Qgon.Y(); Si[2][1]=Qgon.Z(); Si[2][2]=0.; Si[2][3]=-Qgon.X();
Si[3][0]=Qgon.Z(); Si[3][1]=-Qgon.Y(); Si[3][2]=Qgon.X(); Si[3][3]=0.;
HS+=(Hi*Si);
}
}
//check if enough peaks are indexed or if HS is 0
if ((nIndexedpeaks<2) || (found2nc==false)) throw std::invalid_argument("Less then two non-colinear peaks indexed");
if (HS==zero) throw std::invalid_argument("Something really bad happened");
Matrix<double> Eval;
Matrix<double> Diag;
HS.Diagonalise(Eval,Diag);
Eval.sortEigen(Diag);
Mantid::Kernel::Quat qR(Eval[0][0],Eval[1][0],Eval[2][0],Eval[3][0]);//the first column corresponds to the highest eigenvalue
DblMatrix U(qR.getRotation());
o.setU(U);
ws->mutableSample().setOrientedLattice(new OrientedLattice(o));
}
std::vector<simd_vector> HLLC_flux(const conserved_vars& UL, const conserved_vars& UR, const simd_vector& phi,
dimension dim) {
std::vector<simd_vector> F(NF, simd_vector (G2));
integer shear1, shear2;
if (dim == XDIM) {
shear1 = s_i + YDIM;
shear2 = s_i + ZDIM;
} else if (dim == YDIM) {
shear1 = s_i + XDIM;
shear2 = s_i + ZDIM;
} else {
shear1 = s_i + XDIM;
shear2 = s_i + YDIM;
}
auto qs = [](real p_star, real ps) {
real gp1over2g = (fgamma + real(1)) / (real(2) * fgamma);
const real H = p_star / ps;
real r;
if( H < real(1)) {
r= real(1);
} else {
r= std::sqrt(real(1) + gp1over2g * (H- real(1)));
}
return r;
};
constexpr
real hf = real(1) / real(2);
real _1over8 = real(1) / real(8);
const auto VR = UR.to_primitive();
const auto VL = UL.to_primitive();
const simd_vector& uR = VR.v(dim);
const simd_vector& uL = VL.v(dim);
const simd_vector& pR = VR.p();
const simd_vector& pL = VL.p();
const simd_vector& aR = VR.c();
const simd_vector& aL = VL.c();
const simd_vector& rhoR = VR.rho();
const simd_vector& rhoL = VL.rho();
simd_vector u_star = hf * (uL + uR) - (real(2) * (pR - pL)) / ((rhoR + rhoL) * (aR + aL));
simd_vector p_star = hf * (pL + pR) - (_1over8 * (uR - uL) * (rhoR + rhoL) * (aR + aL));
simd_vector& sM = u_star;
simd_vector qR(G2), qL(G2);
for (integer g = 0; g != G2; ++g) {
qR[g] = qs(p_star[g], pR[g]);
qL[g] = qs(p_star[g], pL[g]);
}
simd_vector sL = uL - aL * qL;
simd_vector sR = uR + aR * qR;
for (integer g = 0; g != G2; ++g) {
sL[g] = std::min(real(0), sL[g]);
sR[g] = std::max(real(0), sR[g]);
sM[g] = std::min(sR[g], sM[g]);
sM[g] = std::max(sL[g], sM[g]);
}
const auto fR = UR.flux(VR, dim);
const auto fL = UL.flux(VL, dim);
std::vector<simd_vector> Q(NF, simd_vector (G2));
std::vector<simd_vector> R(NF, simd_vector (G2));
for (integer f = 0; f != NF; ++f) {
Q[f] = sL * UL[f] - fL[f];
R[f] = sR * UR[f] - fR[f];
}
std::array < simd_vector, NF > U_starR;
std::array < simd_vector, NF > U_starL;
p_star = ((sM * Q[rho_i] - Q[s_i + dim]) + (sM * R[rho_i] - R[s_i + dim])) / real(2);
p_star /= real(2);
p_star = (p_star + std::abs(p_star));
U_starL[rho_i] = Q[rho_i] / (sL - sM);
U_starR[rho_i] = R[rho_i] / (sR - sM);
U_starL[s_i + dim] = U_starL[rho_i] * u_star;
U_starR[s_i + dim] = U_starR[rho_i] * u_star;
U_starL[shear1] = UL[shear1];
U_starR[shear1] = UR[shear1];
U_starL[shear2] = UL[shear2];
U_starR[shear2] = UR[shear2];
U_starL[egas_i] = p_star / (fgamma - real(1))
+ hf
* (std::pow(U_starL[s_i + XDIM], real(2)) + std::pow(U_starL[s_i + YDIM], real(2))
+ std::pow(U_starL[s_i + ZDIM], real(2))) / U_starL[rho_i];
U_starR[egas_i] = p_star / (fgamma - real(1))
+ hf
* (std::pow(U_starR[s_i + XDIM], real(2)) + std::pow(U_starR[s_i + YDIM], real(2))
+ std::pow(U_starR[s_i + ZDIM], real(2))) / U_starR[rho_i];
U_starL[tau_i] = U_starR[tau_i] = std::pow(p_star / (fgamma - real(1)), real(1) / fgamma);
for (integer f = 0; f != NF; ++f) {
for (integer g = 0; g != G2; ++g) {
assert(sR[g] >= sM[g]);
assert(sM[g] >= sL[g]);
if (sL[g] >= real(0) && sM[g] >= real(0)) {
F[f][g] = fL[f][g];
} else if (sL[g] < real(0) && sR[g] > real(0)) {
if (sM[g] >= real(0)) {
F[f][g] = fL[f][g] + sL[g] * (U_starL[f][g] - UL[f][g]);
} else if (sM[g] <= real(0)) {
F[f][g] = fR[f][g] + sR[g] * (U_starR[f][g] - UR[f][g]);
}
} else if (sM[g] <= real(0) && sR[g] <= real(0)) {
F[f][g] = fR[f][g];
} else {
assert(false);
}
}
}
const integer f = egas_i;
for (integer g = 0; g != G2; ++g) {
assert(sR[g] >= sM[g]);
assert(sM[g] >= sL[g]);
if (sL[g] >= real(0) && sM[g] >= real(0)) {
F[f][g] += UL[s_i + dim][g] * phi[g];
} else if (sL[g] < real(0) && sR[g] > real(0)) {
if (sM[g] >= real(0)) {
F[f][g] += phi[g] * (UL[s_i + dim][g] + sL[g] * (U_starL[rho_i][g] - UL[rho_i][g]));
} else if (sM[g] <= real(0)) {
F[f][g] += phi[g] * (UR[s_i + dim][g] + sR[g] * (U_starR[rho_i][g] - UR[rho_i][g]));
}
} else if (sM[g] <= real(0) && sR[g] <= real(0)) {
F[f][g] += UR[s_i + dim][g] * phi[g];
} else {
assert(false);
}
}
return F;
}
