void solve_bicgstab(const MatrixExpression &A,
const PreconditionerExpression &preconditioner,
VectorTypeX &result,
const VectorExpressionB &b, double tol, unsigned max_iterations,
unsigned *iteration_count = NULL, unsigned debug_level = 0)
{
typedef
VectorTypeX
vector_t;
typedef
typename vector_t::value_type
v_t;
typedef
typename decomplexify<v_t>::type
real_t;
if (A().size1() != A().size2())
throw std::runtime_error("bicgstab: A is not quadratic");
if (debug_level >= 2)
std::cout << "rhs:" << b << std::endl;
// typed up from Figure 2.10 of
// Templates for the Solution of Linear Systems:
// Building Blocks for Iterative Methods
// (R. Barrett, M. Berry, T. F. Chan, et al.)
unsigned iterations = 0;
// "next" refers to i
// "" refers to i-1
// "last" refers to i-2
v_t rho, last_rho, alpha, omega;
vector_t p, v, x(result);
vector_t r(b-prod(A,x));
vector_t r_tilde(r);
real_t initial_residual = norm_2(r);
// silence "used uninitialized" warnings
last_rho = 0;
alpha = 0;
while (iterations < max_iterations)
{
rho = inner_prod(conj(r_tilde), r);
if (absolute_value(rho) == 0)
throw std::runtime_error("bicgstab failed, rho == 0");
if (iterations == 0)
{
p = r;
}
else
{
v_t beta = (rho/last_rho)*(alpha/omega);
p = r + beta*(p-omega*v);
}
vector_t p_hat = prod(preconditioner, p);
v = prod(A, p_hat);
alpha = rho/inner_prod(conj(r_tilde), v);
vector_t s = r - alpha*v;
{
real_t norm_s = norm_2(s);
if (norm_s < tol * initial_residual)
{
x += alpha*p_hat;
break;
}
}
vector_t s_hat = prod(preconditioner, s);
vector_t t = prod(A, s_hat);
omega = inner_prod(conj(t), s)/inner_prod(conj(t), t);
x += alpha * p_hat + omega * s_hat;
r = s - omega * t;
{
real_t norm_r = norm_2(r);
if (norm_r < tol * initial_residual)
break;
}
if (absolute_value(omega) == 0)
throw std::runtime_error("bicgstab failed, omega == 0");
last_rho = rho;
if (debug_level)
{
if (debug_level >= 2 || iterations % 10 == 0)
std::cout << double(norm_2(r)) << std::endl;
}
iterations++;
}
result = x;
if ( iterations == max_iterations)
throw std::runtime_error("bicgstab failed to converge");
if (iteration_count)
*iteration_count = iterations;
}
// this method loads the output file from the previous method and computes the activation
// time (defined as the time V becomes positive) for each node.
void ConvertToActivationMap(double h, double dt, bool useSvi)
{
//TetrahedralMesh<3,3> mesh1;
//double h1=0.01; // 0.01, 0.02, 0.05
//mesh1.ConstructRegularSlabMesh(h1, 2.0, 0.7, 0.3);
//MeshalyzerMeshWriter<3,3> writer("Mesh0.01", "mesh01");
//writer.WriteFilesUsingMesh(mesh1);
TetrahedralMesh<3,3> mesh;
double printing_dt=0.1;
mesh.ConstructRegularSlabMesh(h, 2.0, 0.7, 0.3);
std::stringstream input_dir;
input_dir << "Benchmark" << "_h" << h << "_dt" << dt;
Hdf5DataReader reader(input_dir.str(),"results");
unsigned num_timesteps = reader.GetUnlimitedDimensionValues().size();
DistributedVectorFactory factory(mesh.GetNumNodes());
Vec voltage = factory.CreateVec();
std::vector<double> activation_times(mesh.GetNumNodes(), -1.0);
std::vector<double> last_negative_voltage(mesh.GetNumNodes(), 1.0);
for(unsigned timestep=0; timestep<num_timesteps; timestep++)
{
reader.GetVariableOverNodes(voltage, "V", timestep);
ReplicatableVector voltage_repl(voltage);
for(unsigned i=0; i<mesh.GetNumNodes(); i++)
{
double V = voltage_repl[i];
if(V > 0 && activation_times[i] < 0.0)
{
double old = last_negative_voltage[i];
assert(old < 0);
activation_times[i] = (timestep-V/(V-old))*printing_dt;
}
else if (V<=0)
{
last_negative_voltage[i]=V;
}
}
}
OutputFileHandler handler("ActivationMaps", false);
if (PetscTools::AmMaster() == false)
{
return;
}
//Only master proceeds to write
c_vector<double, 3> top_corner;
top_corner[0] = 2.0;
top_corner[1] = 0.7;
top_corner[2] = 0.3;
c_vector<double, 3> unit_diagonal = top_corner/norm_2(top_corner);
std::stringstream output_file1;
output_file1 << "diagonal" << "_h" << h << "_dt" << dt;
if (useSvi)
{
output_file1 << "_svi.dat";
}
else
{
output_file1 << "_ici.dat";
}
out_stream p_diag_file = handler.OpenOutputFile(output_file1.str());
for(unsigned i=0; i<mesh.GetNumNodes(); i++)
{
c_vector<double, 3> position = mesh.GetNode(i)->rGetLocation();
c_vector<double, 3> projected_diagonal = unit_diagonal*inner_prod(unit_diagonal, position);
double off_diagonal = norm_2(position - projected_diagonal);
if (off_diagonal < h/3)
{
double distance = norm_2(position);
(*p_diag_file) << distance<<"\t"<< activation_times[i]<<"\t"<<off_diagonal<<"\n";
if( fabs(position[0]-2.0) < 1e-8)
{
std::cout << "h, dt = " << h << ", " << dt << "\n\t";
std::cout << "activation_times[" << i << "] = " << activation_times[i] << "\n";
}
}
}
p_diag_file->close();
std::stringstream output_file;
output_file << "activation" << "_h" << h << "_dt" << dt << ".dat";
out_stream p_file = handler.OpenOutputFile(output_file.str());
for(unsigned i=0; i<activation_times.size(); i++)
{
*p_file << activation_times[i] << "\n";
}
p_file->close();
for(unsigned i=0; i<activation_times.size(); i++)
{
if(activation_times[i] < 0.0)
{
std::cout << "\n\n\n**Some nodes unactivated**\n\n\n";
output_file << "__error";
out_stream p_file2 = handler.OpenOutputFile(output_file.str());
p_file2->close();
return;
}
}
}
void
SolverUnconstrained<Data,Problem>::CGstep( vector_type & _x, value_type _Delta, vector_type & _sCG,
value_type &norm_s_til,
int &_CGiter,
int &_n_restarts, int &_n_indef,
int &_n_crosses_def, int &_n_crosses_indef,
int &_n_truss_exit_def,
int &_n_truss_exit_indef,
value_type &_s_til_x_G_til_x_s_til,
value_type &phi_til )
{
bool _restart = false;
bool _done = false;
_n_restarts = 0;
_CGiter = 0;
_n_indef = 0;
_n_crosses_def = 0;
_n_crosses_indef = 0;
_n_truss_exit_def = 0;
_n_truss_exit_indef = 0;
f_type __fx;
vector_type _neg_grad_fx ( _x.size() );
vector_type _Tgrad_fx ( _x.size() );
vector_type _Htil_d ( _x.size() ); // Htil * dCG
vector_type _Htil_s_til ( _x.size() ); // Htil * s_til
vector_type _s_til ( _x.size() );
vector_type _s_til_old ( _x.size() );
vector_type _s_til_eps ( _x.size() );
vector_type _rCG ( _x.size() );
vector_type _rCG_old ( _x.size() );
vector_type _dCG ( _x.size() );
value_type _alpha = 0;
value_type _gamma; // d' Htil d
value_type _beta;
value_type _xi = 0;
value_type _tau;
// diagonal matrices
banded_matrix_type _Hg ( _x.size(), _x.size(), 0, 0 ); // Gtil
symmetric_matrix_type _Thess_fxT ( _E_nA, _E_nA ); // Btil
symmetric_matrix_type _Htil ( _E_nA, _E_nA ); // Htil
DVLOG(2) << "\n\n[value_type SolverUnconstrained<Data,Problem>::CGstep]...\n";
// INITIALIZE:
this->makeCauchyStep( _x, _Delta, __fx, _Tgrad_fx, _Hg, _Thess_fxT, _Htil, _neg_grad_fx );
DVLOG(2) << "Trust region active (C) : " << M_theta.isTrustRegionActive() << "\n";
value_type _norm_gtil = norm_2( _Tgrad_fx );
_s_til = zero_vector<value_type>( _s_til.size() );
_s_til_old = _s_til;
_rCG = -_Tgrad_fx;
_rCG_old = _rCG;
_dCG = _rCG;
size_t _inner_iter = 0;
_CGiter++;
while ( !_done )
{
if ( _restart )
{
_n_restarts++;
_CGiter++;
// RE-INITIALIZE:
_inner_iter = 0;
_s_til_eps = _s_til + M_options.deps * _dCG;
this->makeStep( _x, _s_til_eps, _Delta,
__fx,
_Tgrad_fx, _Hg, _Thess_fxT, _Htil );
_norm_gtil = norm_2( _Tgrad_fx );
_Htil_s_til = prod( _Htil, _s_til );
_s_til_old = _s_til;
_rCG = -_Tgrad_fx - _Htil_s_til;
_rCG_old = _rCG ;
_dCG = _rCG;
_restart = false;
}
//
// STEP 1:
//
_gamma = inner_prod( _dCG, prod( _Htil,_dCG ) );
if ( _gamma <= 0 )
{
_n_indef++;
_tau = this->tau( _s_til, _dCG, _Delta );
if ( ( !M_theta.isTrustRegionActive() ) || ( _CGiter == 1 ) )
{
_xi = _tau;
}
else if ( _inner_iter == 0 )
{
_xi = this->xi( _s_til_eps, _dCG, _tau );
}
else
{
_xi = this->xi( _s_til, _dCG, _tau );
}
_s_til_old = _s_til;
_s_til += _xi * _dCG;
if ( _xi < _tau )
{
_n_crosses_indef++;
_restart = true;
}
else if ( M_theta.isTrustRegionActive() )
{
_sCG = _s_til;
_n_truss_exit_indef++;
_done = true;
}
}
//
// STEP 2:
//
if ( !_restart && !_done )
{
_alpha = inner_prod( _rCG, _rCG ) / _gamma;
if ( ( !M_theta.isTrustRegionActive() ) || ( _CGiter == 1 ) )
_xi = _alpha;
else if ( _inner_iter == 0 )
_xi = this->xi( _s_til_eps, _dCG, _alpha );
else
_xi = this->xi( _s_til, _dCG, _alpha );
_s_til_old = _s_til;
_s_til += _xi * _dCG;
if ( norm_2( _s_til ) > _Delta )
{
_tau = this->tau( _s_til_old, _dCG, _Delta );
_sCG = _s_til_old + _tau * _dCG;
_n_truss_exit_def++;
_done = true;
}
}
//
// STEP 3:
//
if ( !_restart && !_done )
{
if ( _xi >= _alpha )
{
_rCG_old = _rCG;
_rCG -= _alpha * prod( _Htil, _dCG );
}
else if ( M_theta.isTrustRegionActive() )
{
_n_crosses_def++;
_restart = true;
}
}
if ( norm_2( _rCG ) / _norm_gtil < M_options.CGtol )
{
_sCG = _s_til;
DVLOG(2) << "\n\nNormal CG exit 1: ||rCG||/||g_til|| = " << norm_2( _rCG ) / _norm_gtil << "\n";
_done = true;
}
if ( _CGiter >= _x.size() )
{
_sCG = _s_til;
DVLOG(2) << "\n\nNormal CG exit 2: _CGiter = " << _CGiter << "\n";
_done = true;
}
//
// STEP 4:
//
if ( !_restart && !_done )
{
_beta = inner_prod( _rCG, _rCG ) / inner_prod( _rCG_old, _rCG_old );
DVLOG(2) << "\nbeta = " << _beta << "\n";
_dCG = _rCG + _beta * _dCG;
_CGiter++;
_inner_iter++;
}
}
_s_til_x_G_til_x_s_til = inner_prod( _sCG, prod( _Hg, _sCG ) );
phi_til = inner_prod( _Tgrad_fx, _sCG ) + 0.5 * inner_prod( _sCG, prod( _Htil,_sCG ) );
norm_s_til = norm_2( _sCG );
// restoring to original space
_sCG = prod( M_theta(), _sCG );
}
/// dot product with another Vector3d
double Vector3d::dot(const Vector3d& other) const
{
return inner_prod(m_storage, other.m_storage);
}
/** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
static spx_word32_t pitch_gain_search_3tap(
const spx_word16_t target[], /* Target vector */
const spx_coef_t ak[], /* LPCs for this subframe */
const spx_coef_t awk1[], /* Weighted LPCs #1 for this subframe */
const spx_coef_t awk2[], /* Weighted LPCs #2 for this subframe */
spx_sig_t exc[], /* Excitation */
const signed char *gain_cdbk,
int gain_cdbk_size,
int pitch, /* Pitch value */
int p, /* Number of LPC coeffs */
int nsf, /* Number of samples in subframe */
SpeexBits *bits,
char *stack,
const spx_word16_t *exc2,
const spx_word16_t *r,
spx_word16_t *new_target,
int *cdbk_index,
int plc_tuning,
spx_word32_t cumul_gain,
int scaledown
)
{
int i,j;
VARDECL(spx_word16_t *tmp1);
VARDECL(spx_word16_t *e);
spx_word16_t *x[3];
spx_word32_t corr[3];
spx_word32_t A[3][3];
spx_word16_t gain[3];
spx_word32_t err;
spx_word16_t max_gain=128;
int best_cdbk=0;
ALLOC(tmp1, 3*nsf, spx_word16_t);
ALLOC(e, nsf, spx_word16_t);
if (cumul_gain > 262144)
max_gain = 31;
x[0]=tmp1;
x[1]=tmp1+nsf;
x[2]=tmp1+2*nsf;
for (j=0;j<nsf;j++)
new_target[j] = target[j];
{
int bound;
VARDECL(spx_mem_t *mm);
int pp=pitch-1;
ALLOC(mm, p, spx_mem_t);
bound = nsf;
if (nsf-pp>0)
bound = pp;
for (j=0;j<bound;j++)
e[j]=exc2[j-pp];
bound = nsf;
if (nsf-pp-pitch>0)
bound = pp+pitch;
for (;j<bound;j++)
e[j]=exc2[j-pp-pitch];
for (;j<nsf;j++)
e[j]=0;
#ifdef FIXED_POINT
/* Scale target and excitation down if needed (avoiding overflow) */
if (scaledown)
{
for (j=0;j<nsf;j++)
e[j] = SHR16(e[j],1);
for (j=0;j<nsf;j++)
new_target[j] = SHR16(new_target[j],1);
}
#endif
for (j=0;j<p;j++)
mm[j] = 0;
iir_mem16(e, ak, e, nsf, p, mm, stack);
for (j=0;j<p;j++)
mm[j] = 0;
filter10(e, awk1, awk2, e, nsf, mm, stack);
for (j=0;j<nsf;j++)
x[2][j] = e[j];
}
for (i=1;i>=0;i--)
{
spx_word16_t e0=exc2[-pitch-1+i];
#ifdef FIXED_POINT
/* Scale excitation down if needed (avoiding overflow) */
if (scaledown)
e0 = SHR16(e0,1);
#endif
x[i][0]=MULT16_16_Q14(r[0], e0);
for (j=0;j<nsf-1;j++)
x[i][j+1]=ADD32(x[i+1][j],MULT16_16_P14(r[j+1], e0));
}
for (i=0;i<3;i++)
corr[i]=inner_prod(x[i],new_target,nsf);
for (i=0;i<3;i++)
for (j=0;j<=i;j++)
A[i][j]=A[j][i]=inner_prod(x[i],x[j],nsf);
{
spx_word32_t C[9];
#ifdef FIXED_POINT
spx_word16_t C16[9];
#else
spx_word16_t *C16=C;
#endif
C[0]=corr[2];
C[1]=corr[1];
C[2]=corr[0];
C[3]=A[1][2];
C[4]=A[0][1];
C[5]=A[0][2];
C[6]=A[2][2];
C[7]=A[1][1];
C[8]=A[0][0];
/*plc_tuning *= 2;*/
if (plc_tuning<2)
plc_tuning=2;
if (plc_tuning>30)
plc_tuning=30;
#ifdef FIXED_POINT
C[0] = SHL32(C[0],1);
C[1] = SHL32(C[1],1);
C[2] = SHL32(C[2],1);
C[3] = SHL32(C[3],1);
C[4] = SHL32(C[4],1);
C[5] = SHL32(C[5],1);
C[6] = MAC16_32_Q15(C[6],MULT16_16_16(plc_tuning,655),C[6]);
C[7] = MAC16_32_Q15(C[7],MULT16_16_16(plc_tuning,655),C[7]);
C[8] = MAC16_32_Q15(C[8],MULT16_16_16(plc_tuning,655),C[8]);
normalize16(C, C16, 32767, 9);
#else
C[6]*=.5*(1+.02*plc_tuning);
C[7]*=.5*(1+.02*plc_tuning);
C[8]*=.5*(1+.02*plc_tuning);
#endif
best_cdbk = pitch_gain_search_3tap_vq(gain_cdbk, gain_cdbk_size, C16, max_gain);
#ifdef FIXED_POINT
gain[0] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*4]);
gain[1] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*4+1]);
gain[2] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*4+2]);
/*printf ("%d %d %d %d\n",gain[0],gain[1],gain[2], best_cdbk);*/
#else
gain[0] = 0.015625*gain_cdbk[best_cdbk*4] + .5;
gain[1] = 0.015625*gain_cdbk[best_cdbk*4+1]+ .5;
gain[2] = 0.015625*gain_cdbk[best_cdbk*4+2]+ .5;
#endif
*cdbk_index=best_cdbk;
}
SPEEX_MEMSET(exc, 0, nsf);
for (i=0;i<3;i++)
{
int j;
int tmp1, tmp3;
int pp=pitch+1-i;
tmp1=nsf;
if (tmp1>pp)
tmp1=pp;
for (j=0;j<tmp1;j++)
exc[j]=MAC16_16(exc[j],SHL16(gain[2-i],7),exc2[j-pp]);
tmp3=nsf;
if (tmp3>pp+pitch)
tmp3=pp+pitch;
for (j=tmp1;j<tmp3;j++)
exc[j]=MAC16_16(exc[j],SHL16(gain[2-i],7),exc2[j-pp-pitch]);
}
for (i=0;i<nsf;i++)
{
spx_word32_t tmp = ADD32(ADD32(MULT16_16(gain[0],x[2][i]),MULT16_16(gain[1],x[1][i])),
MULT16_16(gain[2],x[0][i]));
new_target[i] = SUB16(new_target[i], EXTRACT16(PSHR32(tmp,6)));
}
err = inner_prod(new_target, new_target, nsf);
return err;
}
void LinearElastic3DLaw::CalculateMaterialResponseKirchhoff (Parameters& rValues)
{
//-----------------------------//
//a.-Check if the constitutive parameters are passed correctly to the law calculation
//CheckParameters(rValues);
//b.- Get Values to compute the constitutive law:
Flags &Options=rValues.GetOptions();
const Properties& MaterialProperties = rValues.GetMaterialProperties();
Vector& StrainVector = rValues.GetStrainVector();
Vector& StressVector = rValues.GetStressVector();
//-----------------------------//
//1.- Lame constants
const double& YoungModulus = MaterialProperties[YOUNG_MODULUS];
const double& PoissonCoefficient = MaterialProperties[POISSON_RATIO];
// //1.1- Thermal constants
// double ThermalExpansionCoefficient = 0;
// if( MaterialProperties.Has(THERMAL_EXPANSION_COEFFICIENT) )
// ThermalExpansionCoefficient = MaterialProperties[THERMAL_EXPANSION_COEFFICIENT];
// double ReferenceTemperature = 0;
// if( MaterialProperties.Has(REFERENCE_TEMPERATURE) )
// ReferenceTemperature = MaterialProperties[REFERENCE_TEMPERATURE];
if(Options.Is( ConstitutiveLaw::COMPUTE_STRAIN )) //large strains
{
//1.-Compute total deformation gradient
const Matrix& DeformationGradientF = rValues.GetDeformationGradientF();
//2.-Left Cauchy-Green tensor b
Matrix LeftCauchyGreenMatrix = prod(DeformationGradientF,trans(DeformationGradientF));
//3.-Almansi Strain:
//e= 0.5*(1-invFT*invF)
this->CalculateAlmansiStrain(LeftCauchyGreenMatrix,StrainVector);
//LARGE STRAINS OBJECTIVE MESURE KIRCHHOFF MATERIAL:
// Kirchhoff model is set with S = CE
this->CalculateMaterialResponsePK2 (rValues);
//1.- Obtain parameters
const double& DeterminantF = rValues.GetDeterminantF();
//2.-Almansi Strain:
// if(Options.Is( ConstitutiveLaw::COMPUTE_STRAIN ))
// {
// TransformStrains (StrainVector, DeformationGradientF, StrainMeasure_GreenLagrange, StrainMeasure_Almansi);
// }
//3.-Calculate Total Kirchhoff stress
if( Options.Is( ConstitutiveLaw::COMPUTE_STRESS ) )
{
TransformStresses(StressVector, DeformationGradientF, DeterminantF, StressMeasure_PK2, StressMeasure_Kirchhoff);
}
// COMMENTED BECAUSE THE CONVERGENCE IS NOT IMPROVED AND IS TIME CONSUMING:
//4.-Calculate Cauchy constitutive tensor
// if( Options.Is( ConstitutiveLaw::COMPUTE_CONSTITUTIVE_TENSOR ) )
// {
// Matrix& ConstitutiveMatrix = rValues.GetConstitutiveMatrix();
// PushForwardConstitutiveMatrix(ConstitutiveMatrix, DeformationGradientF);
// }
if( Options.Is( ConstitutiveLaw::COMPUTE_STRAIN_ENERGY ) )
{
mStrainEnergy *= DeterminantF;
}
}
else{ //small strains
//7.-Calculate total Kirchhoff stress
if( Options.Is( ConstitutiveLaw::COMPUTE_STRESS ) ){
if( Options.Is( ConstitutiveLaw::COMPUTE_CONSTITUTIVE_TENSOR ) ){
Matrix& ConstitutiveMatrix = rValues.GetConstitutiveMatrix();
this->CalculateLinearElasticMatrix( ConstitutiveMatrix, YoungModulus, PoissonCoefficient );
this->CalculateStress( StrainVector, ConstitutiveMatrix, StressVector );
}
else {
Matrix ConstitutiveMatrix = ZeroMatrix( StrainVector.size() ,StrainVector.size());
this->CalculateLinearElasticMatrix( ConstitutiveMatrix, YoungModulus, PoissonCoefficient );
this->CalculateStress( StrainVector, ConstitutiveMatrix, StressVector );
}
}
else if( Options.IsNot( ConstitutiveLaw::COMPUTE_STRESS ) && Options.Is( ConstitutiveLaw::COMPUTE_CONSTITUTIVE_TENSOR ) )
{
Matrix& ConstitutiveMatrix = rValues.GetConstitutiveMatrix();
this->CalculateLinearElasticMatrix( ConstitutiveMatrix, YoungModulus, PoissonCoefficient );
}
if( Options.Is( ConstitutiveLaw::COMPUTE_STRAIN_ENERGY ) )
{
if( Options.IsNot( ConstitutiveLaw::COMPUTE_STRESS ) ){
if(Options.Is( ConstitutiveLaw::COMPUTE_CONSTITUTIVE_TENSOR )){
Matrix ConstitutiveMatrix = ZeroMatrix( StrainVector.size(), StrainVector.size());
this->CalculateLinearElasticMatrix( ConstitutiveMatrix, YoungModulus, PoissonCoefficient );
this->CalculateStress( StrainVector, ConstitutiveMatrix, StressVector );
}
else{
Matrix& ConstitutiveMatrix = rValues.GetConstitutiveMatrix();
this->CalculateStress( StrainVector, ConstitutiveMatrix, StressVector );
}
}
mStrainEnergy = 0.5 * inner_prod(StrainVector,StressVector); //Belytschko Nonlinear Finite Elements pag 226 (5.4.3) : w = 0.5*E:C:E
}
}
//std::cout<<" Strain "<<StrainVector<<std::endl;
//std::cout<<" Stress "<<StressVector<<std::endl;
//Matrix& ConstitutiveMatrix = rValues.GetConstitutiveMatrix();
//std::cout<<" Constitutive "<<ConstitutiveMatrix<<std::endl;
}
double ParameterEstimatorImpl::iterate(ostream* log)
{
// DerivativeTest::testDerivatives< complex<double> >(e_, p_); // testing only
double error_old = error();
ublas::vector<double> d = e_.dp(p_);
ublas::matrix<double> d2 = e_.dp2(p_);
// calculate new estimate:
// correction == inverse(d2) * d
// p_new = p_old - correction
Parameters correction = solve(d2, d);
// compare error change to prediction from parabolic approximation
ublas::vector<double> dp = -correction;
double error_change_predicted = inner_prod(d, dp) + .5*inner_prod(dp, prod(d2,dp));
double error_change_actual = e_(p_-correction) - error_old;
if (log) *log << "d: " << d << endl;
if (log) *log << "d2: " << d2 << endl;
if (log) *log << "correction: " << correction << endl;
if (log) *log << "error_change_predicted: " << error_change_predicted << endl;
if (log) *log << "error_change_actual: " << error_change_actual << endl;
// if we can decrease error -- go for it!
if (error_change_actual < 0)
{
p_ -= correction;
return error_change_actual;
}
// error is going to increase if we make the full correction;
// backtrack along correction gradient to find decreasing error
Parameters correction_backtrack = correction;
int zeroCount = 0;
for (int i=0; i<10; i++)
{
correction_backtrack /= 2;
double error_change_backtrack = e_(p_ - correction_backtrack) - error_old;
if (log) *log << "error_change_backtrack: " << error_change_backtrack << endl;
if (error_change_backtrack < 0)
{
// found negative error change -- go for it!
p_ -= correction_backtrack;
return error_change_backtrack;
}
else if (error_change_backtrack == 0)
{
zeroCount++;
if (zeroCount >= 3) // stuck on zero -- we're outta here
break;
}
}
// don't correct
if (log) *log << "No correction.\n";
return 0;
}
/** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
static spx_word64_t pitch_gain_search_3tap(
const spx_sig_t target[], /* Target vector */
const spx_coef_t ak[], /* LPCs for this subframe */
const spx_coef_t awk1[], /* Weighted LPCs #1 for this subframe */
const spx_coef_t awk2[], /* Weighted LPCs #2 for this subframe */
spx_sig_t exc[], /* Excitation */
const void *par,
int pitch, /* Pitch value */
int p, /* Number of LPC coeffs */
int nsf, /* Number of samples in subframe */
SpeexBits *bits,
char *stack,
const spx_sig_t *exc2,
const spx_word16_t *r,
spx_sig_t *new_target,
int *cdbk_index,
int cdbk_offset,
int plc_tuning
)
{
int i,j;
VARDECL(spx_sig_t *tmp1);
VARDECL(spx_sig_t *tmp2);
spx_sig_t *x[3];
spx_sig_t *e[3];
spx_word32_t corr[3];
spx_word32_t A[3][3];
int gain_cdbk_size;
const signed char *gain_cdbk;
spx_word16_t gain[3];
spx_word64_t err;
const ltp_params *params;
params = (const ltp_params*) par;
gain_cdbk_size = 1<<params->gain_bits;
gain_cdbk = params->gain_cdbk + 3*gain_cdbk_size*cdbk_offset;
ALLOC(tmp1, 3*nsf, spx_sig_t);
ALLOC(tmp2, 3*nsf, spx_sig_t);
x[0]=tmp1;
x[1]=tmp1+nsf;
x[2]=tmp1+2*nsf;
e[0]=tmp2;
e[1]=tmp2+nsf;
e[2]=tmp2+2*nsf;
for (i=2; i>=0; i--)
{
int pp=pitch+1-i;
for (j=0; j<nsf; j++)
{
if (j-pp<0)
e[i][j]=exc2[j-pp];
else if (j-pp-pitch<0)
e[i][j]=exc2[j-pp-pitch];
else
e[i][j]=0;
}
if (i==2)
syn_percep_zero(e[i], ak, awk1, awk2, x[i], nsf, p, stack);
else {
for (j=0; j<nsf-1; j++)
x[i][j+1]=x[i+1][j];
x[i][0]=0;
for (j=0; j<nsf; j++)
{
x[i][j]=ADD32(x[i][j],SHL32(MULT16_32_Q15(r[j], e[i][0]),1));
}
}
}
#ifdef FIXED_POINT
{
/* If using fixed-point, we need to normalize the signals first */
spx_word16_t *y[3];
VARDECL(spx_word16_t *ytmp);
VARDECL(spx_word16_t *t);
spx_sig_t max_val=1;
int sig_shift;
ALLOC(ytmp, 3*nsf, spx_word16_t);
#if 0
ALLOC(y[0], nsf, spx_word16_t);
ALLOC(y[1], nsf, spx_word16_t);
ALLOC(y[2], nsf, spx_word16_t);
#else
y[0] = ytmp;
y[1] = ytmp+nsf;
y[2] = ytmp+2*nsf;
#endif
ALLOC(t, nsf, spx_word16_t);
for (j=0; j<3; j++)
{
for (i=0; i<nsf; i++)
{
spx_sig_t tmp = x[j][i];
if (tmp<0)
tmp = -tmp;
if (tmp > max_val)
max_val = tmp;
}
}
for (i=0; i<nsf; i++)
{
spx_sig_t tmp = target[i];
if (tmp<0)
tmp = -tmp;
if (tmp > max_val)
max_val = tmp;
}
sig_shift=0;
while (max_val>16384)
{
sig_shift++;
max_val >>= 1;
}
for (j=0; j<3; j++)
{
for (i=0; i<nsf; i++)
{
y[j][i] = EXTRACT16(SHR32(x[j][i],sig_shift));
}
}
for (i=0; i<nsf; i++)
{
t[i] = EXTRACT16(SHR32(target[i],sig_shift));
}
for (i=0; i<3; i++)
corr[i]=inner_prod(y[i],t,nsf);
for (i=0; i<3; i++)
for (j=0; j<=i; j++)
A[i][j]=A[j][i]=inner_prod(y[i],y[j],nsf);
}
#else
{
for (i=0; i<3; i++)
corr[i]=inner_prod(x[i],target,nsf);
for (i=0; i<3; i++)
for (j=0; j<=i; j++)
A[i][j]=A[j][i]=inner_prod(x[i],x[j],nsf);
}
#endif
{
spx_word32_t C[9];
const signed char *ptr=gain_cdbk;
int best_cdbk=0;
spx_word32_t best_sum=0;
C[0]=corr[2];
C[1]=corr[1];
C[2]=corr[0];
C[3]=A[1][2];
C[4]=A[0][1];
C[5]=A[0][2];
C[6]=A[2][2];
C[7]=A[1][1];
C[8]=A[0][0];
/*plc_tuning *= 2;*/
if (plc_tuning<2)
plc_tuning=2;
#ifdef FIXED_POINT
C[0] = MAC16_32_Q15(C[0],MULT16_16_16(plc_tuning,-327),C[0]);
C[1] = MAC16_32_Q15(C[1],MULT16_16_16(plc_tuning,-327),C[1]);
C[2] = MAC16_32_Q15(C[2],MULT16_16_16(plc_tuning,-327),C[2]);
#else
C[0]*=1-.01*plc_tuning;
C[1]*=1-.01*plc_tuning;
C[2]*=1-.01*plc_tuning;
C[6]*=.5*(1+.01*plc_tuning);
C[7]*=.5*(1+.01*plc_tuning);
C[8]*=.5*(1+.01*plc_tuning);
#endif
for (i=0; i<gain_cdbk_size; i++)
{
spx_word32_t sum=0;
spx_word16_t g0,g1,g2;
spx_word16_t pitch_control=64;
spx_word16_t gain_sum;
ptr = gain_cdbk+3*i;
g0=ADD16((spx_word16_t)ptr[0],32);
g1=ADD16((spx_word16_t)ptr[1],32);
g2=ADD16((spx_word16_t)ptr[2],32);
gain_sum = g1;
if (g0>0)
gain_sum += g0;
if (g2>0)
gain_sum += g2;
if (gain_sum > 64)
{
gain_sum = SUB16(gain_sum, 64);
if (gain_sum > 127)
gain_sum = 127;
#ifdef FIXED_POINT
pitch_control = SUB16(64,EXTRACT16(PSHR32(MULT16_16(64,MULT16_16_16(plc_tuning, gain_sum)),10)));
#else
pitch_control = 64*(1.-.001*plc_tuning*gain_sum);
#endif
if (pitch_control < 0)
pitch_control = 0;
}
sum = ADD32(sum,MULT16_32_Q14(MULT16_16_16(g0,pitch_control),C[0]));
sum = ADD32(sum,MULT16_32_Q14(MULT16_16_16(g1,pitch_control),C[1]));
sum = ADD32(sum,MULT16_32_Q14(MULT16_16_16(g2,pitch_control),C[2]));
sum = SUB32(sum,MULT16_32_Q14(MULT16_16_16(g0,g1),C[3]));
sum = SUB32(sum,MULT16_32_Q14(MULT16_16_16(g2,g1),C[4]));
sum = SUB32(sum,MULT16_32_Q14(MULT16_16_16(g2,g0),C[5]));
sum = SUB32(sum,MULT16_32_Q15(MULT16_16_16(g0,g0),C[6]));
sum = SUB32(sum,MULT16_32_Q15(MULT16_16_16(g1,g1),C[7]));
sum = SUB32(sum,MULT16_32_Q15(MULT16_16_16(g2,g2),C[8]));
/* We could force "safe" pitch values to handle packet loss better */
if (sum>best_sum || i==0)
{
best_sum=sum;
best_cdbk=i;
}
}
#ifdef FIXED_POINT
gain[0] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*3]);
gain[1] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*3+1]);
gain[2] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*3+2]);
/*printf ("%d %d %d %d\n",gain[0],gain[1],gain[2], best_cdbk);*/
#else
gain[0] = 0.015625*gain_cdbk[best_cdbk*3] + .5;
gain[1] = 0.015625*gain_cdbk[best_cdbk*3+1]+ .5;
gain[2] = 0.015625*gain_cdbk[best_cdbk*3+2]+ .5;
#endif
*cdbk_index=best_cdbk;
}
#ifdef FIXED_POINT
for (i=0; i<nsf; i++)
exc[i]=SHL32(ADD32(ADD32(MULT16_32_Q15(SHL16(gain[0],7),e[2][i]), MULT16_32_Q15(SHL16(gain[1],7),e[1][i])),
MULT16_32_Q15(SHL16(gain[2],7),e[0][i])), 2);
err=0;
for (i=0; i<nsf; i++)
{
spx_word16_t perr2;
spx_sig_t tmp = SHL32(ADD32(ADD32(MULT16_32_Q15(SHL16(gain[0],7),x[2][i]),MULT16_32_Q15(SHL16(gain[1],7),x[1][i])),
MULT16_32_Q15(SHL16(gain[2],7),x[0][i])),2);
spx_sig_t perr=SUB32(target[i],tmp);
new_target[i] = SUB32(target[i], tmp);
perr2 = EXTRACT16(PSHR32(perr,15));
err = ADD64(err,MULT16_16(perr2,perr2));
}
#else
for (i=0; i<nsf; i++)
exc[i]=gain[0]*e[2][i]+gain[1]*e[1][i]+gain[2]*e[0][i];
err=0;
for (i=0; i<nsf; i++)
{
spx_sig_t tmp = gain[2]*x[0][i]+gain[1]*x[1][i]+gain[0]*x[2][i];
new_target[i] = target[i] - tmp;
err+=new_target[i]*new_target[i];
}
#endif
return err;
}
bool Isotropic_Rankine_Yield_Function::One_Vector_Return_Mapping_To_Main_Plane(const array_1d<double,3>& PrincipalStress, Vector& delta_lamda, array_1d<double,3>& Sigma)
{
unsigned int iter = 0;
double norma = 1.00;
double delta_lamda_a = 0.00;
double E = (*mprops)[YOUNG_MODULUS];
double NU = (*mprops)[POISSON_RATIO];
double G = 0.5*E / (1.00 + NU);
double K = E / (3.00 * (1.00-2.00*NU));
double H = 0.00;
double d = 0.00;
double residual = 0.00;
const double toler = 1E-3;
const double raiz_2_3 = 0.81649658092773;
unsigned int max = 1000;
double Partial_Ep_gama_a = 0.00;
double Inc = 0.00;
const double d3 = 0.3333333333333333;
const double raiz2d3 = 0.8164958092773;
double Ppvs = 0.00; /// principal plastic volumetric strain
array_1d<double,3> Ppds = ZeroVector(3); /// principal plastic desviatoric strain
array_1d<double,3> Pps = ZeroVector(3); /// principal plastic strain
array_1d<double,3> I;
I[0] = 1.00;
I[1] = 1.00;
I[2] = 1.00;
delta_lamda = ZeroVector(1);
residual = PrincipalStress[0] - mcurrent_Ft;;
norma = residual/mcurrent_Ft;
Vector Imput_Parameters(4);
Imput_Parameters = ZeroVector(4);
Imput_Parameters[0] = mhe;
Imput_Parameters[1] = mrankine_accumulated_plastic_strain_current;
while(iter++<=max && norma>= toler)
{
Partial_Ep_gama_a = raiz_2_3; /// Acumulated Vond misses
H = mpSofteningBehaviorFt->FirstDerivateFunctionBehavior(Imput_Parameters);
d = -4.00 * G / 3.00 - K - H * Partial_Ep_gama_a;
delta_lamda[0] = delta_lamda[0] - (residual / d);
///normal acuulated
///mrankine_accumulated_plastic_strain_current= mrankine_accumulated_plastic_strain_old+ delta_lamda[0];
/// von mises
mrankine_accumulated_plastic_strain_current = mrankine_accumulated_plastic_strain_old + raiz_2_3 * delta_lamda[0];
/// computing Kp_punto.
/// volumetric and desviatoric plastic strain
Pps[0] = delta_lamda_a;
Pps[1] = 0.00;
Pps[2] = 0.00;
Ppvs = Pps[0] + Pps[1] + Pps[2];
noalias(Ppds) = Pps - d3 * Ppvs * I;
Inc = raiz2d3* std::sqrt(inner_prod(Pps,Pps));
mrankine_accumulated_plastic_strain_current = mrankine_accumulated_plastic_strain_old + Inc;
ComputeActualStrees(Ppvs, Ppds, PrincipalStress, Sigma);
ComputeActualStrain(Pps);
CalculatePlasticDamage(Sigma);
///* Updatinf mFt
Imput_Parameters[1] = mrankine_accumulated_plastic_strain_current;
Imput_Parameters[2] = mpastic_damage_old;
Imput_Parameters[3] = mpastic_damage_current;
mcurrent_Ft = mpSofteningBehaviorFt->FunctionBehavior(Imput_Parameters);
delta_lamda_a = delta_lamda[0];
///* comprobando si mft se cumplio
if(mcurrent_Ft <= toler)
{
mcurrent_Ft = toler;
break;
}
else
{
///* update teh current value
residual = PrincipalStress[0] - mcurrent_Ft - delta_lamda_a * (4.00 * G / 3.00 + K);
norma = fabs(residual/mcurrent_Ft);
}
}
if(iter>=max) //|| std::delta_lamda_a<0.00){
{
//return false;
KRATOS_WATCH(norma)
KRATOS_WATCH(PrincipalStress[0])
KRATOS_WATCH(PrincipalStress[1])
KRATOS_WATCH(PrincipalStress[2])
KRATOS_WATCH(delta_lamda_a)
KRATOS_WATCH(residual)
KRATOS_WATCH(mcurrent_Ft)
std::cout<< "RETURN MAPPING TO MAIN PLANE RANKINE NOT CONVERGED" << std::endl;
KRATOS_THROW_ERROR(std::logic_error, "RETURN MAPPING TO MAIN PLANE RANKINE NOT CONVERGED" , "");
}
///* Updating Stress
if(mcurrent_Ft <=toler)
{
Sigma[0] = 0.00;
Sigma[1] = 0.00;
Sigma[2] = 0.00;
}
else
{
/// volumetric and desviatoric plastic strain
Pps[0] = delta_lamda_a;
Pps[1] = 0.00;
Pps[2] = 0.00;
Ppvs = Pps[0] + Pps[1] + Pps[2];
noalias(Ppds) = Pps - d3 * Ppvs * I;
//Sigma[0] = PrincipalStress[0] - delta_lamda_a*(4.00 * G / 3.00 + K );
//Sigma[1] = PrincipalStress[1] - delta_lamda_a*(-2.00 * G / 3.00 + K );
//Sigma[2] = PrincipalStress[2] - delta_lamda_a*(-2.00 * G / 3.00 + K );
noalias(Sigma) = PrincipalStress - 2.00 * G * Ppds - K * Ppvs * I;
}
bool check = CheckValidity(Sigma);
if(check==true)
{
Vector PPS_bar(3);
PPS_bar = ZeroVector(3);
ComputePlasticStrainBar(mplastic_strain_old ,m_inv_DeltaF, PPS_bar);
//updating the correct principal pastic strain
mPrincipalPlasticStrain_current[0] = /*mPrincipalPlasticStrain_old[0]*/ PPS_bar[0] + delta_lamda_a;
mPrincipalPlasticStrain_current[1] = /*mPrincipalPlasticStrain_old[1]*/ PPS_bar[1];
mPrincipalPlasticStrain_current[2] = /*mPrincipalPlasticStrain_old[2]*/ PPS_bar[2];
return true;
}
else
return false;
}
void Isotropic_Rankine_Yield_Function::Three_Vector_Return_Mapping_To_Apex(const array_1d<double,3>& PrincipalStress, Vector& delta_lamda ,array_1d<double,3>& Sigma)
{
unsigned int iter = 0;
unsigned int max = 10;
// int singular = 0;
double norma = 1.00;
double delta_lamda_a = 0.00;
double delta_lamda_b = 0.00;
double delta_lamda_c = 0.00;
const double toler = 1E-3;
double E = (*mprops)[YOUNG_MODULUS];
double NU = (*mprops)[POISSON_RATIO];
double G = 0.5*E / (1.00 + NU);
double K = E / (3.00 * (1.00-2.00*NU));
double H = mH;
Matrix d;
d.resize(3,3,false);
noalias(d) = ZeroMatrix(3,3);
Matrix d_inv;
d_inv.resize(3,3,false);
noalias(d_inv)= ZeroMatrix(3,3);
Vector residual = ZeroVector(3);
delta_lamda = ZeroVector(3);
Vector Imput_Parameters(4);
Imput_Parameters = ZeroVector(4);
Imput_Parameters[0] = mhe;
Imput_Parameters[1] = mrankine_accumulated_plastic_strain_current;
residual[0] = PrincipalStress[0] - mcurrent_Ft;
residual[1] = PrincipalStress[1] - mcurrent_Ft;
residual[2] = PrincipalStress[2] - mcurrent_Ft;
H = mpSofteningBehaviorFt->FirstDerivateFunctionBehavior(Imput_Parameters);
double Partial_Ep_gama_a = 0.00;
double Partial_Ep_gama_b = 0.00;
double Partial_Ep_gama_c = 0.00;
double prod_H_a = 0.00;
double prod_H_b = 0.00;
double prod_H_c = 0.00;
double Inc = 0.00;
d.resize(3,3);
d_inv.resize(3,3);
const double raiz2d3 = 0.8164958092773;
const double d3 = 0.3333333333333333;
double Ppvs = 0.00; /// principal plastic volumetric strain
array_1d<double,3> Ppds = ZeroVector(3); /// principal plastic desviatoric strain
array_1d<double,3> Pps = ZeroVector(3); /// principal plastic strain
array_1d<double,3> I;
I[0] = 1.00;
I[1] = 1.00;
I[2] = 1.00;
while(iter++<=max && norma>= toler)
{
prod_H_a = H * Partial_Ep_gama_a;
prod_H_b = H * Partial_Ep_gama_b;
prod_H_c = H * Partial_Ep_gama_c;
d(0,0) = -4.00 * G / 3.00 - K - prod_H_a;
d(0,1) = 2.00 * G / 3.00 - K - prod_H_b;
d(0,2) = 2.00 * G / 3.00 - K - prod_H_c;
d(1,0) = 2.00 * G / 3.00 - K - prod_H_a;
d(1,1) = -4.00 * G / 3.00 - K - prod_H_b;
d(1,2) = 2.00 * G / 3.00 - K - prod_H_c;
d(2,0) = 2.00 * G / 3.00 - K - prod_H_a;
d(2,1) = 2.00 * G / 3.00 - K - prod_H_b;
d(2,2) = -4.00 * G / 3.00 - K - prod_H_c;
// singular = SD_MathUtils<double>::InvertMatrix(d, d_inv);
noalias(delta_lamda) = delta_lamda - Vector(prod(d_inv, residual));
delta_lamda_a = delta_lamda[0];
delta_lamda_b = delta_lamda[1];
delta_lamda_c = delta_lamda[2];
/// normal
///mrankine_accumulated_plastic_strain_current= mrankine_accumulated_plastic_strain_old+ delta_lamda_a + delta_lamda_b + delta_lamda_c;
///von mises
Inc = 0.81649658092773 * norm_2(delta_lamda);
mrankine_accumulated_plastic_strain_current = mrankine_accumulated_plastic_strain_old + Inc;
/// computing Kp_punto.
/// volumetric and desviatoric plastic strain
Pps[0] = delta_lamda_a;
Pps[1] = delta_lamda_b;
Pps[2] = delta_lamda_c;
Ppvs = Pps[0] + Pps[1] + Pps[2];
noalias(Ppds) = Pps - d3 * Ppvs * I;
Inc = raiz2d3* std::sqrt(inner_prod(Pps,Pps));
mrankine_accumulated_plastic_strain_current = mrankine_accumulated_plastic_strain_old + Inc;
ComputeActualStrees(Ppvs, Ppds, PrincipalStress, Sigma);
ComputeActualStrain(Pps);
CalculatePlasticDamage(Sigma);
/// Updatinf mFt
Partial_Ep_gama_a = (2.00/3.00) * delta_lamda_a/Inc;
Partial_Ep_gama_b = (2.00/3.00) * delta_lamda_b/Inc;
Partial_Ep_gama_b = (2.00/3.00) * delta_lamda_c/Inc;
Imput_Parameters[1] = mrankine_accumulated_plastic_strain_current;
Imput_Parameters[2] = mpastic_damage_old;
Imput_Parameters[3] = mpastic_damage_current;
mcurrent_Ft = mpSofteningBehaviorFt->FunctionBehavior(Imput_Parameters);
H = mpSofteningBehaviorFt->FirstDerivateFunctionBehavior(Imput_Parameters);
/// ft se anulan
if(mcurrent_Ft<=toler)
{
mcurrent_Ft = 0.00;
//delta_lamda_a = delta_lamda[0];
//delta_lamda_b = delta_lamda[1];
//delta_lamda_c = delta_lamda[2];
break;
}
else
{
residual[0] = PrincipalStress[0] - mcurrent_Ft;
residual[1] = PrincipalStress[1] - mcurrent_Ft;
residual[2] = PrincipalStress[2] - mcurrent_Ft;
residual[0] = residual[0] - delta_lamda_a*( 4.00 * G / 3.00 + K ) - delta_lamda_b*( -2.00 * G / 3.00 + K ) - delta_lamda_c*( -2.00 * G / 3.00 + K );
residual[1] = residual[1] - delta_lamda_a*(-2.00 * G / 3.00 + K ) - delta_lamda_b*( 4.00 * G / 3.00 + K ) - delta_lamda_c*( -2.00 * G / 3.00 + K );
residual[2] = residual[2] - delta_lamda_a*(-2.00 * G / 3.00 + K ) - delta_lamda_b*( -2.00 * G / 3.00 + K ) - delta_lamda_c*( 4.00 * G / 3.00 + K );
norma = norm_2(residual);
}
}
if(iter>=max || delta_lamda_a<0.0 || delta_lamda_b<0.0 || delta_lamda_c<0)
{
KRATOS_WATCH(iter)
KRATOS_WATCH(norma)
KRATOS_WATCH(mcurrent_Ft)
KRATOS_WATCH(Sigma)
KRATOS_WATCH(PrincipalStress)
KRATOS_WATCH(delta_lamda)
std::cout<< "RETURN MAPPING APEX RANKINE NOT CONVERGED" << std::endl;
KRATOS_THROW_ERROR(std::logic_error, "RETURN MAPPING APEX RANKINE NOT CONVERGED" , "");
}
if(mcurrent_Ft <= 0.00)
{
Sigma[0] = 0.00;
Sigma[1] = 0.00;
Sigma[2] = 0.00;
}
else
{
/// volumetric and desviatoric plastic strain
Pps[0] = delta_lamda_a;
Pps[1] = delta_lamda_b;
Pps[2] = delta_lamda_c;
Ppvs = Pps[0] + Pps[1] + Pps[2];
noalias(Ppds) = Pps - d3 * Ppvs * I;
noalias(Sigma) = PrincipalStress - 2.00 * G * Ppds - K * Ppvs * I;
//Sigma[0] = PrincipalStress[0] - delta_lamda_a*( 4.00 * G / 3.00 + K ) - delta_lamda_b*( -2.00 * G / 3.00 + K ) - delta_lamda_c*( -2.00 * G / 3.00 + K );
//Sigma[1] = PrincipalStress[1] - delta_lamda_a*(-2.00 * G / 3.00 + K ) - delta_lamda_b*( 4.00 * G / 3.00 + K ) - delta_lamda_c*( -2.00 * G / 3.00 + K );
//Sigma[2] = PrincipalStress[2] - delta_lamda_a*(-2.00 * G / 3.00 + K ) - delta_lamda_b*( -2.00 * G / 3.00 + K ) - delta_lamda_c*( 4.00 * G / 3.00 + K );
}
Vector PPS_bar(3);
Imput_Parameters = ZeroVector(3);
ComputePlasticStrainBar(mplastic_strain_old ,m_inv_DeltaF, PPS_bar);
mPrincipalPlasticStrain_current[0] = /*mPrincipalPlasticStrain_old[0]*/ PPS_bar[0] + delta_lamda_a;
mPrincipalPlasticStrain_current[1] = /*mPrincipalPlasticStrain_old[0]*/ PPS_bar[1] + delta_lamda_b;
mPrincipalPlasticStrain_current[2] = /*mPrincipalPlasticStrain_old[0]*/ PPS_bar[2] + delta_lamda_c;
}
void open_loop_nbest_pitch(spx_word16_t *sw, int start, int end, int len, int *pitch, spx_word16_t *gain, int N, char *stack)
{
int i,j,k;
VARDECL(spx_word32_t *best_score);
VARDECL(spx_word32_t *best_ener);
spx_word32_t e0;
VARDECL(spx_word32_t *corr);
VARDECL(spx_word32_t *energy);
ALLOC(best_score, N, spx_word32_t);
ALLOC(best_ener, N, spx_word32_t);
ALLOC(corr, end-start+1, spx_word32_t);
ALLOC(energy, end-start+2, spx_word32_t);
for (i=0;i<N;i++)
{
best_score[i]=-1;
best_ener[i]=0;
pitch[i]=start;
}
energy[0]=inner_prod(sw-start, sw-start, len);
e0=inner_prod(sw, sw, len);
for (i=start;i<end;i++)
{
/* Update energy for next pitch*/
energy[i-start+1] = SUB32(ADD32(energy[i-start],SHR32(MULT16_16(sw[-i-1],sw[-i-1]),6)), SHR32(MULT16_16(sw[-i+len-1],sw[-i+len-1]),6));
if (energy[i-start+1] < 0)
energy[i-start+1] = 0;
}
pitch_xcorr(sw, sw-end, corr, len, end-start+1, stack);
/* FIXME: Fixed-point and floating-point code should be merged */
#ifdef FIXED_POINT
{
VARDECL(spx_word16_t *corr16);
VARDECL(spx_word16_t *ener16);
ALLOC(corr16, end-start+1, spx_word16_t);
ALLOC(ener16, end-start+1, spx_word16_t);
/* Normalize to 180 so we can square it and it still fits in 16 bits */
normalize16(corr, corr16, 180, end-start+1);
normalize16(energy, ener16, 180, end-start+1);
for (i=start;i<=end;i++)
{
spx_word16_t tmp = MULT16_16_16(corr16[i-start],corr16[i-start]);
/* Instead of dividing the tmp by the energy, we multiply on the other side */
if (MULT16_16(tmp,best_ener[N-1])>MULT16_16(best_score[N-1],ADD16(1,ener16[i-start])))
{
/* We can safely put it last and then check */
best_score[N-1]=tmp;
best_ener[N-1]=ener16[i-start]+1;
pitch[N-1]=i;
/* Check if it comes in front of others */
for (j=0;j<N-1;j++)
{
if (MULT16_16(tmp,best_ener[j])>MULT16_16(best_score[j],ADD16(1,ener16[i-start])))
{
for (k=N-1;k>j;k--)
{
best_score[k]=best_score[k-1];
best_ener[k]=best_ener[k-1];
pitch[k]=pitch[k-1];
}
best_score[j]=tmp;
best_ener[j]=ener16[i-start]+1;
pitch[j]=i;
break;
}
}
}
}
}
#else
for (i=start;i<=end;i++)
{
float tmp = corr[i-start]*corr[i-start];
if (tmp*best_ener[N-1]>best_score[N-1]*(1+energy[i-start]))
{
for (j=0;j<N;j++)
{
if (tmp*best_ener[j]>best_score[j]*(1+energy[i-start]))
{
for (k=N-1;k>j;k--)
{
best_score[k]=best_score[k-1];
best_ener[k]=best_ener[k-1];
pitch[k]=pitch[k-1];
}
best_score[j]=tmp;
best_ener[j]=energy[i-start]+1;
pitch[j]=i;
break;
}
}
}
}
#endif
/* Compute open-loop gain */
if (gain)
{
for (j=0;j<N;j++)
{
spx_word16_t g;
i=pitch[j];
g = DIV32(corr[i-start], 10+SHR32(MULT16_16(spx_sqrt(e0),spx_sqrt(energy[i-start])),6));
/* FIXME: g = max(g,corr/energy) */
if (g<0)
g = 0;
gain[j]=g;
}
}
}
void AbstractCardiacMechanicsSolver<ELASTICITY_SOLVER,DIM>::AddActiveStressAndStressDerivative(c_matrix<double,DIM,DIM>& rC,
unsigned elementIndex,
unsigned currentQuadPointGlobalIndex,
c_matrix<double,DIM,DIM>& rT,
FourthOrderTensor<DIM,DIM,DIM,DIM>& rDTdE,
bool addToDTdE)
{
for(unsigned i=0; i<DIM; i++)
{
mCurrentElementFibreDirection(i) = this->mChangeOfBasisMatrix(i,0);
}
//Compute the active tension and add to the stress and stress-derivative
double I4_fibre = inner_prod(mCurrentElementFibreDirection, prod(rC, mCurrentElementFibreDirection));
double lambda_fibre = sqrt(I4_fibre);
double active_tension = 0;
double d_act_tension_dlam = 0.0; // Set and used if assembleJacobian==true
double d_act_tension_d_dlamdt = 0.0; // Set and used if assembleJacobian==true
GetActiveTensionAndTensionDerivs(lambda_fibre, currentQuadPointGlobalIndex, addToDTdE,
active_tension, d_act_tension_dlam, d_act_tension_d_dlamdt);
double detF = sqrt(Determinant(rC));
rT += (active_tension*detF/I4_fibre)*outer_prod(mCurrentElementFibreDirection,mCurrentElementFibreDirection);
// amend the stress and dTdE using the active tension
double dTdE_coeff1 = -2*active_tension*detF/(I4_fibre*I4_fibre); // note: I4_fibre*I4_fibre = lam^4
double dTdE_coeff2 = active_tension*detF/I4_fibre;
double dTdE_coeff_s1 = 0.0; // only set non-zero if we apply cross fibre tension (in 2/3D)
double dTdE_coeff_s2 = 0.0; // only set non-zero if we apply cross fibre tension (in 2/3D)
double dTdE_coeff_s3 = 0.0; // only set non-zero if we apply cross fibre tension and implicit (in 2/3D)
double dTdE_coeff_n1 = 0.0; // only set non-zero if we apply cross fibre tension in 3D
double dTdE_coeff_n2 = 0.0; // only set non-zero if we apply cross fibre tension in 3D
double dTdE_coeff_n3 = 0.0; // only set non-zero if we apply cross fibre tension in 3D and implicit
if(IsImplicitSolver())
{
double dt = mNextTime-mCurrentTime;
//std::cout << "d sigma / d lamda = " << d_act_tension_dlam << ", d sigma / d lamdat = " << d_act_tension_d_dlamdt << "\n" << std::flush;
dTdE_coeff1 += (d_act_tension_dlam + d_act_tension_d_dlamdt/dt)*detF/(lambda_fibre*I4_fibre); // note: I4_fibre*lam = lam^3
}
bool apply_cross_fibre_tension = (this->mrElectroMechanicsProblemDefinition.GetApplyCrossFibreTension()) && (DIM > 1);
if(apply_cross_fibre_tension)
{
double sheet_cross_fraction = mrElectroMechanicsProblemDefinition.GetSheetTensionFraction();
for(unsigned i=0; i<DIM; i++)
{
mCurrentElementSheetDirection(i) = this->mChangeOfBasisMatrix(i,1);
}
double I4_sheet = inner_prod(mCurrentElementSheetDirection, prod(rC, mCurrentElementSheetDirection));
// amend the stress and dTdE using the active tension
dTdE_coeff_s1 = -2*sheet_cross_fraction*detF*active_tension/(I4_sheet*I4_sheet); // note: I4*I4 = lam^4
if(IsImplicitSolver())
{
double dt = mNextTime-mCurrentTime;
dTdE_coeff_s3 = sheet_cross_fraction*(d_act_tension_dlam + d_act_tension_d_dlamdt/dt)*detF/(lambda_fibre*I4_sheet); // note: I4*lam = lam^3
}
rT += sheet_cross_fraction*(active_tension*detF/I4_sheet)*outer_prod(mCurrentElementSheetDirection,mCurrentElementSheetDirection);
dTdE_coeff_s2 = active_tension*sheet_cross_fraction*detF/I4_sheet;
if (DIM>2)
{
double sheet_normal_cross_fraction = mrElectroMechanicsProblemDefinition.GetSheetNormalTensionFraction();
for(unsigned i=0; i<DIM; i++)
{
mCurrentElementSheetNormalDirection(i) = this->mChangeOfBasisMatrix(i,2);
}
double I4_sheet_normal = inner_prod(mCurrentElementSheetNormalDirection, prod(rC, mCurrentElementSheetNormalDirection));
dTdE_coeff_n1 =-2*sheet_normal_cross_fraction*detF*active_tension/(I4_sheet_normal*I4_sheet_normal); // note: I4*I4 = lam^4
rT += sheet_normal_cross_fraction*(active_tension*detF/I4_sheet_normal)*outer_prod(mCurrentElementSheetNormalDirection,mCurrentElementSheetNormalDirection);
dTdE_coeff_n2 = active_tension*sheet_normal_cross_fraction*detF/I4_sheet_normal;
if(IsImplicitSolver())
{
double dt = mNextTime-mCurrentTime;
dTdE_coeff_n3 = sheet_normal_cross_fraction*(d_act_tension_dlam + d_act_tension_d_dlamdt/dt)*detF/(lambda_fibre*I4_sheet_normal); // note: I4*lam = lam^3
}
}
}
if(addToDTdE)
{
c_matrix<double,DIM,DIM> invC = Inverse(rC);
for (unsigned M=0; M<DIM; M++)
{
for (unsigned N=0; N<DIM; N++)
{
for (unsigned P=0; P<DIM; P++)
{
for (unsigned Q=0; Q<DIM; Q++)
{
rDTdE(M,N,P,Q) += dTdE_coeff1 * mCurrentElementFibreDirection(M)
* mCurrentElementFibreDirection(N)
* mCurrentElementFibreDirection(P)
* mCurrentElementFibreDirection(Q)
+ dTdE_coeff2 * mCurrentElementFibreDirection(M)
* mCurrentElementFibreDirection(N)
* invC(P,Q);
if(apply_cross_fibre_tension)
{
rDTdE(M,N,P,Q) += dTdE_coeff_s1 * mCurrentElementSheetDirection(M)
* mCurrentElementSheetDirection(N)
* mCurrentElementSheetDirection(P)
* mCurrentElementSheetDirection(Q)
+ dTdE_coeff_s2 * mCurrentElementSheetDirection(M)
* mCurrentElementSheetDirection(N)
* invC(P,Q)
+ dTdE_coeff_s3 * mCurrentElementSheetDirection(M)
* mCurrentElementSheetDirection(N)
* mCurrentElementFibreDirection(P)
* mCurrentElementFibreDirection(Q);
if (DIM>2)
{
rDTdE(M,N,P,Q) += dTdE_coeff_n1 * mCurrentElementSheetNormalDirection(M)
* mCurrentElementSheetNormalDirection(N)
* mCurrentElementSheetNormalDirection(P)
* mCurrentElementSheetNormalDirection(Q)
+ dTdE_coeff_n2 * mCurrentElementSheetNormalDirection(M)
* mCurrentElementSheetNormalDirection(N)
* invC(P,Q)
+ dTdE_coeff_n3 * mCurrentElementSheetNormalDirection(M)
* mCurrentElementSheetNormalDirection(N)
* mCurrentElementFibreDirection(P)
* mCurrentElementFibreDirection(Q);
}
}
}
}
}
}
}
// ///\todo #2180 The code below applies a cross fibre tension in the 2D case. Things that need doing:
// // * Refactor the common code between the block below and the block above to avoid duplication.
// // * Handle the 3D case.
// if(this->mrElectroMechanicsProblemDefinition.GetApplyCrossFibreTension() && DIM > 1)
// {
// double sheet_cross_fraction = mrElectroMechanicsProblemDefinition.GetSheetTensionFraction();
//
// for(unsigned i=0; i<DIM; i++)
// {
// mCurrentElementSheetDirection(i) = this->mChangeOfBasisMatrix(i,1);
// }
//
// double I4_sheet = inner_prod(mCurrentElementSheetDirection, prod(rC, mCurrentElementSheetDirection));
//
// // amend the stress and dTdE using the active tension
// double dTdE_coeff_s1 = -2*sheet_cross_fraction*detF*active_tension/(I4_sheet*I4_sheet); // note: I4*I4 = lam^4
//
// ///\todo #2180 The code below is specific to the implicit cardiac mechanics solver. Currently
// // the cross-fibre code is only tested using the explicit solver so the code below fails coverage.
// // This will need to be added back in once an implicit test is in place.
// double lambda_sheet = sqrt(I4_sheet);
// if(IsImplicitSolver())
// {
// double dt = mNextTime-mCurrentTime;
// dTdE_coeff_s1 += (d_act_tension_dlam + d_act_tension_d_dlamdt/dt)/(lambda_sheet*I4_sheet); // note: I4*lam = lam^3
// }
//
// rT += sheet_cross_fraction*(active_tension*detF/I4_sheet)*outer_prod(mCurrentElementSheetDirection,mCurrentElementSheetDirection);
//
// double dTdE_coeff_s2 = active_tension*detF/I4_sheet;
// if(addToDTdE)
// {
// for (unsigned M=0; M<DIM; M++)
// {
// for (unsigned N=0; N<DIM; N++)
// {
// for (unsigned P=0; P<DIM; P++)
// {
// for (unsigned Q=0; Q<DIM; Q++)
// {
// rDTdE(M,N,P,Q) += dTdE_coeff_s1 * mCurrentElementSheetDirection(M)
// * mCurrentElementSheetDirection(N)
// * mCurrentElementSheetDirection(P)
// * mCurrentElementSheetDirection(Q)
//
// + dTdE_coeff_s2 * mCurrentElementFibreDirection(M)
// * mCurrentElementFibreDirection(N)
// * invC(P,Q);
//
// }
// }
// }
// }
// }
// }
}
void LiftedSQPInternal::init(){
// Call the init method of the base class
NlpSolverInternal::init();
// Number of lifted variables
nv = getOption("num_lifted");
if(verbose_){
cout << "Initializing SQP method with " << nx_ << " variables and " << ng_ << " constraints." << endl;
cout << "Lifting " << nv << " variables." << endl;
if(gauss_newton_){
cout << "Gauss-Newton objective with " << F_.input().numel() << " terms." << endl;
}
}
// Read options
max_iter_ = getOption("max_iter");
max_iter_ls_ = getOption("max_iter_ls");
toldx_ = getOption("toldx");
tolgl_ = getOption("tolgl");
sigma_ = getOption("sigma");
rho_ = getOption("rho");
mu_safety_ = getOption("mu_safety");
eta_ = getOption("eta");
tau_ = getOption("tau");
// Assume SXFunction for now
SXFunction ffcn = shared_cast<SXFunction>(F_);
casadi_assert(!ffcn.isNull());
SXFunction gfcn = shared_cast<SXFunction>(G_);
casadi_assert(!gfcn.isNull());
// Extract the free variables and split into independent and dependent variables
SX x = ffcn.inputExpr(0);
int nx = x.size();
nu = nx-nv;
SX u = x[Slice(0,nu)];
SX v = x[Slice(nu,nu+nv)];
// Extract the constraint equations and split into constraints and definitions of dependent variables
SX f1 = ffcn.outputExpr(0);
int nf1 = f1.numel();
SX g = gfcn.outputExpr(0);
int nf2 = g.numel()-nv;
SX v_eq = g(Slice(0,nv));
SX f2 = g(Slice(nv,nv+nf2));
// Definition of v
SX v_def = v_eq + v;
// Objective function
SX f;
// Multipliers
SX lam_x, lam_g, lam_f2;
if(gauss_newton_){
// Least square objective
f = inner_prod(f1,f1)/2;
} else {
// Scalar objective function
f = f1;
// Lagrange multipliers for the simple bounds on u
SX lam_u = ssym("lam_u",nu);
// Lagrange multipliers for the simple bounds on v
SX lam_v = ssym("lam_v",nv);
// Lagrange multipliers for the simple bounds on x
lam_x = vertcat(lam_u,lam_v);
// Lagrange multipliers corresponding to the definition of the dependent variables
SX lam_v_eq = ssym("lam_v_eq",nv);
// Lagrange multipliers for the nonlinear constraints that aren't eliminated
lam_f2 = ssym("lam_f2",nf2);
if(verbose_){
cout << "Allocated intermediate variables." << endl;
}
// Lagrange multipliers for constraints
lam_g = vertcat(lam_v_eq,lam_f2);
// Lagrangian function
SX lag = f + inner_prod(lam_x,x);
if(!f2.empty()) lag += inner_prod(lam_f2,f2);
if(!v.empty()) lag += inner_prod(lam_v_eq,v_def);
// Gradient of the Lagrangian
SX lgrad = casadi::gradient(lag,x);
if(!v.empty()) lgrad -= vertcat(SX::zeros(nu),lam_v_eq); // Put here to ensure that lgrad is of the form "h_extended -v_extended"
makeDense(lgrad);
if(verbose_){
cout << "Generated the gradient of the Lagrangian." << endl;
}
// Condensed gradient of the Lagrangian
f1 = lgrad[Slice(0,nu)];
nf1 = nu;
// Gradient of h
SX v_eq_grad = lgrad[Slice(nu,nu+nv)];
// Reverse lam_v_eq and v_eq_grad
SX v_eq_grad_reversed = v_eq_grad;
copy(v_eq_grad.rbegin(),v_eq_grad.rend(),v_eq_grad_reversed.begin());
SX lam_v_eq_reversed = lam_v_eq;
copy(lam_v_eq.rbegin(),lam_v_eq.rend(),lam_v_eq_reversed.begin());
// Augment h and lam_v_eq
v_eq.append(v_eq_grad_reversed);
v.append(lam_v_eq_reversed);
}
// Residual function G
SXVector G_in(G_NUM_IN);
G_in[G_X] = x;
G_in[G_LAM_X] = lam_x;
G_in[G_LAM_G] = lam_g;
SXVector G_out(G_NUM_OUT);
G_out[G_D] = v_eq;
G_out[G_G] = g;
G_out[G_F] = f;
rfcn_ = SXFunction(G_in,G_out);
rfcn_.setOption("number_of_fwd_dir",0);
rfcn_.setOption("number_of_adj_dir",0);
rfcn_.setOption("live_variables",true);
rfcn_.init();
if(verbose_){
cout << "Generated residual function ( " << shared_cast<SXFunction>(rfcn_).getAlgorithmSize() << " nodes)." << endl;
}
// Difference vector d
SX d = ssym("d",nv);
if(!gauss_newton_){
vector<SX> dg = ssym("dg",nv).data();
reverse(dg.begin(),dg.end());
d.append(dg);
}
// Substitute out the v from the h
SX d_def = (v_eq + v)-d;
SXVector ex(3);
ex[0] = f1;
ex[1] = f2;
ex[2] = f;
substituteInPlace(v, d_def, ex, false);
SX f1_z = ex[0];
SX f2_z = ex[1];
SX f_z = ex[2];
// Modified function Z
enum ZIn{Z_U,Z_D,Z_LAM_X,Z_LAM_F2,Z_NUM_IN};
SXVector zfcn_in(Z_NUM_IN);
zfcn_in[Z_U] = u;
zfcn_in[Z_D] = d;
zfcn_in[Z_LAM_X] = lam_x;
zfcn_in[Z_LAM_F2] = lam_f2;
enum ZOut{Z_D_DEF,Z_F12,Z_NUM_OUT};
SXVector zfcn_out(Z_NUM_OUT);
zfcn_out[Z_D_DEF] = d_def;
zfcn_out[Z_F12] = vertcat(f1_z,f2_z);
SXFunction zfcn(zfcn_in,zfcn_out);
zfcn.init();
if(verbose_){
cout << "Generated reconstruction function ( " << zfcn.getAlgorithmSize() << " nodes)." << endl;
}
// Matrix A and B in lifted Newton
SX B = zfcn.jac(Z_U,Z_F12);
SX B1 = B(Slice(0,nf1),Slice(0,B.size2()));
SX B2 = B(Slice(nf1,B.size1()),Slice(0,B.size2()));
if(verbose_){
cout << "Formed B1 (dimension " << B1.size1() << "-by-" << B1.size2() << ", "<< B1.size() << " nonzeros) " <<
"and B2 (dimension " << B2.size1() << "-by-" << B2.size2() << ", "<< B2.size() << " nonzeros)." << endl;
}
// Step in u
SX du = ssym("du",nu);
SX dlam_f2 = ssym("dlam_f2",lam_f2.sparsity());
SX b1 = f1_z;
SX b2 = f2_z;
SX e;
if(nv > 0){
// Directional derivative of Z
vector<vector<SX> > Z_fwdSeed(2,zfcn_in);
vector<vector<SX> > Z_fwdSens(2,zfcn_out);
vector<vector<SX> > Z_adjSeed;
vector<vector<SX> > Z_adjSens;
Z_fwdSeed[0][Z_U].setZero();
Z_fwdSeed[0][Z_D] = -d;
Z_fwdSeed[0][Z_LAM_X].setZero();
Z_fwdSeed[0][Z_LAM_F2].setZero();
Z_fwdSeed[1][Z_U] = du;
Z_fwdSeed[1][Z_D] = -d;
Z_fwdSeed[1][Z_LAM_X].setZero();
Z_fwdSeed[1][Z_LAM_F2] = dlam_f2;
zfcn.eval(zfcn_in,zfcn_out,Z_fwdSeed,Z_fwdSens,Z_adjSeed,Z_adjSens);
b1 += Z_fwdSens[0][Z_F12](Slice(0,nf1));
b2 += Z_fwdSens[0][Z_F12](Slice(nf1,B.size1()));
e = Z_fwdSens[1][Z_D_DEF];
}
if(verbose_){
cout << "Formed b1 (dimension " << b1.size1() << "-by-" << b1.size2() << ", "<< b1.size() << " nonzeros) " <<
"and b2 (dimension " << b2.size1() << "-by-" << b2.size2() << ", "<< b2.size() << " nonzeros)." << endl;
}
// Generate Gauss-Newton Hessian
if(gauss_newton_){
b1 = mul(trans(B1),b1);
B1 = mul(trans(B1),B1);
if(verbose_){
cout << "Gauss Newton Hessian (dimension " << B1.size1() << "-by-" << B1.size2() << ", "<< B1.size() << " nonzeros)." << endl;
}
}
// Make sure b1 and b2 are dense vectors
makeDense(b1);
makeDense(b2);
// Quadratic approximation
SXVector lfcn_in(LIN_NUM_IN);
lfcn_in[LIN_X] = x;
lfcn_in[LIN_D] = d;
lfcn_in[LIN_LAM_X] = lam_x;
lfcn_in[LIN_LAM_G] = lam_g;
SXVector lfcn_out(LIN_NUM_OUT);
lfcn_out[LIN_F1] = b1;
lfcn_out[LIN_J1] = B1;
lfcn_out[LIN_F2] = b2;
lfcn_out[LIN_J2] = B2;
lfcn_ = SXFunction(lfcn_in,lfcn_out);
// lfcn_.setOption("verbose",true);
lfcn_.setOption("number_of_fwd_dir",0);
lfcn_.setOption("number_of_adj_dir",0);
lfcn_.setOption("live_variables",true);
lfcn_.init();
if(verbose_){
cout << "Generated linearization function ( " << shared_cast<SXFunction>(lfcn_).getAlgorithmSize() << " nodes)." << endl;
}
// Step expansion
SXVector efcn_in(EXP_NUM_IN);
copy(lfcn_in.begin(),lfcn_in.end(),efcn_in.begin());
efcn_in[EXP_DU] = du;
efcn_in[EXP_DLAM_F2] = dlam_f2;
efcn_ = SXFunction(efcn_in,e);
efcn_.setOption("number_of_fwd_dir",0);
efcn_.setOption("number_of_adj_dir",0);
efcn_.setOption("live_variables",true);
efcn_.init();
if(verbose_){
cout << "Generated step expansion function ( " << shared_cast<SXFunction>(efcn_).getAlgorithmSize() << " nodes)." << endl;
}
// Current guess for the primal solution
DMatrix &x_k = output(NLP_SOLVER_X);
// Current guess for the dual solution
DMatrix &lam_x_k = output(NLP_SOLVER_LAM_X);
DMatrix &lam_g_k = output(NLP_SOLVER_LAM_G);
// Allocate a QP solver
QpSolverCreator qp_solver_creator = getOption("qp_solver");
qp_solver_ = qp_solver_creator(B1.sparsity(),B2.sparsity());
// Set options if provided
if(hasSetOption("qp_solver_options")){
Dictionary qp_solver_options = getOption("qp_solver_options");
qp_solver_.setOption(qp_solver_options);
}
// Initialize the QP solver
qp_solver_.init();
if(verbose_){
cout << "Allocated QP solver." << endl;
}
// Residual
d_k_ = DMatrix(d.sparsity(),0);
// Primal step
dx_k_ = DMatrix(x_k.sparsity());
// Dual step
dlam_x_k_ = DMatrix(lam_x_k.sparsity());
dlam_g_k_ = DMatrix(lam_g_k.sparsity());
}
void ContactDomainUtilities::CalculateBaseDistances (std::vector<BaseLengths>& BaseVector,
PointType& P1,
PointType& P2,
PointType& P3,
PointType& PS,
PointType& Normal)
{
BaseVector[0].L=norm_2(P2-P1);
BaseVector[1].L=norm_2(P3-P2);
BaseVector[2].L=norm_2(P1-P3);
//the normal points from the side to the slave node:
PointType V1;
PointType V2;
// V1 = P2-P1;
// V2 = P3-P1;
// MathUtils<double>::CrossProduct(Normal,V1,V2);
// if( norm_2(Normal) != 0 )
// Normal/=norm_2(Normal);
//projection of the slave on the master plane:
PointType PPS = PS-P1;
PPS-=Normal*(inner_prod(PPS,Normal));
PPS+=P1;
//Comtutacion of the line bases:
//BaseVector[0]:
V1 = P2-P1;
V2 = P3-P2;
V1 /= norm_2(V1);
V2 /= norm_2(V2);
CalculateLineIntersection(BaseVector[0].B, P1, PPS, V1, V2 );
BaseVector[0].A = BaseVector[0].L-BaseVector[0].B;
//BaseVector[1]:
V1 = P3-P2;
V2 = P1-P3;
V1 /= norm_2(V1);
V2 /= norm_2(V2);
CalculateLineIntersection(BaseVector[1].B, P2, PPS, V1, V2 );
BaseVector[1].A = BaseVector[1].L-BaseVector[1].B;
//BaseVector[2]:
V1 = P1-P3;
V2 = P2-P1;
V1 /= norm_2(V1);
V2 /= norm_2(V2);
CalculateLineIntersection(BaseVector[1].B, P3, PPS, V1, V2 );
BaseVector[2].A = BaseVector[2].L-BaseVector[2].B;
std::cout<<" BaseVector 0-> L: "<<BaseVector[0].L<<" A: "<<BaseVector[0].A<<" B: "<<BaseVector[0].B<<std::endl;
std::cout<<" BaseVector 1-> L: "<<BaseVector[1].L<<" A: "<<BaseVector[1].A<<" B: "<<BaseVector[1].B<<std::endl;
std::cout<<" BaseVector 2-> L: "<<BaseVector[2].L<<" A: "<<BaseVector[2].A<<" B: "<<BaseVector[2].B<<std::endl;
}
c_vector<double,2> CryptProjectionForce::CalculateForceBetweenNodes(unsigned nodeAGlobalIndex, unsigned nodeBGlobalIndex, AbstractCellPopulation<2>& rCellPopulation)
{
MeshBasedCellPopulation<2>* p_static_cast_cell_population = static_cast<MeshBasedCellPopulation<2>*>(&rCellPopulation);
// We should only ever calculate the force between two distinct nodes
assert(nodeAGlobalIndex != nodeBGlobalIndex);
// Get the node locations in 2D
c_vector<double,2> node_a_location_2d = rCellPopulation.GetNode(nodeAGlobalIndex)->rGetLocation();
c_vector<double,2> node_b_location_2d = rCellPopulation.GetNode(nodeBGlobalIndex)->rGetLocation();
// "Get the unit vector parallel to the line joining the two nodes" [GeneralisedLinearSpringForce]
// Create a unit vector in the direction of the 3D spring
c_vector<double,3> unit_difference = mNode3dLocationMap[nodeBGlobalIndex] - mNode3dLocationMap[nodeAGlobalIndex];
// Calculate the distance between the two nodes
double distance_between_nodes = norm_2(unit_difference);
assert(distance_between_nodes > 0);
assert(!std::isnan(distance_between_nodes));
unit_difference /= distance_between_nodes;
// If mUseCutOffLength has been set, then there is zero force between
// two nodes located a distance apart greater than mUseCutOffLength
if (this->mUseCutOffLength)
{
if (distance_between_nodes >= mMechanicsCutOffLength)
{
// Return zero (2D projected) force
return zero_vector<double>(2);
}
}
// Calculate the rest length of the spring connecting the two nodes
double rest_length = 1.0;
CellPtr p_cell_A = rCellPopulation.GetCellUsingLocationIndex(nodeAGlobalIndex);
CellPtr p_cell_B = rCellPopulation.GetCellUsingLocationIndex(nodeBGlobalIndex);
double ageA = p_cell_A->GetAge();
double ageB = p_cell_B->GetAge();
assert(!std::isnan(ageA));
assert(!std::isnan(ageB));
/*
* If the cells are both newly divided, then the rest length of the spring
* connecting them grows linearly with time, until mMeinekeSpringGrowthDuration hour after division.
*/
if (ageA < mMeinekeSpringGrowthDuration && ageB < mMeinekeSpringGrowthDuration)
{
/*
* The spring rest length increases from a predefined small parameter
* to a normal rest length of 1.0, over a period of one hour.
*/
std::pair<CellPtr,CellPtr> cell_pair = p_static_cast_cell_population->CreateCellPair(p_cell_A, p_cell_B);
if (p_static_cast_cell_population->IsMarkedSpring(cell_pair))
{
double lambda = mMeinekeDivisionRestingSpringLength;
rest_length = lambda + (1.0 - lambda) * ageA/mMeinekeSpringGrowthDuration;
}
if (ageA+SimulationTime::Instance()->GetTimeStep() >= mMeinekeSpringGrowthDuration)
{
// This spring is about to go out of scope
p_static_cast_cell_population->UnmarkSpring(cell_pair);
}
}
double a_rest_length = rest_length*0.5;
double b_rest_length = a_rest_length;
/*
* If either of the cells has begun apoptosis, then the length of the spring
* connecting them decreases linearly with time.
*/
if (p_cell_A->HasApoptosisBegun())
{
double time_until_death_a = p_cell_A->GetTimeUntilDeath();
a_rest_length = a_rest_length * time_until_death_a / p_cell_A->GetApoptosisTime();
}
if (p_cell_B->HasApoptosisBegun())
{
double time_until_death_b = p_cell_B->GetTimeUntilDeath();
b_rest_length = b_rest_length * time_until_death_b / p_cell_B->GetApoptosisTime();
}
rest_length = a_rest_length + b_rest_length;
// Assert that the rest length does not exceed 1
assert(rest_length <= 1.0+1e-12);
bool is_closer_than_rest_length = true;
if (distance_between_nodes - rest_length > 0)
{
is_closer_than_rest_length = false;
}
/*
* Although in this class the 'spring constant' is a constant parameter, in
* subclasses it can depend on properties of each of the cells.
*/
double multiplication_factor = 1.0;
multiplication_factor *= VariableSpringConstantMultiplicationFactor(nodeAGlobalIndex, nodeBGlobalIndex, rCellPopulation, is_closer_than_rest_length);
// Calculate the 3D force between the two points
c_vector<double,3> force_between_nodes = multiplication_factor * this->GetMeinekeSpringStiffness() * unit_difference * (distance_between_nodes - rest_length);
// Calculate an outward normal unit vector to the tangent plane of the crypt surface at the 3D point corresponding to node B
c_vector<double,3> outward_normal_unit_vector;
double dfdr = CalculateCryptSurfaceDerivativeAtPoint(node_b_location_2d);
double theta_B = atan2(node_b_location_2d[1], node_b_location_2d[0]); // use atan2 to determine the quadrant
double normalization_factor = sqrt(1 + dfdr*dfdr);
outward_normal_unit_vector[0] = dfdr*cos(theta_B)/normalization_factor;
outward_normal_unit_vector[1] = dfdr*sin(theta_B)/normalization_factor;
outward_normal_unit_vector[2] = -1.0/normalization_factor;
// Calculate the projection of the force onto the plane z=0
c_vector<double,2> projected_force_between_nodes_2d;
double force_dot_normal = inner_prod(force_between_nodes, outward_normal_unit_vector);
for (unsigned i=0; i<2; i++)
{
projected_force_between_nodes_2d[i] = force_between_nodes[i]
- force_dot_normal*outward_normal_unit_vector[i]
+ force_dot_normal*outward_normal_unit_vector[2];
}
return projected_force_between_nodes_2d;
}
void open_loop_nbest_pitch(spx_sig_t *sw, int start, int end, int len, int *pitch, spx_word16_t *gain, int N, char *stack)
{
int i,j,k;
VARDECL(spx_word32_t *best_score);
spx_word32_t e0;
VARDECL(spx_word32_t *corr);
VARDECL(spx_word32_t *energy);
VARDECL(spx_word32_t *score);
#ifdef FIXED_POINT
VARDECL(spx_word16_t *swn2);
#endif
spx_word16_t *swn;
ALLOC(best_score, N, spx_word32_t);
ALLOC(corr, end-start+1, spx_word32_t);
ALLOC(energy, end-start+2, spx_word32_t);
ALLOC(score, end-start+1, spx_word32_t);
#ifdef FIXED_POINT
ALLOC(swn2, end+len, spx_word16_t);
normalize16(sw-end, swn2, 16384, end+len);
swn = swn2 + end;
#else
swn = sw;
#endif
for (i=0; i<N; i++)
{
best_score[i]=-1;
pitch[i]=start;
}
energy[0]=inner_prod(swn-start, swn-start, len);
e0=inner_prod(swn, swn, len);
for (i=start; i<=end; i++)
{
/* Update energy for next pitch*/
energy[i-start+1] = SUB32(ADD32(energy[i-start],SHR32(MULT16_16(swn[-i-1],swn[-i-1]),6)), SHR32(MULT16_16(swn[-i+len-1],swn[-i+len-1]),6));
}
pitch_xcorr(swn, swn-end, corr, len, end-start+1, stack);
#ifdef FIXED_POINT
{
VARDECL(spx_word16_t *corr16);
VARDECL(spx_word16_t *ener16);
ALLOC(corr16, end-start+1, spx_word16_t);
ALLOC(ener16, end-start+1, spx_word16_t);
normalize16(corr, corr16, 16384, end-start+1);
normalize16(energy, ener16, 16384, end-start+1);
for (i=start; i<=end; i++)
{
spx_word16_t g;
spx_word32_t tmp;
tmp = corr16[i-start];
if (tmp>0)
{
if (SHR16(corr16[i-start],4)>ener16[i-start])
tmp = SHL32(EXTEND32(ener16[i-start]),14);
else if (-SHR16(corr16[i-start],4)>ener16[i-start])
tmp = -SHL32(EXTEND32(ener16[i-start]),14);
else
tmp = SHL32(tmp,10);
g = DIV32_16(tmp, 8+ener16[i-start]);
score[i-start] = MULT16_16(corr16[i-start],g);
} else
{
score[i-start] = 1;
}
}
}
#else
for (i=start; i<=end; i++)
{
float g = corr[i-start]/(1+energy[i-start]);
if (g>16)
g = 16;
else if (g<-16)
g = -16;
score[i-start] = g*corr[i-start];
}
#endif
/* Extract best scores */
for (i=start; i<=end; i++)
{
if (score[i-start]>best_score[N-1])
{
for (j=0; j<N; j++)
{
if (score[i-start] > best_score[j])
{
for (k=N-1; k>j; k--)
{
best_score[k]=best_score[k-1];
pitch[k]=pitch[k-1];
}
best_score[j]=score[i-start];
pitch[j]=i;
break;
}
}
}
}
/* Compute open-loop gain */
if (gain)
{
for (j=0; j<N; j++)
{
spx_word16_t g;
i=pitch[j];
g = DIV32(corr[i-start], 10+SHR32(MULT16_16(spx_sqrt(e0),spx_sqrt(energy[i-start])),6));
/* FIXME: g = max(g,corr/energy) */
if (g<0)
g = 0;
gain[j]=g;
}
}
}
T norm_2(const vex::vector<T> &x) {
return sqrt(inner_prod(x, x));
}
void open_loop_nbest_pitch(spx_word16_t *sw, int start, int end, int len, int *pitch, spx_word16_t *gain, int N, char *stack)
{
int i,j,k;
VARDECL(spx_word32_t *best_score);
VARDECL(spx_word32_t *best_ener);
spx_word32_t e0;
VARDECL(spx_word32_t *corr);
#ifdef FIXED_POINT
/* In fixed-point, we need only one (temporary) array of 32-bit values and two (corr16, ener16)
arrays for (normalized) 16-bit values */
VARDECL(spx_word16_t *corr16);
VARDECL(spx_word16_t *ener16);
spx_word32_t *energy;
int cshift=0, eshift=0;
int scaledown = 0;
ALLOC(corr16, end-start+1, spx_word16_t);
ALLOC(ener16, end-start+1, spx_word16_t);
ALLOC(corr, end-start+1, spx_word32_t);
energy = corr;
#else
/* In floating-point, we need to float arrays and no normalized copies */
VARDECL(spx_word32_t *energy);
spx_word16_t *corr16;
spx_word16_t *ener16;
ALLOC(energy, end-start+2, spx_word32_t);
ALLOC(corr, end-start+1, spx_word32_t);
corr16 = corr;
ener16 = energy;
#endif
ALLOC(best_score, N, spx_word32_t);
ALLOC(best_ener, N, spx_word32_t);
for (i=0;i<N;i++)
{
best_score[i]=-1;
best_ener[i]=0;
pitch[i]=start;
}
#ifdef FIXED_POINT
for (i=-end;i<len;i++)
{
if (ABS16(sw[i])>16383)
{
scaledown=1;
break;
}
}
/* If the weighted input is close to saturation, then we scale it down */
if (scaledown)
{
for (i=-end;i<len;i++)
{
sw[i]=SHR16(sw[i],1);
}
}
#endif
energy[0]=inner_prod(sw-start, sw-start, len);
e0=inner_prod(sw, sw, len);
for (i=start;i<end;i++)
{
/* Update energy for next pitch*/
energy[i-start+1] = SUB32(ADD32(energy[i-start],SHR32(MULT16_16(sw[-i-1],sw[-i-1]),6)), SHR32(MULT16_16(sw[-i+len-1],sw[-i+len-1]),6));
if (energy[i-start+1] < 0)
energy[i-start+1] = 0;
}
#ifdef FIXED_POINT
eshift = normalize16(energy, ener16, 32766, end-start+1);
#endif
/* In fixed-point, this actually overrites the energy array (aliased to corr) */
pitch_xcorr(sw, sw-end, corr, len, end-start+1, stack);
#ifdef FIXED_POINT
/* Normalize to 180 so we can square it and it still fits in 16 bits */
cshift = normalize16(corr, corr16, 180, end-start+1);
/* If we scaled weighted input down, we need to scale it up again (OK, so we've just lost the LSB, who cares?) */
if (scaledown)
{
for (i=-end;i<len;i++)
{
sw[i]=SHL16(sw[i],1);
}
}
#endif
/* Search for the best pitch prediction gain */
for (i=start;i<=end;i++)
{
spx_word16_t tmp = MULT16_16_16(corr16[i-start],corr16[i-start]);
/* Instead of dividing the tmp by the energy, we multiply on the other side */
if (MULT16_16(tmp,best_ener[N-1])>MULT16_16(best_score[N-1],ADD16(1,ener16[i-start])))
{
/* We can safely put it last and then check */
best_score[N-1]=tmp;
best_ener[N-1]=ener16[i-start]+1;
pitch[N-1]=i;
/* Check if it comes in front of others */
for (j=0;j<N-1;j++)
{
if (MULT16_16(tmp,best_ener[j])>MULT16_16(best_score[j],ADD16(1,ener16[i-start])))
{
for (k=N-1;k>j;k--)
{
best_score[k]=best_score[k-1];
best_ener[k]=best_ener[k-1];
pitch[k]=pitch[k-1];
}
best_score[j]=tmp;
best_ener[j]=ener16[i-start]+1;
pitch[j]=i;
break;
}
}
}
}
/* Compute open-loop gain if necessary */
if (gain)
{
for (j=0;j<N;j++)
{
spx_word16_t g;
i=pitch[j];
g = DIV32(SHL32(EXTEND32(corr16[i-start]),cshift), 10+SHR32(MULT16_16(spx_sqrt(e0),spx_sqrt(SHL32(EXTEND32(ener16[i-start]),eshift))),6));
/* FIXME: g = max(g,corr/energy) */
if (g<0)
g = 0;
gain[j]=g;
}
}
}
bool MutableMesh<ELEMENT_DIM, SPACE_DIM>::CheckIsVoronoi(Element<ELEMENT_DIM, SPACE_DIM>* pElement, double maxPenetration)
{
assert(ELEMENT_DIM == SPACE_DIM);
unsigned num_nodes = pElement->GetNumNodes();
std::set<unsigned> neighbouring_elements_indices;
std::set< Element<ELEMENT_DIM,SPACE_DIM> *> neighbouring_elements;
std::set<unsigned> neighbouring_nodes_indices;
// Form a set of neighbouring elements via the nodes
for (unsigned i=0; i<num_nodes; i++)
{
Node<SPACE_DIM>* p_node = pElement->GetNode(i);
neighbouring_elements_indices = p_node->rGetContainingElementIndices();
for (std::set<unsigned>::const_iterator it = neighbouring_elements_indices.begin();
it != neighbouring_elements_indices.end();
++it)
{
neighbouring_elements.insert(this->GetElement(*it));
}
}
neighbouring_elements.erase(pElement);
// For each neighbouring element find the supporting nodes
typedef typename std::set<Element<ELEMENT_DIM,SPACE_DIM> *>::const_iterator ElementIterator;
for (ElementIterator it = neighbouring_elements.begin();
it != neighbouring_elements.end();
++it)
{
for (unsigned i=0; i<num_nodes; i++)
{
neighbouring_nodes_indices.insert((*it)->GetNodeGlobalIndex(i));
}
}
// Remove the nodes that support this element
for (unsigned i = 0; i < num_nodes; i++)
{
neighbouring_nodes_indices.erase(pElement->GetNodeGlobalIndex(i));
}
// Get the circumsphere information
c_vector<double, SPACE_DIM+1> this_circum_centre;
this_circum_centre = pElement->CalculateCircumsphere(this->mElementJacobians[pElement->GetIndex()], this->mElementInverseJacobians[pElement->GetIndex()]);
// Copy the actualy circumcentre into a smaller vector
c_vector<double, ELEMENT_DIM> circum_centre;
for (unsigned i=0; i<ELEMENT_DIM; i++)
{
circum_centre[i] = this_circum_centre[i];
}
for (std::set<unsigned>::const_iterator it = neighbouring_nodes_indices.begin();
it != neighbouring_nodes_indices.end();
++it)
{
c_vector<double, ELEMENT_DIM> node_location = this->GetNode(*it)->rGetLocation();
// Calculate vector from circumcenter to node
node_location -= circum_centre;
// This is to calculate the squared distance betweeen them
double squared_distance = inner_prod(node_location, node_location);
// If the squared idstance is less than the elements circum-radius(squared),
// then the Voronoi property is violated.
if (squared_distance < this_circum_centre[ELEMENT_DIM])
{
// We know the node is inside the circumsphere, but we don't know how far
double radius = sqrt(this_circum_centre[ELEMENT_DIM]);
double distance = radius - sqrt(squared_distance);
// If the node penetration is greater than supplied maximum penetration factor
if (distance/radius > maxPenetration)
{
return false;
}
}
}
return true;
}
typename promote_traits<typename V1::value_type, typename V2::value_type>::promote_type
dot (const V1 &v1, const V2 &v2) {
return inner_prod (v1, v2);
}
void LinearElastic3DLaw::CalculateMaterialResponsePK2 (Parameters& rValues)
{
//-----------------------------//
//a.-Check if the constitutive parameters are passed correctly to the law calculation
//CheckParameters(rValues);
//b.- Get Values to compute the constitutive law:
Flags &Options=rValues.GetOptions();
mStrainEnergy = 0.0; //When it is not calculated, a zero will be returned
const Properties& MaterialProperties = rValues.GetMaterialProperties();
Vector& StrainVector = rValues.GetStrainVector();
Vector& StressVector = rValues.GetStressVector();
//-----------------------------//
//1.- Lame constants
const double& YoungModulus = MaterialProperties[YOUNG_MODULUS];
const double& PoissonCoefficient = MaterialProperties[POISSON_RATIO];
// //1.1- Thermal constants
// double ThermalExpansionCoefficient = 0;
// if( MaterialProperties.Has(THERMAL_EXPANSION_COEFFICIENT) )
// ThermalExpansionCoefficient = MaterialProperties[THERMAL_EXPANSION_COEFFICIENT];
// double ReferenceTemperature = 0;
// if( MaterialProperties.Has(REFERENCE_TEMPERATURE) )
// ReferenceTemperature = MaterialProperties[REFERENCE_TEMPERATURE];
if(Options.Is( ConstitutiveLaw::COMPUTE_STRAIN )) //large strains
{
//1.-Compute total deformation gradient
const Matrix& DeformationGradientF = rValues.GetDeformationGradientF();
//2.-Right Cauchy-Green tensor C
Matrix RightCauchyGreen = prod(trans(DeformationGradientF),DeformationGradientF);
//3.-Green-Lagrange Strain:
//E= 0.5*(FT*F-1)
this->CalculateGreenLagrangeStrain(RightCauchyGreen,StrainVector);
}
//7.-Calculate Total PK2 stress
if( Options.Is( ConstitutiveLaw::COMPUTE_STRESS ) )
{
if( Options.Is( ConstitutiveLaw::COMPUTE_CONSTITUTIVE_TENSOR ) ){
Matrix& ConstitutiveMatrix = rValues.GetConstitutiveMatrix();
this->CalculateLinearElasticMatrix( ConstitutiveMatrix, YoungModulus, PoissonCoefficient );
this->CalculateStress( StrainVector, ConstitutiveMatrix, StressVector );
}
else {
Matrix ConstitutiveMatrix = ZeroMatrix( StrainVector.size(), StrainVector.size() );
this->CalculateLinearElasticMatrix( ConstitutiveMatrix, YoungModulus, PoissonCoefficient );
this->CalculateStress( StrainVector, ConstitutiveMatrix, StressVector );
}
}
else if( Options.IsNot( ConstitutiveLaw::COMPUTE_STRESS ) && Options.Is( ConstitutiveLaw::COMPUTE_CONSTITUTIVE_TENSOR ) )
{
Matrix& ConstitutiveMatrix = rValues.GetConstitutiveMatrix();
this->CalculateLinearElasticMatrix( ConstitutiveMatrix, YoungModulus, PoissonCoefficient );
}
if( Options.Is( ConstitutiveLaw::COMPUTE_STRAIN_ENERGY ) )
{
if( Options.IsNot( ConstitutiveLaw::COMPUTE_STRESS ) )
{
if(Options.Is( ConstitutiveLaw::COMPUTE_CONSTITUTIVE_TENSOR ))
{
Matrix ConstitutiveMatrix = ZeroMatrix( StrainVector.size(), StrainVector.size() );
this->CalculateLinearElasticMatrix( ConstitutiveMatrix, YoungModulus, PoissonCoefficient );
this->CalculateStress( StrainVector, ConstitutiveMatrix, StressVector );
}
else
{
Matrix& ConstitutiveMatrix = rValues.GetConstitutiveMatrix();
this->CalculateStress( StrainVector, ConstitutiveMatrix, StressVector );
}
}
mStrainEnergy = 0.5 * inner_prod(StrainVector,StressVector); //Belytschko Nonlinear Finite Elements pag 226 (5.4.3) : w = 0.5*E:C:E
}
//std::cout<<" Strain "<<StrainVector<<std::endl;
//std::cout<<" Stress "<<StressVector<<std::endl;
//Matrix& ConstitutiveMatrix = rValues.GetConstitutiveMatrix();
//std::cout<<" Constitutive "<<ConstitutiveMatrix<<std::endl;
}
