//--------------------------------------------------------------------------
void Omu_IntODE::solve(int kk, double tstart, double tend,
const Omu_VariableVec &x, const Omu_VariableVec &u,
Omu_Program *sys, Omu_DependentVec &Ft,
Omu_StateVec &xt)
{
int i, j;
_sys = sys; // propagate to syseq()
_xt_ptr = &xt;
_Ft_ptr = &Ft;
v_zero(_y);
for (i = 0; i < _nd; i++) {
_u[i] = xt[i];
}
for (i = 0; i < _n; i++) {
_y[i] = xt[_nd + i]; // initial states
}
for (i = 0; i < _nu; i++) {
_u[_nd + i] = u[i];
}
v_zero(_dxt); // time derivatives passed to continuous
if (_sa) {
for (i = 0; i < _n; i++) {
for (j = 0; j < _nx; j++) {
_y[(1 + j) * _n + i] = xt.Sx[_nd + i][j];
}
for (j = 0; j < _nu; j++) {
_y[(1 + _nx + j) * _n + i] = xt.Su[_nd + i][j];
}
}
m_zero(_dxt.Sx);
m_zero(_dxt.Su);
}
_kk = kk; // propagate to syseq()
ode_solve(tstart, _y, _u, tend);
for (i = 0; i < _n; i++) {
xt[_nd + i] = _y[i];
}
if (_sa) {
for (i = 0; i < _n; i++) {
for (j = 0; j < _nx; j++) {
xt.Sx[_nd + i][j] = _y[(1 + j) * _n + i];
}
for (j = 0; j < _nu; j++) {
xt.Su[_nd + i][j] = _y[(1 + _nx + j) * _n + i];
}
}
}
}
SpectralDifferenceAudioCurve::SpectralDifferenceAudioCurve(Parameters parameters) :
AudioCurveCalculator(parameters)
{
m_mag = allocate<double>(m_lastPerceivedBin + 1);
m_tmpbuf = allocate<double>(m_lastPerceivedBin + 1);
v_zero(m_mag, m_lastPerceivedBin + 1);
}
MAT *_hhtrcols(MAT *M, unsigned int i0, unsigned int j0,
const VEC *hh, double beta, VEC *w)
#endif
{
/* Real ip, scale; */
int i /*, k */;
/* STATIC VEC *w = VNULL; */
if ( M == MNULL || hh == VNULL || w == VNULL )
error(E_NULL,"_hhtrcols");
if ( M->m != hh->dim )
error(E_SIZES,"_hhtrcols");
if ( i0 > M->m || j0 > M->n )
error(E_BOUNDS,"_hhtrcols");
if ( beta == 0.0 ) return (M);
if ( w->dim < M->n )
w = v_resize(w,M->n);
/* MEM_STAT_REG(w,TYPE_VEC); */
v_zero(w);
for ( i = i0; i < M->m; i++ )
if ( hh->ve[i] != 0.0 )
__mltadd__(&(w->ve[j0]),&(M->me[i][j0]),hh->ve[i],
(int)(M->n-j0));
for ( i = i0; i < M->m; i++ )
if ( hh->ve[i] != 0.0 )
__mltadd__(&(M->me[i][j0]),&(w->ve[j0]),-beta*hh->ve[i],
(int)(M->n-j0));
return (M);
}
/* hhtrcols -- transform a matrix by a Householder vector by columns
starting at row i0 from column j0 -- in-situ */
MAT *hhtrcols(MAT *M,unsigned int i0,unsigned int j0,VEC *hh,double beta)
{
/* Real ip, scale; */
int i /*, k */;
static VEC *w = VNULL;
if ( M==(MAT *)NULL || hh==(VEC *)NULL )
error(E_NULL,"hhtrcols");
if ( M->m != hh->dim )
error(E_SIZES,"hhtrcols");
if ( i0 > M->m || j0 > M->n )
error(E_BOUNDS,"hhtrcols");
if ( beta == 0.0 ) return (M);
w = v_resize(w,M->n);
MEM_STAT_REG(w,TYPE_VEC);
v_zero(w);
for ( i = i0; i < M->m; i++ )
if ( hh->ve[i] != 0.0 )
__mltadd__(&(w->ve[j0]),&(M->me[i][j0]),hh->ve[i],
(int)(M->n-j0));
for ( i = i0; i < M->m; i++ )
if ( hh->ve[i] != 0.0 )
__mltadd__(&(M->me[i][j0]),&(w->ve[j0]),-beta*hh->ve[i],
(int)(M->n-j0));
return (M);
}
VEC *vm_mlt(const MAT *A, const VEC *b, VEC *out)
#endif
{
unsigned int j,m,n;
/* Real sum,**A_v,*b_v; */
if ( A==(MAT *)NULL || b==(VEC *)NULL )
error(E_NULL,"vm_mlt");
if ( A->m != b->dim )
error(E_SIZES,"vm_mlt");
if ( b == out )
error(E_INSITU,"vm_mlt");
if ( out == (VEC *)NULL || out->dim != A->n )
out = v_resize(out,A->n);
m = A->m; n = A->n;
v_zero(out);
for ( j = 0; j < m; j++ )
if ( b->ve[j] != 0.0 )
__mltadd__(out->ve,A->me[j],b->ve[j],(int)n);
/**************************************************
A_v = A->me; b_v = b->ve;
for ( j=0; j<n; j++ )
{
sum = 0.0;
for ( i=0; i<m; i++ )
sum += b_v[i]*A_v[i][j];
out->ve[j] = sum;
}
**************************************************/
return out;
}
/* normalise vector */
void v_norm(Vec *v2, Vec *v1)
{
if (v_len(v1) > 0)
v_div_sca(v2, v1, v_len(v1));
else
v_zero(v2);
}
/* normalise vector */
void vector_norm(Vec *v1, Vec *v2)
{
if (v_len(v1) > 0)
vector_div_sca(v1, v_len(v1), v2);
else
v_zero(v2);
}
MAT *m_inverse(const MAT *A, MAT *out)
{
unsigned int i;
char MatrixTempBuffer[ 4000 ];
VEC *tmp = VNULL, *tmp2 = VNULL;
MAT *A_cp = MNULL;
PERM *pivot = PNULL;
if ( ! A )
error(E_NULL,"m_inverse");
if ( A->m != A->n )
error(E_SQUARE,"m_inverse");
if ( ! out || out->m < A->m || out->n < A->n )
out = m_resize(out,A->m,A->n);
if( SET_MAT_SIZE( A->m, A->n ) < 1000 )
mat_get( &A_cp, (void *)( MatrixTempBuffer + 2000 ), A->m, A->n );
else
A_cp = matrix_get( A->m, A->n );
A_cp = m_copy( A, A_cp );
if( SET_VEC_SIZE( A->m ) < 1000 ) {
vec_get( &tmp, (void *)MatrixTempBuffer, A->m );
vec_get( &tmp2, (void *)(MatrixTempBuffer + 1000), A->m );
} else {
tmp = v_get( A->m );
tmp2 = v_get( A->m );
}
if( SET_PERM_SIZE( A->m ) < 1000 ) {
perm_get( &pivot, (void *)( MatrixTempBuffer + 3000 ), A->m );
} else {
pivot = px_get( A->m );
}
LUfactor(A_cp,pivot);
//tracecatch_matrix(LUfactor(A_cp,pivot),"m_inverse");
for ( i = 0; i < A->n; i++ ){
v_zero(tmp);
tmp->ve[i] = 1.0;
LUsolve(A_cp,pivot,tmp,tmp2);
//tracecatch_matrix(LUsolve(A_cp,pivot,tmp,tmp2),"m_inverse");
set_col(out,i,tmp2);
}
if( tmp != (VEC *)(MatrixTempBuffer ) ) // память выделялась, надо освободить
V_FREE(tmp);
if( tmp2 != (VEC *)(MatrixTempBuffer + 1000) ) // память выделялась, надо освободить
V_FREE(tmp2);
if( A_cp != (MAT *)(MatrixTempBuffer + 2000) ) // память выделялась, надо освободить
M_FREE(A_cp);
if( pivot != (PERM *)(MatrixTempBuffer + 3000) ) // память выделялась, надо освободить
PX_FREE( pivot );
return out;
}
void
pl_normal(const Polygon * pl, vec normal)
{
assert(pl);
if (pl->last < 3) v_zero(normal);
else{
pl_area_vec(pl, normal);
v_normal(normal, normal);
}
}
void quat_rot_vec(VEC *q1, VEC *v1){
VEC *quat_of_vec, *qmulv, *qinv, *fin_rot_vec; //all are quaternions
quat_of_vec = v_get(4);
v_zero(quat_of_vec);
qmulv = v_get(4);
v_zero(qmulv);
qinv = v_get(4);
v_zero(qinv);
fin_rot_vec = v_get(4);
v_zero(fin_rot_vec);
vec2quat(v1, quat_of_vec);
quat_inv(q1, qinv);
quat_mul(qinv, quat_of_vec, qmulv);
quat_mul(qmulv, q1, fin_rot_vec);
quat2vec(fin_rot_vec, v1);
v_free(quat_of_vec);
v_free(qmulv);
v_free(qinv);
v_free(fin_rot_vec);
}
VEC *QRTsolve(const MAT *A, const VEC *diag, const VEC *c, VEC *sc)
#endif
{
int i, j, k, n, p;
Real beta, r_ii, s, tmp_val;
if ( ! A || ! diag || ! c )
error(E_NULL,"QRTsolve");
if ( diag->dim < min(A->m,A->n) )
error(E_SIZES,"QRTsolve");
sc = v_resize(sc,A->m);
n = sc->dim;
p = c->dim;
if ( n == p )
k = p-2;
else
k = p-1;
v_zero(sc);
sc->ve[0] = c->ve[0]/A->me[0][0];
if ( n == 1)
return sc;
if ( p > 1)
{
for ( i = 1; i < p; i++ )
{
s = 0.0;
for ( j = 0; j < i; j++ )
s += A->me[j][i]*sc->ve[j];
if ( A->me[i][i] == 0.0 )
error(E_SING,"QRTsolve");
sc->ve[i]=(c->ve[i]-s)/A->me[i][i];
}
}
for (i = k; i >= 0; i--)
{
s = diag->ve[i]*sc->ve[i];
for ( j = i+1; j < n; j++ )
s += A->me[j][i]*sc->ve[j];
r_ii = fabs(A->me[i][i]);
tmp_val = (r_ii*fabs(diag->ve[i]));
beta = ( tmp_val == 0.0 ) ? 0.0 : 1.0/tmp_val;
tmp_val = beta*s;
sc->ve[i] -= tmp_val*diag->ve[i];
for ( j = i+1; j < n; j++ )
sc->ve[j] -= tmp_val*A->me[j][i];
}
return sc;
}
void booz_sensors_model_accel_run( double time ) {
if (time < bsm.accel_next_update)
return;
static VEC* accel_ltp = VNULL;
accel_ltp = v_resize(accel_ltp, AXIS_NB);
/* substract gravity to acceleration in ltp frame */
accel_ltp = v_sub(bfm.accel_ltp, bfm.g_ltp, accel_ltp);
/* convert to body frame */
static VEC* accel_body = VNULL;
accel_body = v_resize(accel_body, AXIS_NB);
mv_mlt(bfm.dcm, accel_ltp, accel_body);
/* convert to imu frame */
static VEC* accel_imu = VNULL;
accel_imu = v_resize(accel_imu, AXIS_NB);
mv_mlt(bsm.body_to_imu, accel_body, accel_imu);
// printf(" accel_body ~ %f %f %f\n", accel_body->ve[AXIS_X], accel_body->ve[AXIS_Y], accel_body->ve[AXIS_Z]);
/* compute accel reading */
mv_mlt(bsm.accel_sensitivity, accel_imu, bsm.accel);
v_add(bsm.accel, bsm.accel_neutral, bsm.accel);
/* compute accel error readings */
static VEC *accel_error = VNULL;
accel_error = v_resize(accel_error, AXIS_NB);
accel_error = v_zero(accel_error);
/* add a gaussian noise */
accel_error = v_add_gaussian_noise(accel_error, bsm.accel_noise_std_dev, accel_error);
/* constant bias */
accel_error = v_add(accel_error, bsm.accel_bias, accel_error);
/* scale to adc units FIXME : should use full adc gain ? sum ? */
accel_error->ve[AXIS_X] = accel_error->ve[AXIS_X] * bsm.accel_sensitivity->me[AXIS_X][AXIS_X];
accel_error->ve[AXIS_Y] = accel_error->ve[AXIS_Y] * bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y];
accel_error->ve[AXIS_Z] = accel_error->ve[AXIS_Z] * bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z];
/* add per accel error reading */
bsm.accel = v_add(bsm.accel, accel_error, bsm.accel);
/* round signal to account for adc discretisation */
RoundSensor(bsm.accel);
/* saturation */
BoundSensor(bsm.accel, 0, bsm.accel_resolution);
// printf("sim adc %f %f %f\n",bsm.accel->ve[AXIS_X] ,bsm.accel->ve[AXIS_Y] ,bsm.accel->ve[AXIS_Z]);
bsm.accel_next_update += BSM_ACCEL_DT;
bsm.accel_available = TRUE;
}
void
pl_area_vec(const Polygon * pl, vec dest)
{
assert(pl);
v_zero(dest);
if (pl->last < 3) return;
uint i, j;
vec tmp;
for (i=pl->last - 1, j=0; j < pl->last; i=j++){
v_cross(tmp, pl->points[i].co, pl->points[j].co);
v_add(dest, dest, tmp);
}
}
void initparticle(particle_type* p) {
p->position[0] = ps->initpos[0];
p->position[1] = ps->initpos[1];
p->position[2] = ps->initpos[2];
p->position[3] = 0.0;
v_zero(p->velocity);
p->color[0] = 1.0;
p->color[1] = 1.0;
p->color[2] = 1.0;
p->color[3] = 0.0;
vv_cpy(p->prevpos[0], p->position);
vv_cpy(p->prevpos[1], p->position);
vv_cpy(p->prevpos[2], p->position);
vv_cpy(p->prevpos[3], p->position);
vv_cpy(p->prevpos[4], p->position);
p->begin = 0;
}
/* v_conv -- computes convolution product of two vectors */
VEC *v_conv(VEC *x1, VEC *x2, VEC *out)
{
int i;
if ( ! x1 || ! x2 )
error(E_NULL,"v_conv");
if ( x1 == out || x2 == out )
error(E_INSITU,"v_conv");
if ( x1->dim == 0 || x2->dim == 0 )
return out = v_resize(out,0);
out = v_resize(out,x1->dim + x2->dim - 1);
v_zero(out);
for ( i = 0; i < x1->dim; i++ )
__mltadd__(&(out->ve[i]),x2->ve,x1->ve[i],x2->dim);
return out;
}
void booz_sensors_model_mag_run( double time ) {
if (time < bsm.mag_next_update)
return;
/* rotate h to body frame */
static VEC *h_body = VNULL;
h_body = v_resize(h_body, AXIS_NB);
mv_mlt(bfm.dcm, bfm.h_ltp, h_body);
/* rotate to imu frame */
static VEC *h_imu = VNULL;
h_imu = v_resize(h_imu, AXIS_NB);
mv_mlt(bsm.body_to_imu, h_body, h_imu);
/* rotate to sensor frame */
static VEC *h_sensor = VNULL;
h_sensor = v_resize(h_sensor, AXIS_NB);
mv_mlt(bsm.mag_imu_to_sensor, h_imu, h_sensor);
mv_mlt(bsm.mag_sensitivity, h_sensor, bsm.mag);
v_add(bsm.mag, bsm.mag_neutral, bsm.mag);
/* compute mag error readings */
static VEC *mag_error = VNULL;
mag_error = v_resize(mag_error, AXIS_NB);
/* add hard iron now ? */
mag_error = v_zero(mag_error);
/* add a gaussian noise */
mag_error = v_add_gaussian_noise(mag_error, bsm.mag_noise_std_dev, mag_error);
mag_error->ve[AXIS_X] = mag_error->ve[AXIS_X] * bsm.mag_sensitivity->me[AXIS_X][AXIS_X];
mag_error->ve[AXIS_Y] = mag_error->ve[AXIS_Y] * bsm.mag_sensitivity->me[AXIS_Y][AXIS_Y];
mag_error->ve[AXIS_Z] = mag_error->ve[AXIS_Z] * bsm.mag_sensitivity->me[AXIS_Z][AXIS_Z];
/* add error */
v_add(bsm.mag, mag_error, bsm.mag);
// printf("h body %f %f %f\n", h_body->ve[AXIS_X], h_body->ve[AXIS_Y], h_body->ve[AXIS_Z]);
// printf("mag %f %f %f\n", bsm.mag->ve[AXIS_X], bsm.mag->ve[AXIS_Y], bsm.mag->ve[AXIS_Z]);
/* round signal to account for adc discretisation */
RoundSensor(bsm.mag);
bsm.mag_next_update += BSM_MAG_DT;
bsm.mag_available = TRUE;
}
list<vector<Volume> > Node::GetActions(const vector<Volume>& capacities) const
{
auto dimention = volumes.size();
auto d_nodes = vector<const Node*>(dimention);
list<vector<Volume> > result;
for (auto d = 0; d < dimention; d++)
{
vector<Volume> v_zero(volumes);
vector<Volume> v_max(volumes);
v_zero[d] = 0;
v_max[d] = capacities[d];
if (volumes[d] != v_zero[d]) result.push_back(v_zero);
if (volumes[d] != v_max[d]) result.push_back(v_max);
}
for (auto d1 = 0; d1 < dimention; d1++)
{
for (auto d2 = 0; d2 < dimention; d2++)
{
if (d1 == d2) continue;
auto vd1 = volumes[d1] + volumes[d2] - capacities[d2];
auto vd2 = capacities[d2];
if (vd1 < 0)
{
vd2 += vd1;
vd1 = 0;
}
if (volumes[d1] != vd1 && volumes[d2] != vd2)
{
vector<Volume> v_swap(volumes);
v_swap[d1] = vd1;
v_swap[d2] = vd2;
result.push_back(v_swap);
}
}
}
return result;
}
MAT *m_inverse(const MAT *A, MAT *out)
#endif
{
int i;
STATIC VEC *tmp = VNULL, *tmp2 = VNULL;
STATIC MAT *A_cp = MNULL;
STATIC PERM *pivot = PNULL;
if ( ! A )
error(E_NULL,"m_inverse");
if ( A->m != A->n )
error(E_SQUARE,"m_inverse");
if ( ! out || out->m < A->m || out->n < A->n )
out = m_resize(out,A->m,A->n);
A_cp = m_resize(A_cp,A->m,A->n);
A_cp = m_copy(A,A_cp);
tmp = v_resize(tmp,A->m);
tmp2 = v_resize(tmp2,A->m);
pivot = px_resize(pivot,A->m);
MEM_STAT_REG(A_cp,TYPE_MAT);
MEM_STAT_REG(tmp, TYPE_VEC);
MEM_STAT_REG(tmp2,TYPE_VEC);
MEM_STAT_REG(pivot,TYPE_PERM);
tracecatch(LUfactor(A_cp,pivot),"m_inverse");
for ( i = 0; i < A->n; i++ )
{
v_zero(tmp);
tmp->ve[i] = 1.0;
tracecatch(LUsolve(A_cp,pivot,tmp,tmp2),"m_inverse");
set_col(out,i,tmp2);
}
#ifdef THREADSAFE
V_FREE(tmp); V_FREE(tmp2);
M_FREE(A_cp); PX_FREE(pivot);
#endif
return out;
}
/* v_pconv -- computes a periodic convolution product
-- the period is the dimension of x2 */
VEC *v_pconv(VEC *x1, VEC *x2, VEC *out)
{
int i;
if ( ! x1 || ! x2 )
error(E_NULL,"v_pconv");
if ( x1 == out || x2 == out )
error(E_INSITU,"v_pconv");
out = v_resize(out,x2->dim);
if ( x2->dim == 0 )
return out;
v_zero(out);
for ( i = 0; i < x1->dim; i++ )
{
__mltadd__(&(out->ve[i]),x2->ve,x1->ve[i],x2->dim - i);
if ( i > 0 )
__mltadd__(out->ve,&(x2->ve[x2->dim - i]),x1->ve[i],i);
}
return out;
}
//--------------------------------------------------------------------------
void Omu_Integrator::init_stage(int k,
const Omu_VariableVec &x,
const Omu_VariableVec &u,
const Omu_DependentVec &Ft,
bool sa)
{
if (k >= _K) {
m_error(E_INTERN, "Omu_Integrator::init_stage"
" that was called with wrong integrator setup");
}
// initialize dimensions
init_dims(k, x, u, Ft);
_sa = sa;
if ((int)(_Fcs[k]->dim) != _n) {
m_error(E_INTERN, "Omu_Integrator::solve"
" that was called with wrong integrator setup of stage");
}
// initialize call arguments for sys->continuous
_ut.resize(_nu);
_dxt.resize(_nxt, _nx, _nu);
v_zero(_dxt); // zero time derivative of discrete states
// initialize call arguments for high-level integrator interface
_xc.resize(_n, 0, 0, _nq);
_dxc.resize(_n, 0, 0, _nq);
_q.resize(_nq);
// call high-level init
init(k, _xc, _q, _Fcs[k], sa);
// call depreciated init_stage
init_stage(k, (Omu_SVarVec &)x, u, sa);
}
/* sv_mlt -- scalar-vector multiply -- may be in-situ */
VEC *sv_mlt(double scalar,VEC *vector,VEC *out)
{
/* u_int dim, i; */
/* Real *out_ve, *vec_ve; */
if ( vector==(VEC *)NULL )
error(E_NULL,"sv_mlt");
if ( out==(VEC *)NULL || out->dim != vector->dim )
out = v_resize(out,vector->dim);
if ( scalar == 0.0 )
return v_zero(out);
if ( scalar == 1.0 )
return v_copy(vector,out);
__smlt__(vector->ve,(double)scalar,out->ve,(int)(vector->dim));
/**************************************************
dim = vector->dim;
out_ve = out->ve; vec_ve = vector->ve;
for ( i=0; i<dim; i++ )
out->ve[i] = scalar*vector->ve[i];
(*out_ve++) = scalar*(*vec_ve++);
**************************************************/
return (out);
}
void
PercussiveAudioCurve::reset(){v_zero(&m_prevMag[0], m_fftSize/2 + 1);}
int main(int argc, char *argv[])
{
#include "setRootCase.H"
#include "createTime.H"
#include "createMesh.H"
#include "createFields.H"
#include "createTimeControls.H"
#include "createRDeltaT.H"
turbulence->validate();
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#include "readFluxScheme.H"
dimensionedScalar v_zero("v_zero", dimVolume/dimTime, 0.0);
// Courant numbers used to adjust the time-step
scalar CoNum = 0.0;
scalar meanCoNum = 0.0;
Info<< "\nStarting time loop\n" << endl;
while (runTime.run())
{
// --- Directed interpolation of primitive fields onto faces
surfaceScalarField rho_pos(interpolate(rho, pos));
surfaceScalarField rho_neg(interpolate(rho, neg));
surfaceVectorField rhoU_pos(interpolate(rhoU, pos, U.name()));
surfaceVectorField rhoU_neg(interpolate(rhoU, neg, U.name()));
volScalarField rPsi("rPsi", 1.0/psi);
surfaceScalarField rPsi_pos(interpolate(rPsi, pos, T.name()));
surfaceScalarField rPsi_neg(interpolate(rPsi, neg, T.name()));
surfaceScalarField e_pos(interpolate(e, pos, T.name()));
surfaceScalarField e_neg(interpolate(e, neg, T.name()));
surfaceVectorField U_pos("U_pos", rhoU_pos/rho_pos);
surfaceVectorField U_neg("U_neg", rhoU_neg/rho_neg);
surfaceScalarField p_pos("p_pos", rho_pos*rPsi_pos);
surfaceScalarField p_neg("p_neg", rho_neg*rPsi_neg);
surfaceScalarField phiv_pos("phiv_pos", U_pos & mesh.Sf());
surfaceScalarField phiv_neg("phiv_neg", U_neg & mesh.Sf());
volScalarField c("c", sqrt(thermo.Cp()/thermo.Cv()*rPsi));
surfaceScalarField cSf_pos
(
"cSf_pos",
interpolate(c, pos, T.name())*mesh.magSf()
);
surfaceScalarField cSf_neg
(
"cSf_neg",
interpolate(c, neg, T.name())*mesh.magSf()
);
surfaceScalarField ap
(
"ap",
max(max(phiv_pos + cSf_pos, phiv_neg + cSf_neg), v_zero)
);
surfaceScalarField am
(
"am",
min(min(phiv_pos - cSf_pos, phiv_neg - cSf_neg), v_zero)
);
surfaceScalarField a_pos("a_pos", ap/(ap - am));
surfaceScalarField amaxSf("amaxSf", max(mag(am), mag(ap)));
surfaceScalarField aSf("aSf", am*a_pos);
if (fluxScheme == "Tadmor")
{
aSf = -0.5*amaxSf;
a_pos = 0.5;
}
surfaceScalarField a_neg("a_neg", 1.0 - a_pos);
phiv_pos *= a_pos;
phiv_neg *= a_neg;
surfaceScalarField aphiv_pos("aphiv_pos", phiv_pos - aSf);
surfaceScalarField aphiv_neg("aphiv_neg", phiv_neg + aSf);
// Reuse amaxSf for the maximum positive and negative fluxes
// estimated by the central scheme
amaxSf = max(mag(aphiv_pos), mag(aphiv_neg));
#include "centralCourantNo.H"
#include "readTimeControls.H"
if (LTS)
{
#include "setRDeltaT.H"
}
else
{
#include "setDeltaT.H"
}
runTime++;
Info<< "Time = " << runTime.timeName() << nl << endl;
phi = aphiv_pos*rho_pos + aphiv_neg*rho_neg;
surfaceVectorField phiUp
(
(aphiv_pos*rhoU_pos + aphiv_neg*rhoU_neg)
+ (a_pos*p_pos + a_neg*p_neg)*mesh.Sf()
);
surfaceScalarField phiEp
(
"phiEp",
aphiv_pos*(rho_pos*(e_pos + 0.5*magSqr(U_pos)) + p_pos)
+ aphiv_neg*(rho_neg*(e_neg + 0.5*magSqr(U_neg)) + p_neg)
+ aSf*p_pos - aSf*p_neg
);
volScalarField muEff("muEff", turbulence->muEff());
volTensorField tauMC("tauMC", muEff*dev2(Foam::T(fvc::grad(U))));
// --- Solve density
solve(fvm::ddt(rho) + fvc::div(phi));
// --- Solve momentum
solve(fvm::ddt(rhoU) + fvc::div(phiUp));
U.dimensionedInternalField() =
rhoU.dimensionedInternalField()
/rho.dimensionedInternalField();
U.correctBoundaryConditions();
rhoU.boundaryField() == rho.boundaryField()*U.boundaryField();
if (!inviscid)
{
solve
(
fvm::ddt(rho, U) - fvc::ddt(rho, U)
- fvm::laplacian(muEff, U)
- fvc::div(tauMC)
);
rhoU = rho*U;
}
// --- Solve energy
surfaceScalarField sigmaDotU
(
"sigmaDotU",
(
fvc::interpolate(muEff)*mesh.magSf()*fvc::snGrad(U)
+ (mesh.Sf() & fvc::interpolate(tauMC))
)
& (a_pos*U_pos + a_neg*U_neg)
);
solve
(
fvm::ddt(rhoE)
+ fvc::div(phiEp)
- fvc::div(sigmaDotU)
);
e = rhoE/rho - 0.5*magSqr(U);
e.correctBoundaryConditions();
thermo.correct();
rhoE.boundaryField() ==
rho.boundaryField()*
(
e.boundaryField() + 0.5*magSqr(U.boundaryField())
);
if (!inviscid)
{
solve
(
fvm::ddt(rho, e) - fvc::ddt(rho, e)
- fvm::laplacian(turbulence->alphaEff(), e)
);
thermo.correct();
rhoE = rho*(e + 0.5*magSqr(U));
}
p.dimensionedInternalField() =
rho.dimensionedInternalField()
/psi.dimensionedInternalField();
p.correctBoundaryConditions();
rho.boundaryField() == psi.boundaryField()*p.boundaryField();
turbulence->correct();
runTime.write();
Info<< "ExecutionTime = " << runTime.elapsedCpuTime() << " s"
<< " ClockTime = " << runTime.elapsedClockTime() << " s"
<< nl << endl;
}
Info<< "End\n" << endl;
return 0;
}
VEC *iter_gmres(ITER *ip)
#endif
{
STATIC VEC *u=VNULL, *r=VNULL, *rhs = VNULL;
STATIC VEC *givs=VNULL, *givc=VNULL, *z = VNULL;
STATIC MAT *Q = MNULL, *R = MNULL;
VEC *rr, v, v1; /* additional pointers (not real vectors) */
int i,j, done;
Real nres;
/* Real last_h; */
if (ip == INULL)
error(E_NULL,"iter_gmres");
if ( ! ip->Ax || ! ip->b )
error(E_NULL,"iter_gmres");
if ( ! ip->stop_crit )
error(E_NULL,"iter_gmres");
if ( ip->k <= 0 )
error(E_BOUNDS,"iter_gmres");
if (ip->x != VNULL && ip->x->dim != ip->b->dim)
error(E_SIZES,"iter_gmres");
if (ip->eps <= 0.0) ip->eps = MACHEPS;
r = v_resize(r,ip->k+1);
u = v_resize(u,ip->b->dim);
rhs = v_resize(rhs,ip->k+1);
givs = v_resize(givs,ip->k); /* Givens rotations */
givc = v_resize(givc,ip->k);
MEM_STAT_REG(r,TYPE_VEC);
MEM_STAT_REG(u,TYPE_VEC);
MEM_STAT_REG(rhs,TYPE_VEC);
MEM_STAT_REG(givs,TYPE_VEC);
MEM_STAT_REG(givc,TYPE_VEC);
R = m_resize(R,ip->k+1,ip->k);
Q = m_resize(Q,ip->k,ip->b->dim);
MEM_STAT_REG(R,TYPE_MAT);
MEM_STAT_REG(Q,TYPE_MAT);
if (ip->x == VNULL) { /* ip->x == 0 */
ip->x = v_get(ip->b->dim);
ip->shared_x = FALSE;
}
v.dim = v.max_dim = ip->b->dim; /* v and v1 are pointers to rows */
v1.dim = v1.max_dim = ip->b->dim; /* of matrix Q */
if (ip->Bx != (Fun_Ax)NULL) { /* if precondition is defined */
z = v_resize(z,ip->b->dim);
MEM_STAT_REG(z,TYPE_VEC);
}
done = FALSE;
for (ip->steps = 0; ip->steps < ip->limit; ) {
/* restart */
ip->Ax(ip->A_par,ip->x,u); /* u = A*x */
v_sub(ip->b,u,u); /* u = b - A*x */
rr = u; /* rr is a pointer only */
if (ip->Bx) {
(ip->Bx)(ip->B_par,u,z); /* tmp = B*(b-A*x) */
rr = z;
}
nres = v_norm2(rr);
if (ip->steps == 0) {
if (ip->info) ip->info(ip,nres,VNULL,VNULL);
ip->init_res = nres;
}
if ( nres == 0.0 ) {
done = TRUE;
break;
}
v.ve = Q->me[0];
sv_mlt(1.0/nres,rr,&v);
v_zero(r);
v_zero(rhs);
rhs->ve[0] = nres;
for ( i = 0; i < ip->k && ip->steps < ip->limit; i++ ) {
ip->steps++;
v.ve = Q->me[i];
(ip->Ax)(ip->A_par,&v,u);
rr = u;
if (ip->Bx) {
(ip->Bx)(ip->B_par,u,z);
rr = z;
}
if (i < ip->k - 1) {
v1.ve = Q->me[i+1];
v_copy(rr,&v1);
for (j = 0; j <= i; j++) {
v.ve = Q->me[j];
/* r->ve[j] = in_prod(&v,rr); */
/* modified Gram-Schmidt algorithm */
r->ve[j] = in_prod(&v,&v1);
v_mltadd(&v1,&v,-r->ve[j],&v1);
}
r->ve[i+1] = nres = v_norm2(&v1);
if (nres <= MACHEPS*ip->init_res) {
for (j = 0; j < i; j++)
rot_vec(r,j,j+1,givc->ve[j],givs->ve[j],r);
set_col(R,i,r);
done = TRUE;
break;
}
sv_mlt(1.0/nres,&v1,&v1);
}
else { /* i == ip->k - 1 */
/* Q->me[ip->k] need not be computed */
for (j = 0; j <= i; j++) {
v.ve = Q->me[j];
r->ve[j] = in_prod(&v,rr);
}
nres = in_prod(rr,rr) - in_prod(r,r);
if (sqrt(fabs(nres)) <= MACHEPS*ip->init_res) {
for (j = 0; j < i; j++)
rot_vec(r,j,j+1,givc->ve[j],givs->ve[j],r);
set_col(R,i,r);
done = TRUE;
break;
}
if (nres < 0.0) { /* do restart */
i--;
ip->steps--;
break;
}
r->ve[i+1] = sqrt(nres);
}
/* QR update */
/* last_h = r->ve[i+1]; */ /* for test only */
for (j = 0; j < i; j++)
rot_vec(r,j,j+1,givc->ve[j],givs->ve[j],r);
givens(r->ve[i],r->ve[i+1],&givc->ve[i],&givs->ve[i]);
rot_vec(r,i,i+1,givc->ve[i],givs->ve[i],r);
rot_vec(rhs,i,i+1,givc->ve[i],givs->ve[i],rhs);
set_col(R,i,r);
nres = fabs((double) rhs->ve[i+1]);
if (ip->info) ip->info(ip,nres,VNULL,VNULL);
if ( ip->stop_crit(ip,nres,VNULL,VNULL) ) {
done = TRUE;
break;
}
}
/* use ixi submatrix of R */
if (i >= ip->k) i = ip->k - 1;
R = m_resize(R,i+1,i+1);
rhs = v_resize(rhs,i+1);
/* test only */
/* test_gmres(ip,i,Q,R,givc,givs,last_h); */
Usolve(R,rhs,rhs,0.0); /* solve a system: R*x = rhs */
/* new approximation */
for (j = 0; j <= i; j++) {
v.ve = Q->me[j];
v_mltadd(ip->x,&v,rhs->ve[j],ip->x);
}
if (done) break;
/* back to old dimensions */
rhs = v_resize(rhs,ip->k+1);
R = m_resize(R,ip->k+1,ip->k);
}
#ifdef THREADSAFE
V_FREE(u);
V_FREE(r);
V_FREE(rhs);
V_FREE(givs);
V_FREE(givc);
V_FREE(z);
M_FREE(Q);
M_FREE(R);
#endif
return ip->x;
}
VEC *iter_cgs(ITER *ip, VEC *r0)
#endif
{
STATIC VEC *p = VNULL, *q = VNULL, *r = VNULL, *u = VNULL;
STATIC VEC *v = VNULL, *z = VNULL;
VEC *tmp;
Real alpha, beta, nres, rho, old_rho, sigma, inner;
if (ip == INULL)
error(E_NULL,"iter_cgs");
if (!ip->Ax || !ip->b || !r0)
error(E_NULL,"iter_cgs");
if ( ip->x == ip->b )
error(E_INSITU,"iter_cgs");
if (!ip->stop_crit)
error(E_NULL,"iter_cgs");
if ( r0->dim != ip->b->dim )
error(E_SIZES,"iter_cgs");
if ( ip->eps <= 0.0 ) ip->eps = MACHEPS;
p = v_resize(p,ip->b->dim);
q = v_resize(q,ip->b->dim);
r = v_resize(r,ip->b->dim);
u = v_resize(u,ip->b->dim);
v = v_resize(v,ip->b->dim);
MEM_STAT_REG(p,TYPE_VEC);
MEM_STAT_REG(q,TYPE_VEC);
MEM_STAT_REG(r,TYPE_VEC);
MEM_STAT_REG(u,TYPE_VEC);
MEM_STAT_REG(v,TYPE_VEC);
if (ip->Bx) {
z = v_resize(z,ip->b->dim);
MEM_STAT_REG(z,TYPE_VEC);
}
if (ip->x != VNULL) {
if (ip->x->dim != ip->b->dim)
error(E_SIZES,"iter_cgs");
ip->Ax(ip->A_par,ip->x,v); /* v = A*x */
if (ip->Bx) {
v_sub(ip->b,v,v); /* v = b - A*x */
(ip->Bx)(ip->B_par,v,r); /* r = B*(b-A*x) */
}
else v_sub(ip->b,v,r); /* r = b-A*x */
}
else { /* ip->x == 0 */
ip->x = v_get(ip->b->dim); /* x == 0 */
ip->shared_x = FALSE;
if (ip->Bx) (ip->Bx)(ip->B_par,ip->b,r); /* r = B*b */
else v_copy(ip->b,r); /* r = b */
}
v_zero(p);
v_zero(q);
old_rho = 1.0;
for (ip->steps = 0; ip->steps <= ip->limit; ip->steps++) {
inner = in_prod(r,r);
nres = sqrt(fabs(inner));
if (ip->steps == 0) ip->init_res = nres;
if (ip->info) ip->info(ip,nres,r,VNULL);
if ( ip->stop_crit(ip,nres,r,VNULL) ) break;
rho = in_prod(r0,r);
if ( old_rho == 0.0 )
error(E_BREAKDOWN,"iter_cgs");
beta = rho/old_rho;
v_mltadd(r,q,beta,u);
v_mltadd(q,p,beta,v);
v_mltadd(u,v,beta,p);
(ip->Ax)(ip->A_par,p,q);
if (ip->Bx) {
(ip->Bx)(ip->B_par,q,z);
tmp = z;
}
else tmp = q;
sigma = in_prod(r0,tmp);
if ( sigma == 0.0 )
error(E_BREAKDOWN,"iter_cgs");
alpha = rho/sigma;
v_mltadd(u,tmp,-alpha,q);
v_add(u,q,v);
(ip->Ax)(ip->A_par,v,u);
if (ip->Bx) {
(ip->Bx)(ip->B_par,u,z);
tmp = z;
}
else tmp = u;
v_mltadd(r,tmp,-alpha,r);
v_mltadd(ip->x,v,alpha,ip->x);
old_rho = rho;
}
#ifdef THREADSAFE
V_FREE(p);
V_FREE(q);
V_FREE(r);
V_FREE(u);
V_FREE(v);
V_FREE(z);
#endif
return ip->x;
}
MAT *iter_arnoldi(ITER *ip, Real *h_rem, MAT *Q, MAT *H)
#endif
{
STATIC VEC *u=VNULL, *r=VNULL;
VEC v; /* auxiliary vector */
int i,j;
Real h_val, c;
if (ip == INULL)
error(E_NULL,"iter_arnoldi");
if ( ! ip->Ax || ! Q || ! ip->x )
error(E_NULL,"iter_arnoldi");
if ( ip->k <= 0 )
error(E_BOUNDS,"iter_arnoldi");
if ( Q->n != ip->x->dim || Q->m != ip->k )
error(E_SIZES,"iter_arnoldi");
m_zero(Q);
H = m_resize(H,ip->k,ip->k);
m_zero(H);
u = v_resize(u,ip->x->dim);
r = v_resize(r,ip->k);
MEM_STAT_REG(u,TYPE_VEC);
MEM_STAT_REG(r,TYPE_VEC);
v.dim = v.max_dim = ip->x->dim;
c = v_norm2(ip->x);
if ( c <= 0.0)
return H;
else {
v.ve = Q->me[0];
sv_mlt(1.0/c,ip->x,&v);
}
v_zero(r);
for ( i = 0; i < ip->k; i++ )
{
v.ve = Q->me[i];
u = (ip->Ax)(ip->A_par,&v,u);
for (j = 0; j <= i; j++) {
v.ve = Q->me[j];
/* modified Gram-Schmidt */
r->ve[j] = in_prod(&v,u);
v_mltadd(u,&v,-r->ve[j],u);
}
h_val = v_norm2(u);
/* if u == 0 then we have an exact subspace */
if ( h_val <= 0.0 )
{
*h_rem = h_val;
return H;
}
set_col(H,i,r);
if ( i == ip->k-1 )
{
*h_rem = h_val;
continue;
}
/* H->me[i+1][i] = h_val; */
m_set_val(H,i+1,i,h_val);
v.ve = Q->me[i+1];
sv_mlt(1.0/h_val,u,&v);
}
#ifdef THREADSAFE
V_FREE(u);
V_FREE(r);
#endif
return H;
}
MAT *iter_arnoldi_iref(ITER *ip, Real *h_rem, MAT *Q, MAT *H)
#endif
{
STATIC VEC *u=VNULL, *r=VNULL, *s=VNULL, *tmp=VNULL;
VEC v; /* auxiliary vector */
int i,j;
Real h_val, c;
if (ip == INULL)
error(E_NULL,"iter_arnoldi_iref");
if ( ! ip->Ax || ! Q || ! ip->x )
error(E_NULL,"iter_arnoldi_iref");
if ( ip->k <= 0 )
error(E_BOUNDS,"iter_arnoldi_iref");
if ( Q->n != ip->x->dim || Q->m != ip->k )
error(E_SIZES,"iter_arnoldi_iref");
m_zero(Q);
H = m_resize(H,ip->k,ip->k);
m_zero(H);
u = v_resize(u,ip->x->dim);
r = v_resize(r,ip->k);
s = v_resize(s,ip->k);
tmp = v_resize(tmp,ip->x->dim);
MEM_STAT_REG(u,TYPE_VEC);
MEM_STAT_REG(r,TYPE_VEC);
MEM_STAT_REG(s,TYPE_VEC);
MEM_STAT_REG(tmp,TYPE_VEC);
v.dim = v.max_dim = ip->x->dim;
c = v_norm2(ip->x);
if ( c <= 0.0)
return H;
else {
v.ve = Q->me[0];
sv_mlt(1.0/c,ip->x,&v);
}
v_zero(r);
v_zero(s);
for ( i = 0; i < ip->k; i++ )
{
v.ve = Q->me[i];
u = (ip->Ax)(ip->A_par,&v,u);
for (j = 0; j <= i; j++) {
v.ve = Q->me[j];
/* modified Gram-Schmidt */
r->ve[j] = in_prod(&v,u);
v_mltadd(u,&v,-r->ve[j],u);
}
h_val = v_norm2(u);
/* if u == 0 then we have an exact subspace */
if ( h_val <= 0.0 )
{
*h_rem = h_val;
return H;
}
/* iterative refinement -- ensures near orthogonality */
do {
v_zero(tmp);
for (j = 0; j <= i; j++) {
v.ve = Q->me[j];
s->ve[j] = in_prod(&v,u);
v_mltadd(tmp,&v,s->ve[j],tmp);
}
v_sub(u,tmp,u);
v_add(r,s,r);
} while ( v_norm2(s) > 0.1*(h_val = v_norm2(u)) );
/* now that u is nearly orthogonal to Q, update H */
set_col(H,i,r);
/* check once again if h_val is zero */
if ( h_val <= 0.0 )
{
*h_rem = h_val;
return H;
}
if ( i == ip->k-1 )
{
*h_rem = h_val;
continue;
}
/* H->me[i+1][i] = h_val; */
m_set_val(H,i+1,i,h_val);
v.ve = Q->me[i+1];
sv_mlt(1.0/h_val,u,&v);
}
#ifdef THREADSAFE
V_FREE(u);
V_FREE(r);
V_FREE(s);
V_FREE(tmp);
#endif
return H;
}
/*
This routine performs ITMAX=5 Gauss-Seidel iterations to compute an
approximation to (P-inverse)*z, where P = I - gamma*Jd, and
Jd represents the diffusion contributions to the Jacobian.
The answer is stored in z on return, and x is a temporary vector.
The dimensions below assume a global constant NS >= ns.
Some inner loops of length ns are implemented with the small
vector kernels v_sum_prods, v_prod, v_inc_by_prod.
*/
static void GSIter(realtype gamma, N_Vector z, N_Vector x, WebData wdata)
{
int jx, jy, mx, my, x_loc, y_loc;
int ns, mxns, i, iyoff, ic, iter;
realtype beta[NS], beta2[NS], cof1[NS], gam[NS], gam2[NS];
realtype temp, *cox, *coy, *xd, *zd;
xd = NV_DATA_S(x);
zd = NV_DATA_S(z);
ns = wdata->ns;
mx = wdata->mx;
my = wdata->my;
mxns = wdata->mxns;
cox = wdata->cox;
coy = wdata->coy;
/* Write matrix as P = D - L - U.
Load local arrays beta, beta2, gam, gam2, and cof1. */
for (i = 0; i < ns; i++) {
temp = ONE/(ONE + RCONST(2.0)*gamma*(cox[i] + coy[i]));
beta[i] = gamma*cox[i]*temp;
beta2[i] = RCONST(2.0)*beta[i];
gam[i] = gamma*coy[i]*temp;
gam2[i] = RCONST(2.0)*gam[i];
cof1[i] = temp;
}
/* Begin iteration loop.
Load vector x with (D-inverse)*z for first iteration. */
for (jy = 0; jy < my; jy++) {
iyoff = mxns*jy;
for (jx = 0; jx < mx; jx++) {
ic = iyoff + ns*jx;
v_prod(xd+ic, cof1, zd+ic, ns); /* x[ic+i] = cof1[i]z[ic+i] */
}
}
N_VConst(ZERO, z);
/* Looping point for iterations. */
for (iter=1; iter <= ITMAX; iter++) {
/* Calculate (D-inverse)*U*x if not the first iteration. */
if (iter > 1) {
for (jy=0; jy < my; jy++) {
iyoff = mxns*jy;
for (jx=0; jx < mx; jx++) { /* order of loops matters */
ic = iyoff + ns*jx;
x_loc = (jx == 0) ? 0 : ((jx == mx-1) ? 2 : 1);
y_loc = (jy == 0) ? 0 : ((jy == my-1) ? 2 : 1);
switch (3*y_loc+x_loc) {
case 0 :
/* jx == 0, jy == 0 */
/* x[ic+i] = beta2[i]x[ic+ns+i] + gam2[i]x[ic+mxns+i] */
v_sum_prods(xd+ic, beta2, xd+ic+ns, gam2, xd+ic+mxns, ns);
break;
case 1 :
/* 1 <= jx <= mx-2, jy == 0 */
/* x[ic+i] = beta[i]x[ic+ns+i] + gam2[i]x[ic+mxns+i] */
v_sum_prods(xd+ic, beta, xd+ic+ns, gam2, xd+ic+mxns, ns);
break;
case 2 :
/* jx == mx-1, jy == 0 */
/* x[ic+i] = gam2[i]x[ic+mxns+i] */
v_prod(xd+ic, gam2, xd+ic+mxns, ns);
break;
case 3 :
/* jx == 0, 1 <= jy <= my-2 */
/* x[ic+i] = beta2[i]x[ic+ns+i] + gam[i]x[ic+mxns+i] */
v_sum_prods(xd+ic, beta2, xd+ic+ns, gam, xd+ic+mxns, ns);
break;
case 4 :
/* 1 <= jx <= mx-2, 1 <= jy <= my-2 */
/* x[ic+i] = beta[i]x[ic+ns+i] + gam[i]x[ic+mxns+i] */
v_sum_prods(xd+ic, beta, xd+ic+ns, gam, xd+ic+mxns, ns);
break;
case 5 :
/* jx == mx-1, 1 <= jy <= my-2 */
/* x[ic+i] = gam[i]x[ic+mxns+i] */
v_prod(xd+ic, gam, xd+ic+mxns, ns);
break;
case 6 :
/* jx == 0, jy == my-1 */
/* x[ic+i] = beta2[i]x[ic+ns+i] */
v_prod(xd+ic, beta2, xd+ic+ns, ns);
break;
case 7 :
/* 1 <= jx <= mx-2, jy == my-1 */
/* x[ic+i] = beta[i]x[ic+ns+i] */
v_prod(xd+ic, beta, xd+ic+ns, ns);
break;
case 8 :
/* jx == mx-1, jy == my-1 */
/* x[ic+i] = 0.0 */
v_zero(xd+ic, ns);
break;
}
}
}
} /* end if (iter > 1) */
/* Overwrite x with [(I - (D-inverse)*L)-inverse]*x. */
for (jy=0; jy < my; jy++) {
iyoff = mxns*jy;
for (jx=0; jx < mx; jx++) { /* order of loops matters */
ic = iyoff + ns*jx;
x_loc = (jx == 0) ? 0 : ((jx == mx-1) ? 2 : 1);
y_loc = (jy == 0) ? 0 : ((jy == my-1) ? 2 : 1);
switch (3*y_loc+x_loc) {
case 0 :
/* jx == 0, jy == 0 */
break;
case 1 :
/* 1 <= jx <= mx-2, jy == 0 */
/* x[ic+i] += beta[i]x[ic-ns+i] */
v_inc_by_prod(xd+ic, beta, xd+ic-ns, ns);
break;
case 2 :
/* jx == mx-1, jy == 0 */
/* x[ic+i] += beta2[i]x[ic-ns+i] */
v_inc_by_prod(xd+ic, beta2, xd+ic-ns, ns);
break;
case 3 :
/* jx == 0, 1 <= jy <= my-2 */
/* x[ic+i] += gam[i]x[ic-mxns+i] */
v_inc_by_prod(xd+ic, gam, xd+ic-mxns, ns);
break;
case 4 :
/* 1 <= jx <= mx-2, 1 <= jy <= my-2 */
/* x[ic+i] += beta[i]x[ic-ns+i] + gam[i]x[ic-mxns+i] */
v_inc_by_prod(xd+ic, beta, xd+ic-ns, ns);
v_inc_by_prod(xd+ic, gam, xd+ic-mxns, ns);
break;
case 5 :
/* jx == mx-1, 1 <= jy <= my-2 */
/* x[ic+i] += beta2[i]x[ic-ns+i] + gam[i]x[ic-mxns+i] */
v_inc_by_prod(xd+ic, beta2, xd+ic-ns, ns);
v_inc_by_prod(xd+ic, gam, xd+ic-mxns, ns);
break;
case 6 :
/* jx == 0, jy == my-1 */
/* x[ic+i] += gam2[i]x[ic-mxns+i] */
v_inc_by_prod(xd+ic, gam2, xd+ic-mxns, ns);
break;
case 7 :
/* 1 <= jx <= mx-2, jy == my-1 */
/* x[ic+i] += beta[i]x[ic-ns+i] + gam2[i]x[ic-mxns+i] */
v_inc_by_prod(xd+ic, beta, xd+ic-ns, ns);
v_inc_by_prod(xd+ic, gam2, xd+ic-mxns, ns);
break;
case 8 :
/* jx == mx-1, jy == my-1 */
/* x[ic+i] += beta2[i]x[ic-ns+i] + gam2[i]x[ic-mxns+i] */
v_inc_by_prod(xd+ic, beta2, xd+ic-ns, ns);
v_inc_by_prod(xd+ic, gam2, xd+ic-mxns, ns);
break;
}
}
}
/* Add increment x to z : z <- z+x */
N_VLinearSum(ONE, z, ONE, x, z);
}
}
int main(int argc, char *argv[])
{
#include "setRootCase.H"
#include "createTime.H"
#include "createDynamicFvMesh.H"
pimpleControl pimple(mesh);
#include "createFields.H"
#include "readTimeControls.H"
bool checkMeshCourantNo =
readBool(pimple.dict().lookup("checkMeshCourantNo"));
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#include "initContinuityErrs.H"
#include "readCourantType.H"
dimensionedScalar v_zero("v_zero", dimVolume/dimTime, 0.0);
Info<< "\nStarting time loop\n" << endl;
#include "createSurfaceFields.H"
#include "markBadQualityCells.H"
while (runTime.run())
{
#include "acousticCourantNo.H"
#include "compressibleCourantNo.H"
#include "readTimeControls.H"
#include "setDeltaT.H"
runTime++;
psi.oldTime();
rho.oldTime();
p.oldTime();
U.oldTime();
h.oldTime();
Info<< "Time = " << runTime.timeName() << nl << endl;
// --- Move mesh and update fluxes
{
// Do any mesh changes
mesh.update();
if (mesh.changing())
{
if (runTime.timeIndex() > 1)
{
surfaceScalarField amNew = min(min(phiv_pos - fvc::meshPhi(rho,U) - cSf_pos, phiv_neg - fvc::meshPhi(rho,U) - cSf_neg), v_zero);
phiNeg += kappa*(amNew - am)*p_neg*psi_neg;
phiPos += (1.0 - kappa)*(amNew - am)*p_neg*psi_neg;
}
else
{
phiNeg -= fvc::meshPhi(rho,U) * fvc::interpolate(rho);
}
phi = phiPos + phiNeg;
if (checkMeshCourantNo)
{
#include "meshCourantNo.H"
}
#include "markBadQualityCells.H"
}
}
// --- Solve density
solve
(
fvm::ddt(rho) + fvc::div(phi)
);
Info<< "rhoEqn max/min : " << max(rho).value()
<< " " << min(rho).value() << endl;
// --- Solve momentum
#include "UEqn.H"
// --- Solve energy
#include "hEqn.H"
// --- Solve pressure (PISO)
{
while (pimple.correct())
{
#include "pEqnDyM.H"
}
#include "updateKappa.H"
}
// --- Solve turbulence
turbulence->correct();
Ek = 0.5*magSqr(U);
EkChange = fvc::ddt(rho,Ek) + fvc::div(phiPos,Ek) + fvc::div(phiNeg,Ek);
dpdt = fvc::ddt(p) - fvc::div(fvc::meshPhi(rho,U), p);
runTime.write();
Info<< "ExecutionTime = " << runTime.elapsedCpuTime() << " s"
<< " ClockTime = " << runTime.elapsedClockTime() << " s"
<< nl << endl;
}
Info<< "End\n" << endl;
return 0;
}
int main(int argc, char *argv[])
{
# include "setRootCase.H"
# include "createTime.H"
# include "createMesh.H"
# include "createFields.H"
# include "readThermophysicalProperties.H"
# include "readTimeControls.H"
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
# include "readFluxScheme.H"
dimensionedScalar v_zero("v_zero",dimVolume/dimTime, 0.0);
Info<< "\nStarting time loop\n" << endl;
while (runTime.run())
{
// --- upwind interpolation of primitive fields on faces
surfaceScalarField rho_pos =
fvc::interpolate(rho, pos, "reconstruct(rho)");
surfaceScalarField rho_neg =
fvc::interpolate(rho, neg, "reconstruct(rho)");
surfaceVectorField rhoU_pos =
fvc::interpolate(rhoU, pos, "reconstruct(U)");
surfaceVectorField rhoU_neg =
fvc::interpolate(rhoU, neg, "reconstruct(U)");
volScalarField rPsi = 1.0/psi;
surfaceScalarField rPsi_pos =
fvc::interpolate(rPsi, pos, "reconstruct(T)");
surfaceScalarField rPsi_neg =
fvc::interpolate(rPsi, neg, "reconstruct(T)");
surfaceScalarField h_pos =
fvc::interpolate(h, pos, "reconstruct(T)");
surfaceScalarField h_neg =
fvc::interpolate(h, neg, "reconstruct(T)");
surfaceVectorField U_pos = rhoU_pos/rho_pos;
surfaceVectorField U_neg = rhoU_neg/rho_neg;
surfaceScalarField p_pos = rho_pos*rPsi_pos;
surfaceScalarField p_neg = rho_neg*rPsi_neg;
surfaceScalarField phiv_pos = U_pos & mesh.Sf();
surfaceScalarField phiv_neg = U_neg & mesh.Sf();
volScalarField c = sqrt(thermo->Cp()/thermo->Cv()*rPsi);
surfaceScalarField cSf_pos = fvc::interpolate(c, pos, "reconstruct(T)")*mesh.magSf();
surfaceScalarField cSf_neg = fvc::interpolate(c, neg, "reconstruct(T)")*mesh.magSf();
surfaceScalarField ap = max(max(phiv_pos + cSf_pos, phiv_neg + cSf_neg), v_zero);
surfaceScalarField am = min(min(phiv_pos - cSf_pos, phiv_neg - cSf_neg), v_zero);
surfaceScalarField a_pos = ap/(ap - am);
surfaceScalarField amaxSf("amaxSf", max(mag(am), mag(ap)));
# include "compressibleCourantNo.H"
# include "readTimeControls.H"
# include "setDeltaT.H"
runTime++;
Info<< "Time = " << runTime.timeName() << nl << endl;
surfaceScalarField aSf = am*a_pos;
if (fluxScheme == "Tadmor")
{
aSf = -0.5*amaxSf;
a_pos = 0.5;
}
surfaceScalarField a_neg = (1.0 - a_pos);
phiv_pos *= a_pos;
phiv_neg *= a_neg;
surfaceScalarField aphiv_pos = phiv_pos - aSf;
surfaceScalarField aphiv_neg = phiv_neg + aSf;
surfaceScalarField phi("phi", aphiv_pos*rho_pos + aphiv_neg*rho_neg);
surfaceVectorField phiUp =
(aphiv_pos*rhoU_pos + aphiv_neg*rhoU_neg)
+ (a_pos*p_pos + a_neg*p_neg)*mesh.Sf();
surfaceScalarField phiEp =
aphiv_pos*rho_pos*(h_pos + 0.5*magSqr(U_pos))
+ aphiv_neg*rho_neg*(h_neg + 0.5*magSqr(U_neg))
+ aSf*p_pos - aSf*p_neg;
volTensorField tauMC("tauMC", mu*dev2(fvc::grad(U)().T()));
// --- Solve density
solve(fvm::ddt(rho) + fvc::div(phi));
// --- Solve momentum
solve(fvm::ddt(rhoU) + fvc::div(phiUp));
U.dimensionedInternalField() =
rhoU.dimensionedInternalField()
/rho.dimensionedInternalField();
U.correctBoundaryConditions();
rhoU.boundaryField() = rho.boundaryField()*U.boundaryField();
volScalarField rhoBydt(rho/runTime.deltaT());
if (!inviscid)
{
solve
(
fvm::ddt(rho, U) - fvc::ddt(rho, U)
- fvm::laplacian(mu, U)
- fvc::div(tauMC)
);
rhoU = rho*U;
}
// --- Solve energy
surfaceScalarField sigmaDotU =
(
(
fvc::interpolate(mu)*mesh.magSf()*fvc::snGrad(U)
+ (mesh.Sf() & fvc::interpolate(tauMC))
)
& (a_pos*U_pos + a_neg*U_neg)
);
solve
(
fvm::ddt(rhoE)
+ fvc::div(phiEp)
- fvc::div(sigmaDotU)
);
h = (rhoE + p)/rho - 0.5*magSqr(U);
h.correctBoundaryConditions();
thermo->correct();
rhoE.boundaryField() =
rho.boundaryField()*
(
h.boundaryField() + 0.5*magSqr(U.boundaryField())
)
- p.boundaryField();
if (!inviscid)
{
volScalarField k("k", thermo->Cp()*mu/Pr);
solve
(
fvm::ddt(rho, h) - fvc::ddt(rho, h)
- fvm::laplacian(thermo->alpha(), h)
+ fvc::laplacian(thermo->alpha(), h)
- fvc::laplacian(k, T)
);
thermo->correct();
rhoE = rho*(h + 0.5*magSqr(U)) - p;
}
p.dimensionedInternalField() =
rho.dimensionedInternalField()
/psi.dimensionedInternalField();
p.correctBoundaryConditions();
rho.boundaryField() = psi.boundaryField()*p.boundaryField();
runTime.write();
Info<< "ExecutionTime = " << runTime.elapsedCpuTime() << " s"
<< " ClockTime = " << runTime.elapsedClockTime() << " s"
<< nl << endl;
}
Info<< "End\n" << endl;
return(0);
}
