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;
}
bool Localizer::triangulatePoint(const Vector &pxl, const Vector &pxr, Vector &x)
{
if ((pxl.length()<2) || (pxr.length()<2))
{
yError("Not enough values given for the pixels!");
return false;
}
if (PrjL && PrjR)
{
Vector torso=commData->get_torso();
Vector head=commData->get_q();
Vector qL(8);
qL[0]=torso[0];
qL[1]=torso[1];
qL[2]=torso[2];
qL[3]=head[0];
qL[4]=head[1];
qL[5]=head[2];
qL[6]=head[3];
qL[7]=head[4]+head[5]/2.0;
Vector qR=qL;
qR[7]-=head[5];
mutex.lock();
Matrix HL=SE3inv(eyeL->getH(qL));
Matrix HR=SE3inv(eyeR->getH(qR));
mutex.unlock();
Matrix tmp=zeros(3,4); tmp(2,2)=1.0;
tmp(0,2)=pxl[0]; tmp(1,2)=pxl[1];
Matrix AL=(*PrjL-tmp)*HL;
tmp(0,2)=pxr[0]; tmp(1,2)=pxr[1];
Matrix AR=(*PrjR-tmp)*HR;
Matrix A(4,3);
Vector b(4);
for (int i=0; i<2; i++)
{
b[i]=-AL(i,3);
b[i+2]=-AR(i,3);
for (int j=0; j<3; j++)
{
A(i,j)=AL(i,j);
A(i+2,j)=AR(i,j);
}
}
// solve the least-squares problem
x=pinv(A)*b;
return true;
}
else
{
yError("Unspecified projection matrix for at least one camera!");
return false;
}
}
