//------------------------------------------------------------------------------
double PD_OSP::calculateDilationTerm(const std::pair<int, int> &idCol)
{
const int pId = idCol.first;
const int col_i = idCol.second;
const double d_i = m_data(col_i, m_indexD);
vector<pair<int, vector<double>>> & PDconnections = m_particles.pdConnections(pId);
double r_i[3];
double r0_i[3];
for(int d=0; d<m_dim; d++)
{
r_i[d] = m_r(d, col_i);
r0_i[d] = m_r0(d, col_i);
}
const int nConnections = PDconnections.size();
double dr_ij[3];
double theta_i = 0;
for(int l_j=0; l_j<nConnections; l_j++)
{
auto &con = PDconnections[l_j];
if(con.second[m_indexConnected] <= 0.5)
continue;
const int id_j = con.first;
const int j = m_pIds[id_j];
const double d_j = m_data(j, m_indexD);
const double vol_j = m_data(j, m_indexVolume);
const double dr0 = con.second[m_indexDr0];
const double volumeScaling = con.second[m_indexVolumeScaling];
const double d_ij = 0.5*(d_i + d_j);
double dr2 = 0;
double A_ij = 0; // The lambda-factor
for(int d=0; d<m_dim; d++)
{
dr_ij[d] = m_r(d, j) - r_i[d];
dr2 += dr_ij[d]*dr_ij[d];
A_ij += dr_ij[d]*(m_r0(d, j) - r0_i[d]);
}
const double dr = sqrt(dr2);
A_ij /= (dr0*dr);
double ds = dr - dr0;
// To avoid roundoff errors
if (fabs(ds) < THRESHOLD)
ds = 0.0;
const double s = ds/dr0;
theta_i += d_ij*s*A_ij*vol_j*volumeScaling;
}
return m_delta*theta_i;
}
//------------------------------------------------------------------------------
double PD_LPS::calculatePotentialEnergyDensity(const std::pair<int, int> &idCol)
{
const int pId = idCol.first;
const int i = idCol.second;
const double m_i = m_data(i, m_iMass);
vector<pair<int, vector<double>>> & PDconnections = m_particles.pdConnections(pId);
double r_i[m_dim];
double r0_i[m_dim];
for(int d=0; d<m_dim; d++)
{
r_i[d] = m_r(d, i);
r0_i[d] = m_r0(d, i);
}
const int nConnections = PDconnections.size();
double dr_ij[m_dim];
double alpha = 0;
if(m_dim == 3)
alpha = 15*m_mu/m_i;
else
alpha = 8*m_mu/m_i;
double theta_i = this->computeDilation(idCol);
double W_i = 0;
for(int l_j=0; l_j<nConnections; l_j++) {
auto &con = PDconnections[l_j];
if(con.second[m_iConnected] <= 0.5)
continue;
const int id_j = con.first;
const int j = m_pIds[id_j];
const double vol_j = m_data(j, m_iVolume);
const double dr0 = con.second[m_iDr0];
const double volumeScaling = con.second[m_iVolumeScaling];
double dr2 = 0;
for(int d=0; d<m_dim; d++)
{
dr_ij[d] = m_r(d, j) - r_i[d];
dr2 += dr_ij[d]*dr_ij[d];
}
const double dr = sqrt(dr2);
double ds = dr - dr0;
const double extension_term = alpha*(pow(ds - theta_i*dr0/m_dim, 2));
W_i += (extension_term)*vol_j*volumeScaling;
}
W_i += m_k*(pow(theta_i, 2));
return 0.5* W_i;
}
int main(void)
{
//Hier wird alles initialisiert, dh Grundeinstellungen festgelegt
init_Motor(); //Motor einstellen
init_system_tick(); //System Tick einstellen
init_drehzahlsensor(); //Drehzahlsensor einstellen
init_leds(); //LEDs einstellen
init_hupe(); //Hupe einstellen
sei(); //Alle Interrupts einschalten
while(1) //Alles innerhalb der while Schleife wird immer wieder wiederholt
{
if (~PINB & 0x01) //Wenn der invertierte Wert des ersten Bits in PINB 1 ist, dann (also wenn der Button gedrückt wird)
{
led1(AN); //Mach die LEDs 1 und 3 an
led3(AN);
m_r(255,0); //Und die Motoren mit Vollgas (255) in 2 Richtungen
m_l(255,1);
_delay_ms(500); //Warte 500 ms
led1(AUS); //LEDs 1 und 3 aus, LEDs 2 und 4 an
led3(AUS);
led2(AN);
led4(AN);
m_r(255,1); //Richtungen ändern
m_l(255,0);
_delay_ms(500); //und wieder 500ms in die andere Richtung
m_r(0,0); //Motoren aus
m_l(0,0);
led2(AUS); //Leds aus
led4(AUS);
hupe(AN); //Hupe an
_delay_ms(1000); //1 s warten
hupe(AUS); //Hupe aus
_delay_ms(300); //300ms warten
hupe(AN); //Hupe an
_delay_ms(500); //warte 500 ms
hupe(AUS); //Hupe aus
} //wieder nach oben
}
}
//------------------------------------------------------------------------------
double PD_LPS::computeDilation(const std::pair<int, int> &idCol)
{
const int pId = idCol.first;
const int i = idCol.second;
const double m_i = m_data(i, m_iMass);
vector<pair<int, vector<double>>> & PDconnections = m_particles.pdConnections(pId);
const int nConnections = PDconnections.size();
double dr_ij[m_dim];
double theta_i = 0;
for(int l_j=0; l_j<nConnections; l_j++)
{
auto &con = PDconnections[l_j];
if(con.second[m_iConnected] <= 0.5)
continue;
const int id_j = con.first;
const int j = m_pIds[id_j];
const double vol_j = m_data(j, m_iVolume);
const double dr0 = con.second[m_iDr0];
const double volumeScaling = con.second[m_iVolumeScaling];
double dr2 = 0;
for(int d=0; d<m_dim; d++)
{
dr_ij[d] = m_r(d, j) - m_r(d, i);
dr2 += dr_ij[d]*dr_ij[d];
}
const double dr = sqrt(dr2);
const double ds = dr - dr0;
theta_i += dr0*ds*vol_j*volumeScaling;
}
return theta_i*m_dim/m_i;
}
void process_current() {
expr * curr = m_goal->form(m_idx);
expr_ref new_curr(m);
proof_ref new_pr(m);
if (!m_subst->empty()) {
m_r(curr, new_curr, new_pr);
}
else {
new_curr = curr;
if (m.proofs_enabled())
new_pr = m.mk_reflexivity(curr);
}
TRACE("shallow_context_simplifier_bug", tout << mk_ismt2_pp(curr, m) << "\n---->\n" << mk_ismt2_pp(new_curr, m) << "\n";);
DestructiveColPivQR<MatrixType, HouseholderStrorageType>& DestructiveColPivQR<MatrixType, HouseholderStrorageType>::compute()
{
Index rows = m_r.rows();
Index cols = m_r.cols();
Index size = m_r.diagonalSize();
assert(m_r.diagonalSize() > 0);
//TODO : handle this case ?
//m_hCoeffs.resize(size);
m_temp.resize(cols);
m_colsTranspositions.resize(cols);
m_colsIntTranspositions.resize(cols);
if (cols>1)
m_colsIntTranspositions = IntRowVectorType::LinSpaced(cols,0, cols-1);
else
m_colsIntTranspositions = IntRowVectorType::Zero(1); //TODO: treat this special case here. It just requires to check every output data is correctly initilized
Index number_of_transpositions = 0;
m_colSqNorms.resize(cols);
for(Index k = 0; k < cols; ++k)
m_colSqNorms.coeffRef(k) = m_r.col(k).squaredNorm();
//RealScalar threshold_helper = m_colSqNorms.maxCoeff() * internal::abs2(epsilon()) /rows;
// The threshold should be decided wrt. to the norm of ML, while this class only consider
// the norm of L -> TODO: add an initialization of threshold by EPS*norm(L) ... is it really
// necessary, 'cos it is time consuming.
RealScalar threshold_helper = internal::abs2(epsilon());
m_nonzero_pivots = size; // the generic case is that in which all pivots are nonzero (invertible case)
m_maxpivot = RealScalar(0);
for(Index k = 0; k < size; ++k)
{
// first, we look up in our table m_colSqNorms which column has the biggest squared norm
Index biggest_col_index;
RealScalar biggest_col_sq_norm = m_colSqNorms.tail(cols-k).maxCoeff(&biggest_col_index);
biggest_col_index += k;
// since our table m_colSqNorms accumulates imprecision at every step, we must now recompute
// the actual squared norm of the selected column.
// Note that not doing so does result in solve() sometimes returning inf/nan values
// when running the unit test with 1000 repetitions.
biggest_col_sq_norm = m_r.col(m_colsIntTranspositions[biggest_col_index]).tail(rows-k).squaredNorm();
// we store that back into our table: it can't hurt to correct our table.
m_colSqNorms.coeffRef(biggest_col_index) = biggest_col_sq_norm;
// if the current biggest column is smaller than epsilon times the initial biggest column,
// terminate to avoid generating nan/inf values.
// Note that here, if we test instead for "biggest == 0", we get a failure every 1000 (or so)
// repetitions of the unit test, with the result of solve() filled with large values of the order
// of 1/(size*epsilon).
if(biggest_col_sq_norm < threshold_helper * (rows-k))
{
m_nonzero_pivots = k;
m_hCoeffs.tail(size-k).setZero();
m_q.bottomRightCorner(rows-k,rows-k)
.template triangularView<StrictlyLower>()
.setZero();
break;
}
// apply the transposition to the columns
m_colsTranspositions.coeffRef(k) = biggest_col_index;
if(k != biggest_col_index) {
std::swap(m_colsIntTranspositions.coeffRef(k), m_colsIntTranspositions.coeffRef(biggest_col_index));
//std::swap(m_colsInvTranspositions.coeffRef(m_colsTranspositions[k]), m_colsInvTranspositions.coeffRef(m_colsTranspositions[biggest_col_index]));
//m_qr.col(k).swap(m_qr.col(biggest_col_index));
std::swap(m_colSqNorms.coeffRef(k), m_colSqNorms.coeffRef(biggest_col_index));
++number_of_transpositions;
}
// generate the householder vector, store it below the diagonal
RealScalar beta;
//m_qr.col(k).tail(rows-k).makeHouseholderInPlace(m_hCoeffs.coeffRef(k), beta);
VectorBlock<typename MatrixQType::ColXpr> colk = m_q.col(k).tail(rows-k-1);
m_r.col(m_colsIntTranspositions[k]).tail(rows-k).makeHouseholder(colk, m_hCoeffs.coeffRef(k), beta);
// apply the householder transformation to the diagonal coefficient
m_r.coeffRef(k,m_colsIntTranspositions[k]) = beta;
m_r.col(m_colsIntTranspositions[k]).tail(rows-k-1).setZero();
// remember the maximum absolute value of diagonal coefficients
if(internal::abs(beta) > m_maxpivot) m_maxpivot = internal::abs(beta);
// apply the householder transformation
for (Index l = k+1; l<cols; ++l)
m_r.col(m_colsIntTranspositions[l]).tail(rows-k)
.applyHouseholderOnTheLeft(m_q.col(k).tail(rows-k-1), m_hCoeffs.coeffRef(k), &m_temp.coeffRef(k+1));
//m_r.bottomRightCorner(rows-k, cols-k-1)
//.applyHouseholderOnTheLeft(m_q.col(k).tail(rows-k-1), m_hCoeffs.coeffRef(k), &m_temp.coeffRef(k+1));
// update our table of squared norms of the columns
for (Index l = k+1; l<cols; ++l)
m_colSqNorms[l] -= m_r(k,m_colsIntTranspositions[l])*m_r(k,m_colsIntTranspositions[l]);
// Nullify the lower-diag part of the column
//colk.tail(rows-k-1).setZero();
//m_colSqNorms.tail(cols-k-1) -= m_r.row(k).tail(cols-k-1).cwiseAbs2();
//std::cout << "R=" << std::endl << m_r << std::endl;
//std::cout << "norms=" << std::endl << m_colSqNorms << std::endl;
}
m_colsPermutation.setIdentity(cols);
for(Index k = 0; k < m_nonzero_pivots; ++k)
m_colsPermutation.applyTranspositionOnTheRight(k, m_colsTranspositions.coeff(k));
m_det_pq = (number_of_transpositions%2) ? -1 : 1;
m_isInitialized = true;
return *this;
}
//------------------------------------------------------------------------------
void PD_OSP::calculateForces(const std::pair<int, int> &idCol)
{
const int pId = idCol.first;
const int col_i = idCol.second;
const double a_i = m_data(col_i, m_indexA);
const double b_i = m_data(col_i, m_indexB);
const double d_i = m_data(col_i, m_indexD);
const double theta_i = m_data(col_i, m_indexTheta);
vector<pair<int, vector<double>>> & PDconnections = m_particles.pdConnections(pId);
double r_i[3];
double r0_i[3];
double f_i[3];
for(int d=0; d<m_dim; d++)
{
f_i[d] = 0;
r_i[d] = m_r(d, col_i);
r0_i[d] = m_r0(d, col_i);
}
const int nConnections = PDconnections.size();
double dr_ij[3];
double thetaNew = 0;
for(int l_j=0; l_j<nConnections; l_j++)
{
auto &con = PDconnections[l_j];
if(con.second[m_indexConnected] <= 0.5)
continue;
const int id_j = con.first;
const int j = m_pIds[id_j];
const double a_j = m_data(j, m_indexA);
const double b_j = m_data(j, m_indexB);
const double d_j = m_data(j, m_indexD);
const double theta_j = m_data(j, m_indexTheta);
const double vol_j = m_data(j, m_indexVolume);
const double dr0 = con.second[m_indexDr0];
const double volumeScaling = con.second[m_indexVolumeScaling];
const double a_ij = 0.5*(a_i + a_j);
const double b_ij = 0.5*(b_i + b_j);
const double d_ij = 0.5*(d_i + d_j);
const double Gd_ij = con.second[m_indexForceScalingDilation];
const double Gb_ij = con.second[m_indexForceScalingBond];
double dr2 = 0;
double A_ij = 0; // The lambda-factor
for(int d=0; d<m_dim; d++)
{
dr_ij[d] = m_r(d, j) - r_i[d];
dr2 += dr_ij[d]*dr_ij[d];
A_ij += dr_ij[d]*(m_r0(d, j) - r0_i[d]);
}
const double dr = sqrt(dr2);
A_ij /= (dr0*dr);
double ds = dr - dr0;
// To avoid roundoff errors
if (fabs(ds) < THRESHOLD)
ds = 0.0;
const double s = ds/dr0;
const double fbond = (a_ij*d_ij*Gd_ij*A_ij/dr0*(theta_i + theta_j) + b_ij*Gb_ij*s)
*vol_j*volumeScaling/dr;
for(int d=0; d<m_dim; d++)
{
f_i[d] += dr_ij[d]*fbond;
}
thetaNew += d_ij*s*A_ij*vol_j*volumeScaling;
con.second[m_indexStretch] = s;
}
for(int d=0; d<m_dim; d++)
{
m_F(d, col_i) += m_delta*f_i[d];
}
m_data(col_i, m_indexThetaNew) = m_delta*thetaNew;
}
//------------------------------------------------------------------------------
void PD_OSP::applySurfaceCorrection(double mu, double nu, int dim, double strain)
{
arma::vec3 strainFactor;
arma::mat gd = arma::zeros(m_particles.nParticles(), dim); // Dilation correction
arma::mat gb = arma::zeros(m_particles.nParticles(), dim); // Bond correction
//--------------------------------------------------------------------------
// Apllying correction to the dilation term
//--------------------------------------------------------------------------
// Stretching all particle in the x, y and z-direction
strainFactor(0) = strain;
strainFactor(1) = 0;
strainFactor(2) = 0;
for(int a=0; a<dim; a++)
{
if(a == 1)
strainFactor.swap_rows(0,1);
else if(a == 2)
strainFactor.swap_rows(1,2);
#ifdef USE_OPENMP
# pragma omp parallel for
#endif
// Applying uniaxial stretch
for(int i=0; i<m_particles.nParticles(); i++)
{
pair<int, int> idCol(i, i);
int col_i = idCol.second;
for(int d=0; d<dim; d++)
{
m_r(d, col_i) = (1 + strainFactor(d))*m_r(d, col_i);
}
}
double dilation_term = strain;
#ifdef USE_OPENMP
# pragma omp parallel for
#endif
// Calculating the elastic energy density
for(int i=0; i<m_particles.nParticles(); i++)
{
pair<int, int> idCol(i, i);
int col_i = idCol.second;
double theta_i = calculateDilationTerm(idCol);
double factor = dilation_term/theta_i;
gd(col_i, a) = factor;
}
#ifdef USE_OPENMP
# pragma omp parallel for
#endif
// Resetting the positions
for(int i=0; i<m_particles.nParticles(); i++)
{
pair<int, int> idCol(i, i);
int col_i = idCol.second;
for(int d=0; d<dim; d++)
{
m_r(d, col_i) = m_r(d, col_i)/(1 + strainFactor(d));
}
}
}
//--------------------------------------------------------------------------
// Applying correction to the bond term
//--------------------------------------------------------------------------
// Performing a simple shear of all particle in the x, y and z-direction
arma::ivec3 axis;
strainFactor(0) = 0.5*strain;
strainFactor(1) = 0.*strain;
strainFactor(2) = 0;
axis(0) = 1;
axis(1) = 0;
axis(2) = 0;
for(int a=0; a<dim; a++)
{
if(a == 1)
{
strainFactor.swap_rows(1,2);
strainFactor.swap_rows(0,1);
axis(0) = 0;
axis(1) = 2;
axis(2) = 1;
}
else if(a == 2)
{
strainFactor.swap_rows(2,0);
strainFactor.swap_rows(1,2);
axis(0) = 2;
axis(1) = 0;
axis(2) = 0;
}
//#ifdef USE_OPENMP
//# pragma omp parallel for
//#endif
// Applying uniaxial stretch
for(int i=0; i<m_particles.nParticles(); i++)
{
pair<int, int> idCol(i, i);
int col_i = idCol.second;
for(int d=0; d<dim; d++)
{
double shear = strainFactor(d)*m_r(axis(d), col_i);
m_r(d, col_i) = m_r(d, col_i) + shear;
// m_r(d, col_i) = m_r(d, col_i) + strainFactor(d)*m_r(axis(d), col_i);
// m_r(d, col_i) = (1 + strainFactor(d))*m_r(d, col_i);
}
}
double dilation_term = 0.5*mu*strain*strain;
//#ifdef USE_OPENMP
//# pragma omp parallel for
//#endif
// Calculating the elastic energy density
for(int i=0; i<m_particles.nParticles(); i++)
{
pair<int, int> idCol(i, i);
int col_i = idCol.second;
double bond_i = calculateBondPotential(idCol);
double factor = dilation_term/bond_i;
gb(col_i, a) = factor;
}
//#ifdef USE_OPENMP
//# pragma omp parallel for
//#endif
// Resetting the positions
for(int i=0; i<m_particles.nParticles(); i++)
{
pair<int, int> idCol(i, i);
int col_i = idCol.second;
for(int d=0; d<dim; d++)
{
m_r(d, col_i) = m_r(d, col_i) - strainFactor(d)*m_r(axis(d), col_i);
// m_r(d, col_i) = m_r(d, col_i)/(1 + strainFactor(d));
}
}
}
//--------------------------------------------------------------------------
// Calculating the scaling
//--------------------------------------------------------------------------
//#ifdef USE_OPENMP
//# pragma omp parallel for
//#endif
for(int i=0; i<m_particles.nParticles(); i++)
{
pair<int, int> idCol(i, i);
int pId = idCol.first;
int col_i = idCol.second;
vector<pair<int, vector<double>>> & PDconnections = m_particles.pdConnections(pId);
for(auto &con:PDconnections)
{
int id_j = con.first;
int col_j = m_pIds[id_j];
double dr0Len = con.second[m_indexDr0];
arma::vec3 n = (m_r.col(col_i) - m_r.col(col_j))/dr0Len;
arma::vec3 gd_mean;
arma::vec3 gb_mean;
double Gd = 0;
double Gb = 0;
for(int d=0; d<dim; d++)
{
gd_mean(d) = 0.5*(gd(col_i, d) + gd(col_j, d));
gb_mean(d) = 0.5*(gb(col_i, d) + gb(col_j, d));
Gd += pow(n(d)/gd_mean(d), 2);
Gb += pow(n(d)/gb_mean(d), 2);
}
Gd = pow(Gd, -0.5);
Gb = pow(Gb, -0.5);
con.second[m_indexForceScalingDilation] *= Gd;
con.second[m_indexForceScalingBond] *= Gb;
}
}
}
//------------------------------------------------------------------------------
void PD_LPS_porosity_adrmc::calculateForces(const int id, const int i) {
const double theta_i = m_data(i, m_iTheta);
const double m_i = m_data(i, m_iMass);
const double a_i = m_data(i, m_iA);
const double b_i = m_data(i, m_iA);
vector<pair<int, vector<double>>> &PDconnections =
m_particles.pdConnections(id);
const int nConnections = PDconnections.size();
double dr0_ij[m_dim];
double dr_ij[m_dim];
_F.zeros();
double thetaNew = 0;
int nConnected = 0;
//----------------------------------
// TMP - standard stress calc from
// m_data(i, m_indexStress[0]) = 0;
// m_data(i, m_indexStress[1]) = 0;
// m_data(i, m_indexStress[2]) = 0;
//----------------------------------
for (int l_j = 0; l_j < nConnections; l_j++) {
auto &con = PDconnections[l_j];
if (con.second[m_iConnected] <= 0.5)
continue;
const int id_j = con.first;
const int j = m_idToCol_v[id_j];
const double m_j = m_data(j, m_iMass);
const double theta_j = m_data(j, m_iTheta);
const double vol_j = m_data(j, m_iVolume);
const double dr0 = con.second[m_iDr0];
const double volumeScaling = con.second[m_iVolumeScaling];
const double vol = vol_j * volumeScaling;
const double w = weightFunction(dr0);
const double a_j = m_data(j, m_iA);
const double b_j = m_data(j, m_iB);
double dr2 = 0;
for (int d = 0; d < m_dim; d++) {
dr0_ij[d] = m_r0(j, d) - m_r0(i, d);
dr_ij[d] = m_r(j, d) - m_r(i, d);
dr2 += dr_ij[d] * dr_ij[d];
}
const double dr = sqrt(dr2);
const double ds = dr - dr0;
double bond = (b_i * theta_i / m_i + b_j * theta_j / m_j) * dr0;
bond += (a_i / m_i + a_j / m_j) * ds;
bond *= w * vol / dr;
thetaNew += w * dr0 * ds * vol;
for (int d = 0; d < m_dim; d++) {
m_F(i, d) += dr_ij[d] * bond;
for (int d2 = 0; d2 < m_dim; d2++) {
_F(d, d2) += w * dr_ij[d] * dr0_ij[d2] * vol;
}
}
con.second[m_iStretch] = ds / dr0;
//----------------------------------
// TMP - standard stres calc from
// m_data(i, m_indexStress[0]) += 0.5*dr_ij[0]*dr_ij[0]*bond;
// m_data(i, m_indexStress[1]) += 0.5*dr_ij[1]*dr_ij[1]*bond;
// m_data(i, m_indexStress[2]) += 0.5*dr_ij[0]*dr_ij[1]*bond;
//----------------------------------
nConnected++;
}
if (nConnections <= 3) {
m_data(i, m_iThetaNew) = 0;
} else {
m_data(i, m_iThetaNew) = m_dim / m_i * thetaNew;
}
//----------------------------------
// TMP - standard stres calc from
//----------------------------------
// if(nConnected > 5) {
// computeStress(id, i, nConnected);
// }
computeStress(id, i, nConnected);
//--------------------
m_continueState = false;
}
//------------------------------------------------------------------------------
void PD_LPS::calculateForces(const std::pair<int, int> &idCol)
{
const int pId = idCol.first;
const int i = idCol.second;
const double theta_i = m_data(i, m_iTheta);
const double m_i = m_data(i, m_iMass);
double alpha;
if(m_dim == 3)
alpha = 15*m_mu;
else
alpha = 8*m_mu;
const double c = (3*m_k - 5*m_mu);
vector<pair<int, vector<double>>> & PDconnections = m_particles.pdConnections(pId);
double r_i[m_dim];
double r0_i[m_dim];
double f_i[m_dim];
for(int d=0; d<m_dim; d++)
{
f_i[d] = 0;
r_i[d] = m_r(d, i);
r0_i[d] = m_r0(d, i);
}
const int nConnections = PDconnections.size();
double dr_ij[m_dim];
double thetaNew = 0;
for(int l_j=0; l_j<nConnections; l_j++)
{
auto &con = PDconnections[l_j];
if(con.second[m_iConnected] <= 0.5)
continue;
const int id_j = con.first;
const int j = m_pIds[id_j];
const double m_j = m_data(j, m_iMass);
const double theta_j = m_data(j, m_iTheta);
const double vol_j = m_data(j, m_iVolume);
const double dr0 = con.second[m_iDr0];
const double volumeScaling = con.second[m_iVolumeScaling];
const double gb_ij = con.second[m_iForceScalingBond];
const double gd_ij = con.second[m_iForceScalingDilation];
double dr2 = 0;
for(int d=0; d<m_dim; d++)
{
dr_ij[d] = m_r(d, j) - r_i[d];
dr2 += dr_ij[d]*dr_ij[d];
}
const double dr = sqrt(dr2);
const double ds = dr - dr0;
double bond = gd_ij*c*(theta_i/m_i + theta_j/m_j)*dr0;
bond += gb_ij*alpha*(1./m_i + 1./m_j)*ds;
bond *= vol_j*volumeScaling/dr;
thetaNew += dr0*ds*vol_j*volumeScaling;
for(int d=0; d<m_dim; d++)
{
m_F(d, i) += dr_ij[d]*bond;
}
con.second[m_iStretch] = ds/dr0;
}
m_data(i, m_iThetaNew) = m_dim/m_i*thetaNew;
}
//------------------------------------------------------------------------------
void PD_LPS::calculateStress(const std::pair<int, int> &idCol, const int (&indexStress)[6])
{
const int pId = idCol.first;
const int i = idCol.second;
const double theta_i = m_data(i, m_iTheta);
const double m_i = m_data(i, m_iMass);
double beta = 0;
if(m_dim == 3)
beta = 15*m_mu;
else
beta = 8*m_mu;
const double alpha_i = beta/m_i;
vector<pair<int, vector<double>>> & PDconnections = m_particles.pdConnections(pId);
double r_i[3];
double r0_i[3];
for(int d=0; d<m_dim; d++)
{
r_i[d] = m_r(d, i);
r0_i[d] = m_r0(d, i);
}
const int nConnections = PDconnections.size();
double dr_ij[m_dim];
for(int l_j=0; l_j<nConnections; l_j++)
{
auto &con = PDconnections[l_j];
if(con.second[m_iConnected] <= 0.5)
continue;
const int id_j = con.first;
const int j = m_pIds[id_j];
const double m_j = m_data(j, m_iMass);
const double theta_j = m_data(j, m_iTheta);
const double vol_j = m_data(j, m_iVolume);
const double dr0 = con.second[m_iDr0];
const double volumeScaling = con.second[m_iVolumeScaling];
const double alpha_j = beta/m_j;
const double gb_ij = con.second[m_iForceScalingBond];
const double gd_ij = con.second[m_iForceScalingDilation];
double dr2 = 0;
for(int d=0; d<m_dim; d++)
{
dr_ij[d] = m_r(d, j) - r_i[d];
dr2 += dr_ij[d]*dr_ij[d];
}
const double dr = sqrt(dr2);
const double ds = dr - dr0;
double bond_ij = gd_ij*(3*m_k - 5*m_mu)*(theta_i/m_i + theta_j/m_j)*dr0;
bond_ij += gb_ij*(alpha_i + alpha_j)*ds;
bond_ij *= vol_j*volumeScaling/dr;
m_data(i, indexStress[0]) += 0.5*bond_ij*dr_ij[X]*dr_ij[X];
m_data(i, indexStress[1]) += 0.5*bond_ij*dr_ij[Y]*dr_ij[Y];
m_data(i, indexStress[3]) += 0.5*bond_ij*dr_ij[X]*dr_ij[Y];
if(m_dim == 3)
{
m_data(i, indexStress[2]) += 0.5*bond_ij*dr_ij[Z]*dr_ij[Z];
m_data(i, indexStress[4]) += 0.5*bond_ij*dr_ij[X]*dr_ij[Z];
m_data(i, indexStress[5]) += 0.5*bond_ij*dr_ij[Y]*dr_ij[Z];
}
}
}
