static double
strikeyH0 (MagComp component, const fault_params *fault, const magnetic_params *mag, double xi, double et, double qq, double y, double z)
{
double val;
double h = mag->dcurier;
double qd = y * sd + (fault->fdepth - h) * cd;
double K2_val = K2 (component, 1.0, xi, et, qq);
double log_re_val = log_re (component, 1.0, xi, et, qq);
double J2_val = J2 (component, 1.0, xi, et, qq);
// double L2_val = L2 (component, 1.0, xi, et, qq);
double L2_val = L2 (component, 1.0, xi, et, qq) * cd; // todo: check this!
double M2_val = M2 (component, 1.0, xi, et, qq);
double M3_val = M3 (component, 1.0, xi, et, qq);
double M2y_val = M2y (component, 1.0, xi, et, qq);
double M2z_val = M2z (component, 1.0, xi, et, qq);
val = - (2.0 - alpha4) * K2_val
+ alpha4 * log_re_val * sd + alpha3 * J2_val
- alpha3 * (qd * M2_val + (z - h) * L2_val * sd)
- 2.0 * alpha4 * h * (M2_val * cd - L2_val * sd)
- 4.0 * alpha1 * h * L2_val * sd
+ 2.0 * alpha2 * h * M3_val * sd
+ 2.0 * alpha2 * h * ((qd + h * cd) * M2z_val - (z - 2.0 * h) * M2y_val * sd);
return val;
}
//---------------------------------------------------------
DMat& NDG2D::Lift2D()
//---------------------------------------------------------
{
// function [LIFT] = Lift2D()
// Purpose : Compute surface to volume lift term for DG formulation
DMat V1D,massEdge1,massEdge2,massEdge3; DVec faceR,faceS;
Index1D J1(1,Nfp), J2(Nfp+1,2*Nfp), J3(2*Nfp+1,3*Nfp);
DMat Emat(Np, Nfaces*Nfp);
// face 1
faceR = r(Fmask(All,1));
V1D = Vandermonde1D(N, faceR);
massEdge1 = inv(V1D*trans(V1D));
Emat(Fmask(All,1), J1) = massEdge1;
// face 2
faceR = r(Fmask(All,2));
V1D = Vandermonde1D(N, faceR);
massEdge2 = inv(V1D*trans(V1D));
Emat(Fmask(All,2), J2) = massEdge2;
// face 3
faceS = s(Fmask(All,3));
V1D = Vandermonde1D(N, faceS);
massEdge3 = inv(V1D*trans(V1D));
Emat(Fmask(All,3), J3) = massEdge3;
// inv(mass matrix)*\I_n (L_i,L_j)_{edge_n}
LIFT = V*(trans(V)*Emat);
return LIFT;
}
int LSmear::var_to_bisect(IntervalMatrix& J, const IntervalVector& box) const {
int lvar = -1;
//Linearization
LPSolver::Status_Sol stat = LPSolver::UNKNOWN;
Vector dual_solution(1);
if (lsmode==LSMEAR_MG) { //compute the Jacobian in the midpoint
IntervalMatrix J2(sys.f_ctrs.image_dim(), sys.nb_var);
IntervalVector box2(IntervalVector(box.mid()).inflate(1e-8));
// IntervalVector box2(IntervalVector(box.random()));
box2 &= box;
sys.f_ctrs.jacobian(box2,J2);
stat = getdual(J2, box, dual_solution);
} else if (lsmode==LSMEAR) {
stat = getdual(J, box, dual_solution);
}
if (stat == LPSolver::OPTIMAL) {
double max_Lmagn = 0.0;
int k=0;
for (int j=0; j<nbvars; j++) {
Interval lsmear=Interval(0.0);
if ((!too_small(box,j)) && (box[j].mag() <1 || box[j].diam()/ box[j].mag() >= prec(j))){
lsmear=dual_solution[j];
for (int i=0; i<sys.f_ctrs.image_dim(); i++){
lsmear += dual_solution[sys.nb_var+i] * J[i][j];
}
}
lsmear*=(box[j].diam());
if (lsmear.mag() > 1e-10 && (j!=goal_var() || mylinearsolver->get_obj_value().mid() > box[goal_var()].lb() )) {
k++;
if (lsmear.mag() > max_Lmagn) {
max_Lmagn = lsmear.mag();
lvar = j;
}
}
}
if (k==1 && lvar==goal_var()) { lvar=-1; }
}
if (lvar==-1) {
// std::cout << "ssr " << std::endl;
lvar=SmearSumRelative::var_to_bisect(J, box);
}
// std::cout << "lsmear " << lvar << std::endl;
return lvar;
}
RDMat22 SphericalMapping::jacobian(const RDMat24 &nodes, const RDCol2 &xieta, int curvedOuter) const {
// rotate system such that curvedOuter = 2
const RDMat22 &Q2 = sOrthogQ2[curvedOuter];
const RDMat24 &nodes2 = Q2 * nodes;
const RDCol2 &xieta2 = Q2 * xieta;
// get r and theta
RDMat24 rtheta2;
rtheta2.row(0).array() = (nodes2.row(0).array().square() + nodes2.row(1).array().square()).sqrt();
for (int i = 0; i < 4; i++) {
rtheta2(1, i) = atan2(nodes2(0, i), nodes2(1, i));
}
// copy local variables
double r0 = rtheta2(0, Mapping::period0123(curvedOuter - 2));
double r3 = rtheta2(0, Mapping::period0123(curvedOuter + 1));
double t0 = rtheta2(1, Mapping::period0123(curvedOuter - 2));
double t1 = rtheta2(1, Mapping::period0123(curvedOuter - 1));
double t2 = rtheta2(1, Mapping::period0123(curvedOuter - 0));
double t3 = rtheta2(1, Mapping::period0123(curvedOuter + 1));
double xi = xieta2(0);
double eta = xieta2(1);
XMath::makeClose(t2, t3);
XMath::makeClose(t0, t1);
// compute in new system
RDMat22 J2;
J2(0, 0) = (1. + eta) * r3 * (t2 - t3) / 4. * cos(((1. - xi) * t3 + (1. + xi) * t2) / 2.)
+ (1. - eta) * r0 * (t1 - t0) / 4. * cos(((1. - xi) * t0 + (1. + xi) * t1) / 2.);
J2(0, 1) = .5 * (r3 * sin(((1. - xi) * t3 + (1. + xi) * t2) / 2.)
- r0 * sin(((1. - xi) * t0 + (1. + xi) * t1) / 2.));
J2(1, 0) = - (1. + eta) * r3 * (t2 - t3) / 4. * sin(((1. - xi) * t3 + (1. + xi) * t2) / 2.)
- (1. - eta) * r0 * (t1 - t0) / 4. * sin(((1. - xi) * t0 + (1. + xi) * t1) / 2.);
J2(1, 1) = .5 * (r3 * cos(((1. - xi) * t3 + (1. + xi) * t2) / 2.)
- r0 * cos(((1. - xi) * t0 + (1. + xi) * t1) / 2.));
// rotate back
return Q2.transpose() * J2 * Q2;
}
static double
strikeyHIII (MagComp component, const fault_params *fault, const magnetic_params *mag, double xi, double et, double qq, double y, double z)
{
double val;
double h = mag->dcurier;
double qd = y * sd + (fault->fdepth - h) * cd;
double K2_val = K2 (component, 1.0, xi, et, qq);
double log_re_val = log_re (component, 1.0, xi, et, qq);
double J2_val = J2 (component, 1.0, xi, et, qq);
double L2_val = L2 (component, 1.0, xi, et, qq);
double M2_val = M2 (component, 1.0, xi, et, qq);
val = - alpha4 * K2_val - alpha4 * log_re_val * sd - alpha3 * J2_val
+ alpha3 * (qd * M2_val + (z - h) * L2_val * sd * cd);
return val;
}
void
MovementSolute::solute (const Soil& soil, const SoilWater& soil_water,
const double J_above, Chemical& chemical,
const double dt,
const Scope& scope, Treelog& msg)
{
daisy_assert (std::isfinite (J_above));
const size_t cell_size = geometry ().cell_size ();
const size_t edge_size = geometry ().edge_size ();
// Source term transfered from secondary to primary domain.
std::vector<double> S_extra (cell_size, 0.0);
// Divide top solute flux according to water.
std::map<size_t, double> J_tertiary;
std::map<size_t, double> J_secondary;
std::map<size_t, double> J_primary;
if (J_above > 0.0)
// Outgoing, divide according to content in primary domain only.
divide_top_outgoing (geometry (), chemical, J_above,
J_primary, J_secondary, J_tertiary);
else if (J_above < 0.0)
// Incomming, divide according to all incomming water.
divide_top_incomming (geometry (), soil_water, J_above,
J_primary, J_secondary, J_tertiary);
else
// No flux.
zero_top (geometry (), J_primary, J_secondary, J_tertiary);
// Check result.
{
const std::vector<size_t>& edge_above
= geometry ().cell_edges (Geometry::cell_above);
const size_t edge_above_size = edge_above.size ();
double J_sum = 0.0;
for (size_t i = 0; i < edge_above_size; i++)
{
const size_t edge = edge_above[i];
const double in_sign
= geometry ().cell_is_internal (geometry ().edge_to (edge))
? 1.0 : -1.0;
const double area = geometry ().edge_area (edge); // [cm^2 S]
const double J_edge // [g/cm^2 S/h]
= J_tertiary[edge] + J_secondary[edge] + J_primary[edge];
J_sum += in_sign * J_edge * area; // [g/h]
if (in_sign * J_tertiary[edge] < 0.0)
{
std::ostringstream tmp;
tmp << "J_tertiary[" << edge << "] = " << J_tertiary[edge]
<< ", in_sign = " << in_sign << ", J_above = " << J_above;
msg.bug (tmp.str ());
}
if (in_sign * J_secondary[edge] < 0.0)
{
std::ostringstream tmp;
tmp << "J_secondary[" << edge << "] = " << J_secondary[edge]
<< ", in_sign = " << in_sign << ", J_above = " << J_above;
msg.bug (tmp.str ());
}
}
J_sum /= geometry ().surface_area (); // [g/cm^2 S/h]
daisy_approximate (-J_above, J_sum);
}
// We set a fixed concentration below lower boundary, if specified.
std::map<size_t, double> C_border;
const double C_below = chemical.C_below ();
if (C_below >= 0.0)
{
const std::vector<size_t>& edge_below
= geometry ().cell_edges (Geometry::cell_below);
const size_t edge_below_size = edge_below.size ();
for (size_t i = 0; i < edge_below_size; i++)
{
const size_t edge = edge_below[i];
C_border[edge] = C_below;
}
}
// Tertiary transport.
tertiary->solute (geometry (), soil_water, J_tertiary, dt, chemical, msg);
// Fully adsorbed.
if (chemical.adsorption ().full ())
{
static const symbol solid_name ("immobile transport");
Treelog::Open nest (msg, solid_name);
if (!iszero (J_above))
{
std::ostringstream tmp;
tmp << "J_above = " << J_above << ", expected 0 for full sorbtion";
msg.error (tmp.str ());
}
// Secondary "transport".
std::vector<double> J2 (edge_size, 0.0); // Flux delivered by flow.
std::vector<double> Mn (cell_size); // New content.
for (size_t c = 0; c < cell_size; c++)
{
Mn[c] = chemical.M_secondary (c) + chemical.S_secondary (c) * dt;
if (Mn[c] < 0.0)
{
S_extra[c] = Mn[c] / dt;
Mn[c] = 0.0;
}
else
S_extra[c] = 0.0;
}
chemical.set_secondary (soil, soil_water, Mn, J2);
// Primary "transport".
primary_transport (geometry (), soil, soil_water,
*matrix_solid, sink_sorbed, 0, J_primary, C_border,
chemical, S_extra, dt, scope, msg);
return;
}
// Secondary transport activated.
secondary_transport (geometry (), soil, soil_water, J_secondary, C_border,
chemical, S_extra, dt, scope, msg);
// Solute primary transport.
for (size_t transport_iteration = 0;
transport_iteration < 2;
transport_iteration++)
for (size_t i = 0; i < matrix_solute.size (); i++)
{
solute_attempt (i);
static const symbol solute_name ("solute");
Treelog::Open nest (msg, solute_name, i, matrix_solute[i]->objid);
try
{
primary_transport (geometry (), soil, soil_water,
*matrix_solute[i], sink_sorbed,
transport_iteration,
J_primary, C_border,
chemical, S_extra, dt, scope, msg);
if (i > 0)
msg.debug ("Succeeded");
return;
}
catch (const char* error)
{
msg.debug (std::string ("Solute problem: ") + error);
}
catch (const std::string& error)
{
msg.debug(std::string ("Solute trouble: ") + error);
}
solute_failure (i);
}
throw "Matrix solute transport failed";
}
// keep the poses at start_t and end_t but get rid of all of the ones in between
// don't like how this function is written. Ideally it would be a list of times or feature ids to be removed?
// Currently returning the marginal information, should change this?
// TODO For cooperative localization work this should work with no landmarks at all.
// TODO should make an LCM type for a dense factor?
MatrixXd FactorGraph::marginalize_poses_dense(int64_t start_t, int64_t end_t, int id){
cout << "1: " << time_in_ms() << endl;
// step 1: Get the information matrix:
SparseSystem Js = _slam.jacobian();
MatrixXd J(Js.num_rows(), Js.num_cols());
for (int r=0; r<Js.num_rows(); r++) {
for (int c=0; c<Js.num_cols(); c++) {
J(r,c) = Js(r,c);
}
}
cout << "2: " << time_in_ms() << endl;
//TODO really should go through and find the markov blanket explicitly. For now I just know that its all landmarks as well as the start and end poses.
Pose3dTS_Node* start_node = find_pose_from_time_and_id(start_t, id);
Pose3dTS_Node* end_node = find_pose_from_time_and_id(end_t, id);
MatrixXd J2(J.rows(),J.cols());
int current_col =0;
// first and last pose
for(int i = 0; i< start_node->dim(); i++){
J2.col(i) = J.col(start_node->start() + i);
J2.col(i+start_node->dim()) = J.col(end_node->start() + i);
}
current_col += start_node->dim() + end_node->dim();
// landmarks:
for(int l = 0; l<_features.size(); l++){
for(int i = 0; i<_features[l]->dim(); i++){
J2.col(current_col+i) = J.col(_features[l]->start()+i);
}
current_col += _features[l]->dim();
}
// intermediate poses
Pose3dTS_Node* next_node = find_next_pose_from_pose(start_node);
while(next_node != end_node){
for(int i = 0; i< next_node->dim(); i++){
J2.col(current_col+i) = J.col(next_node->start() + i);
}
current_col+=next_node->dim();
next_node = find_next_pose_from_pose(next_node);
}
cout << "3: " << time_in_ms() << endl;
MatrixXd H(J2.cols(),J2.cols());
H = J2.transpose() * J2; // inf matrix
// ofstream myfile;
// myfile.open("H.txt");
// myfile << H;
// myfile.close();
cout << "4: " << time_in_ms() << endl;
// step 4: schur complement.
MatrixXd A(12 + _features.size()*3, 12 + _features.size()*3);
A = H.topLeftCorner(12 + _features.size()*3, 12 + _features.size()*3);
MatrixXd B(12 + _features.size()*3, 6*(_poses[id].size()-2));
B = H.topRightCorner(12 + _features.size()*3, 6*(_poses[id].size()-2));
MatrixXd C(6*(_poses[id].size()-2),12 + _features.size()*3);
C = H.bottomLeftCorner(6*(_poses[id].size()-2),12 + _features.size()*3);
MatrixXd D(6*(_poses[id].size()-2),6*(_poses[id].size()-2));
D = H.bottomRightCorner(6*(_poses[id].size()-2),6*(_poses[id].size()-2));
MatrixXd H_m(12+_features.size()*3, 12+_features.size()*3);
H_m = A - B*(D.inverse())*C;
cout << "5: " << time_in_ms() << endl;
// TODO for dense should build constraint here and add as well as remove all of the poses that are being marginalized (including the factors which I think happens automatically) maybe look at how Nick C-B does the dense constraints... it's a bit tricky maybe to get general and I'm not sure I really need it... Might be able to directly use the GLC formulation
return H_m;
}
void Foam::kineticTheoryModel::solve(const volTensorField& gradUat)
{
if (!kineticTheory_)
{
return;
}
const scalar sqrtPi = sqrt(constant::mathematical::pi);
surfaceScalarField phi(1.5*rhoa_*phia_*fvc::interpolate(alpha_));
volTensorField dU(gradUat.T()); //fvc::grad(Ua_);
volSymmTensorField D(symm(dU));
// NB, drag = K*alpha*beta,
// (the alpha and beta has been extracted from the drag function for
// numerical reasons)
volScalarField Ur(mag(Ua_ - Ub_));
volScalarField betaPrim(alpha_*(1.0 - alpha_)*draga_.K(Ur));
// Calculating the radial distribution function (solid volume fraction is
// limited close to the packing limit, but this needs improvements)
// The solution is higly unstable close to the packing limit.
gs0_ = radialModel_->g0
(
min(max(alpha_, scalar(1e-6)), alphaMax_ - 0.01),
alphaMax_
);
// particle pressure - coefficient in front of Theta (Eq. 3.22, p. 45)
volScalarField PsCoeff
(
granularPressureModel_->granularPressureCoeff
(
alpha_,
gs0_,
rhoa_,
e_
)
);
// 'thermal' conductivity (Table 3.3, p. 49)
kappa_ = conductivityModel_->kappa(alpha_, Theta_, gs0_, rhoa_, da_, e_);
// particle viscosity (Table 3.2, p.47)
mua_ = viscosityModel_->mua(alpha_, Theta_, gs0_, rhoa_, da_, e_);
dimensionedScalar Tsmall
(
"small",
dimensionSet(0 , 2 ,-2 ,0 , 0, 0, 0),
1.0e-6
);
dimensionedScalar TsmallSqrt = sqrt(Tsmall);
volScalarField ThetaSqrt(sqrt(Theta_));
// dissipation (Eq. 3.24, p.50)
volScalarField gammaCoeff
(
12.0*(1.0 - sqr(e_))*sqr(alpha_)*rhoa_*gs0_*(1.0/da_)*ThetaSqrt/sqrtPi
);
// Eq. 3.25, p. 50 Js = J1 - J2
volScalarField J1(3.0*betaPrim);
volScalarField J2
(
0.25*sqr(betaPrim)*da_*sqr(Ur)
/(max(alpha_, scalar(1e-6))*rhoa_*sqrtPi*(ThetaSqrt + TsmallSqrt))
);
// bulk viscosity p. 45 (Lun et al. 1984).
lambda_ = (4.0/3.0)*sqr(alpha_)*rhoa_*da_*gs0_*(1.0+e_)*ThetaSqrt/sqrtPi;
// stress tensor, Definitions, Table 3.1, p. 43
volSymmTensorField tau(2.0*mua_*D + (lambda_ - (2.0/3.0)*mua_)*tr(D)*I);
if (!equilibrium_)
{
// construct the granular temperature equation (Eq. 3.20, p. 44)
// NB. note that there are two typos in Eq. 3.20
// no grad infront of Ps
// wrong sign infront of laplacian
fvScalarMatrix ThetaEqn
(
fvm::ddt(1.5*alpha_*rhoa_, Theta_)
+ fvm::div(phi, Theta_, "div(phi,Theta)")
==
fvm::SuSp(-((PsCoeff*I) && dU), Theta_)
+ (tau && dU)
+ fvm::laplacian(kappa_, Theta_, "laplacian(kappa,Theta)")
+ fvm::Sp(-gammaCoeff, Theta_)
+ fvm::Sp(-J1, Theta_)
+ fvm::Sp(J2/(Theta_ + Tsmall), Theta_)
);
ThetaEqn.relax();
ThetaEqn.solve();
}
else
{
// equilibrium => dissipation == production
// Eq. 4.14, p.82
volScalarField K1(2.0*(1.0 + e_)*rhoa_*gs0_);
volScalarField K3
(
0.5*da_*rhoa_*
(
(sqrtPi/(3.0*(3.0-e_)))
*(1.0 + 0.4*(1.0 + e_)*(3.0*e_ - 1.0)*alpha_*gs0_)
+1.6*alpha_*gs0_*(1.0 + e_)/sqrtPi
)
);
volScalarField K2
(
4.0*da_*rhoa_*(1.0 + e_)*alpha_*gs0_/(3.0*sqrtPi) - 2.0*K3/3.0
);
volScalarField K4(12.0*(1.0 - sqr(e_))*rhoa_*gs0_/(da_*sqrtPi));
volScalarField trD(tr(D));
volScalarField tr2D(sqr(trD));
volScalarField trD2(tr(D & D));
volScalarField t1(K1*alpha_ + rhoa_);
volScalarField l1(-t1*trD);
volScalarField l2(sqr(t1)*tr2D);
volScalarField l3
(
4.0
*K4
*max(alpha_, scalar(1e-6))
*(2.0*K3*trD2 + K2*tr2D)
);
Theta_ = sqr((l1 + sqrt(l2 + l3))/(2.0*(alpha_ + 1.0e-4)*K4));
}
Theta_.max(1.0e-15);
Theta_.min(1.0e+3);
volScalarField pf
(
frictionalStressModel_->frictionalPressure
(
alpha_,
alphaMinFriction_,
alphaMax_,
Fr_,
eta_,
p_
)
);
PsCoeff += pf/(Theta_+Tsmall);
PsCoeff.min(1.0e+10);
PsCoeff.max(-1.0e+10);
// update particle pressure
pa_ = PsCoeff*Theta_;
// frictional shear stress, Eq. 3.30, p. 52
volScalarField muf
(
frictionalStressModel_->muf
(
alpha_,
alphaMax_,
pf,
D,
phi_
)
);
// add frictional stress
mua_ += muf;
mua_.min(1.0e+2);
mua_.max(0.0);
Info<< "kinTheory: max(Theta) = " << max(Theta_).value() << endl;
volScalarField ktn(mua_/rhoa_);
Info<< "kinTheory: min(nua) = " << min(ktn).value()
<< ", max(nua) = " << max(ktn).value() << endl;
Info<< "kinTheory: min(pa) = " << min(pa_).value()
<< ", max(pa) = " << max(pa_).value() << endl;
}
