FLOTANTE * MetodoQR::getAutovalores(CONTADOR iteraciones, MatrizFullFull & matriz, FLOTANTE presicion) {
convertirSimetrica(matriz);
factorizarQR(matriz);
FLOTANTE ultimo = -matriz.get(0,0);
for(CONTADOR i = 0; i < iteraciones; i++) {
MatrizFullFull & res = multiplicarTridiagonales(getR(),getQ());
factorizarQR(res);
if(-(ultimo + res.get(0,0)) < presicion) {
break;
} else {
ultimo = -res.get(0,0);
}
res.~MatrizFullFull();
}
MatrizFullFull & res = multiplicar(getR(),getQ());
autovalores = new FLOTANTE[res.getFilas()];
for(CONTADOR i = 0; i < res.getFilas(); i++) {
autovalores[i] = res.get(i,i);
}
_nAutovalores = res.getFilas();
_autovaloresCalculados = true;
res.~MatrizFullFull();
return autovalores;
}
int main(int argc, char *argv[])
{
/* input pins to LUT KEY
1 = 0 100000
2 = 1 010000
4 = 2 001000
8 = 3 000100
16 = 4 000010
32 = 5 000001
Lookup table 0-127 (128 symbols) outputs LSB Q and !Q (via assert)
*/
// just for setup:
//outputter();
// debug ^^
Lut lut6;
const unsigned char MAXLUTIN = 252; // 000000** - 111111** inclusive.
const unsigned char MAXLUTOUT = 2; // ******00 - ******10 (so 01 and 10)
unsigned char i = 0;
for (i=0;i<=(MAXLUTIN+MAXLUTOUT);i++) {
if (get_bit_from_char(i, 0) != get_bit_from_char(i,1)) {
lut6.lut=i;
printf("%d\tQ=%d\tQ_PRIME=%d\t",lut6.lut,getQ(lut6.lut),!getQ(lut6.lut));
printf("BINARY: ");
outputter(i);
}
}
return 0;
}
double QgsDistanceArea::computePolygonArea( const QList<QgsPoint>& points ) const
{
double x1, y1, x2, y2, dx, dy;
double Qbar1, Qbar2;
double area;
QgsDebugMsgLevel( "Ellipsoid: " + mEllipsoid, 3 );
if (( ! mEllipsoidalMode ) || ( mEllipsoid == GEO_NONE ) )
{
return computePolygonFlatArea( points );
}
int n = points.size();
x2 = DEG2RAD( points[n-1].x() );
y2 = DEG2RAD( points[n-1].y() );
Qbar2 = getQbar( y2 );
area = 0.0;
for ( int i = 0; i < n; i++ )
{
x1 = x2;
y1 = y2;
Qbar1 = Qbar2;
x2 = DEG2RAD( points[i].x() );
y2 = DEG2RAD( points[i].y() );
Qbar2 = getQbar( y2 );
if ( x1 > x2 )
while ( x1 - x2 > M_PI )
x2 += m_TwoPI;
else if ( x2 > x1 )
while ( x2 - x1 > M_PI )
x1 += m_TwoPI;
dx = x2 - x1;
area += dx * ( m_Qp - getQ( y2 ) );
if (( dy = y2 - y1 ) != 0.0 )
area += dx * getQ( y2 ) - ( dx / dy ) * ( Qbar2 - Qbar1 );
}
if (( area *= m_AE ) < 0.0 )
area = -area;
/* kludge - if polygon circles the south pole the area will be
* computed as if it cirlced the north pole. The correction is
* the difference between total surface area of the earth and
* the "north pole" area.
*/
if ( area > m_E )
area = m_E;
if ( area > m_E / 2 )
area = m_E - area;
return area;
}
void QgsDistanceArea::computeAreaInit()
{
double a2 = ( mSemiMajor * mSemiMajor );
double e2 = 1 - ( a2 / ( mSemiMinor * mSemiMinor ) );
double e4, e6;
m_TwoPI = M_PI + M_PI;
e4 = e2 * e2;
e6 = e4 * e2;
m_AE = a2 * ( 1 - e2 );
m_QA = ( 2.0 / 3.0 ) * e2;
m_QB = ( 3.0 / 5.0 ) * e4;
m_QC = ( 4.0 / 7.0 ) * e6;
m_QbarA = -1.0 - ( 2.0 / 3.0 ) * e2 - ( 3.0 / 5.0 ) * e4 - ( 4.0 / 7.0 ) * e6;
m_QbarB = ( 2.0 / 9.0 ) * e2 + ( 2.0 / 5.0 ) * e4 + ( 4.0 / 7.0 ) * e6;
m_QbarC = - ( 3.0 / 25.0 ) * e4 - ( 12.0 / 35.0 ) * e6;
m_QbarD = ( 4.0 / 49.0 ) * e6;
m_Qp = getQ( M_PI / 2 );
m_E = 4 * M_PI * m_Qp * m_AE;
if ( m_E < 0.0 )
m_E = -m_E;
}
/**
* Returns the Euler angles in radians defined in the Aerospace sequence.
* See Sebastian O.H. Madwick report "An efficient orientation filter for
* inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation
*
* @param angles three floats array which will be populated by the Euler angles in radians
*/
void FreeIMU::getEulerRad(float * angles) {
float q[4]; // quaternion
getQ(q);
angles[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1); // psi
angles[1] = -asin(2 * q[1] * q[3] + 2 * q[0] * q[2]); // theta
angles[2] = atan2(2 * q[2] * q[3] - 2 * q[0] * q[1], 2 * q[0] * q[0] + 2 * q[3] * q[3] - 1); // phi
}
/* this tries to swap two neighbouring eigenvalues, 'it' and '--it',
* and returns 'itadd'. If the blocks can be swapped, new eigenvalues
* can emerge due to possible 2-2 block splits. 'it' then points to
* the last eigenvalue coming from block pointed by 'it' at the
* begining, and 'itadd' points to the first. On swap failure, 'it' is
* not changed, and 'itadd' points to previous eignevalue (which must
* be moved backwards before). In either case, it is necessary to
* resolve eigenvalues from 'itadd' to 'it', before the 'it' can be
* resolved.
* The success is signaled by returned true.
*/
bool SchurDecompEig::tryToSwap(diag_iter& it, diag_iter& itadd)
{
itadd = it;
--itadd;
lapack_int n = getDim();
lapack_int ifst = (*it).getIndex() + 1;
lapack_int ilst = (*itadd).getIndex() + 1;
double* work = new double[n];
lapack_int info;
dtrexc("V", &n, getT().base(), &n, getQ().base(), &n, &ifst, &ilst, work,
&info);
delete [] work;
if (info < 0) {
throw SYLV_MES_EXCEPTION("Wrong argument to dtrexc.");
}
if (info == 0) {
// swap successful
getT().swapDiagLogically(itadd);
//check for 2-2 block splits
getT().checkDiagConsistency(it);
getT().checkDiagConsistency(itadd);
// and go back by 'it' in NEW eigenvalue set
--it;
return true;
}
return false;
}
QString WMSMapAdapter::query(int i, int j, int /*z*/) const
{
return getQ(-180+i*coord_per_x_tile,
90-(j+1)*coord_per_y_tile,
-180+i*coord_per_x_tile+coord_per_x_tile,
90-(j+1)*coord_per_y_tile+coord_per_y_tile);
}
void QgsDistanceArea::computeAreaInit()
{
//don't try to perform calculations if no ellipsoid
if ( mEllipsoid == GEO_NONE )
{
return;
}
double a2 = ( mSemiMajor * mSemiMajor );
double e2 = 1 - ( a2 / ( mSemiMinor * mSemiMinor ) );
double e4, e6;
m_TwoPI = M_PI + M_PI;
e4 = e2 * e2;
e6 = e4 * e2;
m_AE = a2 * ( 1 - e2 );
m_QA = ( 2.0 / 3.0 ) * e2;
m_QB = ( 3.0 / 5.0 ) * e4;
m_QC = ( 4.0 / 7.0 ) * e6;
m_QbarA = -1.0 - ( 2.0 / 3.0 ) * e2 - ( 3.0 / 5.0 ) * e4 - ( 4.0 / 7.0 ) * e6;
m_QbarB = ( 2.0 / 9.0 ) * e2 + ( 2.0 / 5.0 ) * e4 + ( 4.0 / 7.0 ) * e6;
m_QbarC = - ( 3.0 / 25.0 ) * e4 - ( 12.0 / 35.0 ) * e6;
m_QbarD = ( 4.0 / 49.0 ) * e6;
m_Qp = getQ( M_PI / 2 );
m_E = 4 * M_PI * m_Qp * m_AE;
if ( m_E < 0.0 )
m_E = -m_E;
}
//==============================================================================
void FreeJoint::integratePositions(double _dt)
{
const Eigen::Isometry3d Qnext
= getQ() * convertToTransform(getVelocitiesStatic() * _dt);
setPositionsStatic(convertToPositions(Qnext));
}
void FreeIMU1::getQ(float * q) {
int i;
double dQ[4];
getQ(dQ);
for (i = 0; i < 4; i++)
q[i] = (float)dQ[i];
}
long* sub(long* ele1, long* ele2, long index)
{
int i=0;
long *ele3=emptyElement(index);
for(i=0;i<getDeg(index);i++)
{
ele3[i]=mod(ele1[i]-ele2[i]+getQ(index),index);
}
return ele3;
}
void HLayeredBlWStructure::fillMatrixFromP( gsl_matrix* c, const gsl_vector* p ) {
size_t sum_np = 0, sum_nl = 0, l_1, j;
gsl_vector psub;
for (l_1 = 0; l_1 < getQ(); sum_np += getLayerNp(l_1),
sum_nl += getLayerLag(l_1), ++l_1) {
for (j = 0; j < getLayerLag(l_1); ++j) {
psub = gsl_vector_const_subvector(p, sum_np + j, getN()).vector;
gsl_matrix_set_col(c, j + sum_nl, &psub);
}
}
}
long* randNormElement(long index)
{
long *elem;
int i=0;
float u,v,test; //variables needed to generate normal distribution
if( NULL ==(elem = malloc(getDeg(index)*sizeof(long))) ) //allocate required memory for the ring
{printf("not enough memory to generate Ring\n"); //if not enough memory is available prompt error
return (long*) -1;
}
for(i=0;i<getDeg(index);i++) //generate a random number for every parameter
{
u=(float)rand()/RAND_MAX;
v=(float)rand()/RAND_MAX;
test=(sqrt(-2*log(u))*cos(TWO_PI*v));
test=round(test*getQ(index)+0.5); //discretesize values
elem[i]=mod(test+getQ(index)*3,index); //adding 3 times the modulus to the number to make sure that it isnt negative
}
return elem;
}
void HLayeredBlWStructure::multByWInv( gsl_vector* p, long deg ) {
size_t l_1, k, sum_np = 0;
gsl_vector psub;
if (deg == 0) {
return;
}
for (l_1 = 0; l_1 < getQ(); sum_np += getLayerNp(l_1), ++l_1) {
psub = gsl_vector_subvector(p, sum_np, getLayerNp(l_1)).vector;
gsl_vector_scale(&psub, (deg == 2) ? getLayerInvWeight(l_1):
sqrt(getLayerInvWeight(l_1)));
}
}
/**
* Returns the yaw pitch and roll angles, respectively defined as the angles in radians between
* the Earth North and the IMU X axis (yaw), the Earth ground plane and the IMU X axis (pitch)
* and the Earth ground plane and the IMU Y axis.
*
* @note This is not an Euler representation: the rotations aren't consecutive rotations but only
* angles from Earth and the IMU. For Euler representation Yaw, Pitch and Roll see FreeIMU1::getEuler
*
* @param ypr three doubles array which will be populated by Yaw, Pitch and Roll angles in radians
*/
void FreeIMU1::getYawPitchRollRad(double * ypr) {
double q[4]; // quaternion
double gx, gy, gz; // estimated gravity direction
getQ(q);
gx = 2 * (q[1]*q[3] - q[0]*q[2]);
gy = 2 * (q[0]*q[1] + q[2]*q[3]);
gz = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
ypr[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1);
ypr[1] = atan(gx / sqrt(gy*gy + gz*gz));
ypr[2] = atan(gy / sqrt(gx*gx + gz*gz));
}
void processAudio(AudioInputBuffer &input, AudioOutputBuffer &output){
// update filter coefficients
float fn = getFrequency()/getSampleRate();
float Q = getQ();
float g = getDbGain();
peq.setCoeffsPEQ(fn, Q, g) ;
// get samples
int size = input.getSize();
float* inBuf = input.getSamples();
float* outBuf = output.getSamples();
// filter samples
peq.process(size, inBuf, outBuf);
}
void processAudio(AudioBuffer &buffer){
// update filter coefficients
float fn = getFrequency()/getSampleRate();
float Q = getQ();
float g = getDbGain();
peqL.setCoeffsPEQ(fn, Q, g) ;
peqR.setCoeffsPEQ(fn, Q, g) ;
// process
int size = buffer.getSize();
float* left = buffer.getSamples(0);
peqL.process(size, left);
float* right = buffer.getSamples(1);
peqR.process(size, right);
}
void EKFLocalization::run()
{
testGameEvents();
predict();
correct();
float q = getQ(ekf.filter);
//cerr<<"Q = "<<q<<"\t";
if(q<0.5)
{
list<tcellcand> cands;
list<tcellcand>::iterator itcands, mayor;
float qmayor=0.0;
cands.clear();
mgrid->getGridPos(cands);
if(cands.size()>0)
for(itcands=cands.begin(); itcands!=cands.end();++itcands)
if((*itcands).prob>qmayor)
{
mayor = itcands;
qmayor = (*itcands).prob;
}
//cerr<<"mayor = "<<qmayor<<"\t";
if(qmayor>0.35)
{
MatrixCM s(3,1);
s.sete(0,0,(*mayor).x);
s.sete(1,0,(*mayor).y);
s.sete(2,0,(*mayor).t);
MatrixCM Ppos(3);
Ppos.identity(3);
Ppos.sete(0,0,(*mayor).uxy);
Ppos.sete(1,1,(*mayor).uxy);
Ppos.sete(2,2,(*mayor).ut);
ekf.filter->restart(s, Ppos);
}
}
//cerr<<endl;
}
void ExtendedKalman::predict() {
{
Matrix F;
getF(F, X);
// X = F * X
X.dotSelf(F, true);
// P = F * P * F.T + Q
P.dotSelf(F, true).dotSelf(F.transposed());
}
{
Matrix Q;
getQ(Q);
P += Q;
}
}
//----------------------------------------------------------------------------------
void FunctionsSingleton::coutAll() const
{
std::cout << getD() << std::endl;
std::cout << getF() << std::endl;
std::cout << getG() << std::endl;
std::cout << getM() << std::endl;
std::cout << getN() << std::endl;
std::cout << getP() << std::endl;
std::cout << getQ() << std::endl;
std::cout << getR() << std::endl;
std::cout << getW() << std::endl;
std::cout << "Alpha: " << mAlpha << std::endl;
std::cout << "Beta: " << mBeta << std::endl;
std::cout << "Gamma: " << mGamma << std::endl;
}
void HLayeredBlWStructure::multByGtUnweighted( gsl_vector* p,
const gsl_matrix *Rt, const gsl_vector *y,
double alpha, double beta, bool skipFixedBlocks ) {
size_t l, k, sum_np = 0, sum_nl = 0, D = Rt->size2;
gsl_matrix Y = gsl_matrix_const_view_vector(y,getN(), D).matrix, RtSub;
gsl_vector Y_row, psub;
for (l = 0; l < getQ();
sum_np += getLayerNp(l), sum_nl += getLayerLag(l), ++l) {
RtSub = gsl_matrix_const_submatrix(Rt, sum_nl, 0, getLayerLag(l), D).matrix;
if (!(skipFixedBlocks && isLayerExact(l))) {
for (k = 0; k < getN(); k++) {
psub = gsl_vector_subvector(p, k + sum_np, getLayerLag(l)).vector;
Y_row = gsl_matrix_row(&Y, k).vector;
gsl_blas_dgemv(CblasNoTrans, alpha, &RtSub, &Y_row, beta, &psub);
}
}
}
}
int epsilonGreedy(double weights[], float s[DECAY_COUNT], double epsilon)
{
double Q[ACTION_COUNT];
getQ(Q, s, weights, num_tilings, memory_size);
int max, i;
int firstAction = 0;
int lastAction = 4;
if (rand()/((double)RAND_MAX+1) < epsilon) {
printf("random action\n\n");
return firstAction + rand()%(lastAction + 1 - firstAction);
} else {
max = lastAction;
printf("%.1lf %.1lf %.1lf %.1lf\n", Q[0], Q[1], Q[2], Q[3]);
for (i = firstAction; i <= lastAction; i++)
if (Q[i] > Q[max])
max = i;
return max;
}
}
void
EKFLocalization::getPosition(vector<tRobotHipothesis> &hipotheses)
{
hipotheses.clear();
tRobotHipothesis aux;
aux.x = ekf.filter->x();
aux.y = ekf.filter->y();
aux.t = ekf.filter->t();
aux.prob = getQ(ekf.filter);
aux.id = 0;
hipotheses.push_back(aux);
aux.x = -ekf.filter->x();
aux.y = -ekf.filter->y();
aux.t = normalizePi(ekf.filter->t()+M_PI);
aux.id = 1;
hipotheses.push_back(aux);
}
void HLayeredBlWStructure::computeVkParams() {
size_t k, l, i, imax, sum_nl, rep;
gsl_matrix *zk;
gsl_matrix_view wi, zkl;
myA = (gsl_matrix**) malloc(getMaxLag() * sizeof(gsl_matrix *));
/* construct w */
for (k = 0; k < getMaxLag(); k++) {
zk = gsl_matrix_alloc(getM(), getM());
gsl_matrix_set_zero(zk);
for (sum_nl = 0, l = 0; l < getQ(); sum_nl += getLayerLag(l), ++l) {
zkl = gsl_matrix_submatrix(zk, sum_nl, sum_nl, getLayerLag(l),
getLayerLag(l));
if (k < getLayerLag(l)) {
gsl_vector_view diag = gsl_matrix_subdiagonal(&zkl.matrix, k);
gsl_vector_set_all(&diag.vector, getLayerInvWeight(l));
}
}
myA[k] = zk;
}
}
// Get the output length
unsigned long DSAPrivateKey::getOutputLength() const
{
return getQ().size() * 2;
}
ParametricEqPatch() {
registerParameter(PARAMETER_A, "Freq");
registerParameter(PARAMETER_B, "Q");
registerParameter(PARAMETER_D, "Gain");
peq.setCoeffsPEQ(getFrequency()/getSampleRate(), getQ(), getDbGain()) ;
}
// Get the bit length
unsigned long ECPublicKey::getBitLength() const
{
return getQ().size() * 8;
}
int main(int argc, char *argv[]) {
/* Initialize variables used by the Agent thread */
int p;
int a, aprime;
float s[DECAY_COUNT], sprime[DECAY_COUNT];
int reward;
int tile_array[feature_count];
unsigned int i;
double delta;
double Q[ACTION_COUNT];
// Learning parameters
double stepsize = 0.1 / (float) num_tilings;
double lambda = 0.9;
double gamma = 0.9;
struct sigaction act;
struct sigaction oldact;
act.sa_handler = endProgram;
sigemptyset(&act.sa_mask);
act.sa_flags = 0;
sigaction(SIGINT, &act, &oldact);
srand(0);
pthread_mutex_init(&pktNumMutex, NULL);
pthread_mutex_init(&actionMutex, NULL);
pthread_mutex_init(&rewardMusicMutex, NULL);
trajectoryFile.open("trajectory.txt");
if(!trajectoryFile.is_open())
printf("Trajectory file could not be opened.\n");
/* Set up variables used by individual policy components */
// --- begin initialize variables for Tians code
timeStep = 0;
leftCount=0, rightCount =0;
diff = 0;
actionToTake = 1;
count = 0;
alignPhase = 1;
notInRightMode = false;
// --- end initialize variables for Tians code
// --- begin initialize variables for Amirs code
cwTurn = 1;
// --- end initialize variables for Amirs code
// initialize weights
// first try to read the weight file and if there is no file, then initialize randomly
if(!read_weights(weights)){
for (i = 0; i < memory_size; i++) {
weights[i] = -100.0/num_tilings;
}
}
for (i = 0; i < memory_size; i++) {
e[i] = 0;
}
// Set up timing + packet number
p = pktNum;
// State based on IR byte
s[0] = redDecay;
s[1] = greenDecay;
s[2] = bumpDecay;
s[3] = leftDecay;
s[4] = rightDecay;
s[5] = forwardDecay;
s[6] = backwardDecay;
s[7] = stopDecay;
s[8] = chargeDecay;
a = sAPrime[p];//epsilonGreedy(weights, s, epsilon);
// Use a lock to ensure action is changed separately
pthread_mutex_lock( &actionMutex );
action = a; // sets up action to be taken by csp thread
pthread_mutex_unlock( &actionMutex );
prevPktNum = myPktNum;
// Main agent loop
while (TRUE) {
int rval = getNextPacket();
if (rval == -1) {
write_weights(weights);
printf("Complete! Weights saved to weights.txt. Ran %d episodes.", episode + 1);
break;
} else if (rval == 1) {
// Episode complete
for (i = 0; i < memory_size; i++) {
e[i] = 0;
}
episode++;
}
// Get the packet number
p = pktNum;
// Update decays
updateDecay(p, prevPktNum, myPktNum);
//printf("ir: %d\n", sIRbyte[p]);
// Reward of -1 per step
reward = -1;
// Determine the next observation
// TODO: Change this to new state representation
sprime[0] = redDecay;
sprime[1] = greenDecay;
sprime[2] = bumpDecay;
sprime[3] = leftDecay;
sprime[4] = rightDecay;
sprime[5] = forwardDecay;
sprime[6] = backwardDecay;
sprime[7] = stopDecay;
sprime[8] = chargeDecay;
aprime = sAPrime[p];//epsilonGreedy(weights, sprime, epsilon);
// Set action variable
pthread_mutex_lock( &actionMutex );
action = aprime; // sets up action to be taken by csp thread
pthread_mutex_unlock( &actionMutex );
// Get Q values
getQ(Q, s, weights, num_tilings, memory_size);
delta = reward - Q[a];
getQ(Q, sprime, weights, num_tilings, memory_size);
delta += gamma * Q[aprime];
// Update weights
get_tiles(s, a, tile_array, num_tilings, memory_size);
for (i = 0; i < feature_count; i++) {
e[tile_array[i]] = 1;
}
//printf("Docking: s a r s' a':%d %d %d %d %d\n", s, a, reward, sprime, aprime);
for (i = 0; i < memory_size; i++ ) {
weights[i] += stepsize * delta * e[i];
e[i] *= lambda;
}
// Decay sensor traces
performDecay();
for (i = 0; i < DECAY_COUNT; i++) {
s[i] = sprime[i];
}
a = aprime;
prevPktNum = myPktNum;
}
return 0;
}
QString GoogleSatMapAdapter::query(int i, int j, int z) const
{
return getQ(-180+i*coord_per_x_tile,
90-(j+1)*coord_per_y_tile, z);
}
bool FractureElasticityVoigt::evalStress (double lambda, double mu, double Gc,
const SymmTensor& epsil, double* Phi,
SymmTensor* sigma, Matrix* dSdE,
bool postProc, bool printElm) const
{
PROFILE3("FractureEl::evalStress");
unsigned short int a = 0, b = 0;
// Define a Lambda-function to set up the isotropic constitutive matrix
auto&& setIsotropic = [this,a,b](Matrix& C, double lambda, double mu) mutable
{
for (a = 1; a <= C.rows(); a++)
if (a > nsd)
C(a,a) = mu;
else
{
C(a,a) = 2.0*mu;
for (b = 1; b <= nsd; b++)
C(a,b) += lambda;
}
};
// Define some material constants
double trEps = epsil.trace();
double C0 = trEps >= -epsZ ? Gc*lambda : lambda;
double Cp = Gc*mu;
if (trEps >= -epsZ && trEps <= epsZ)
{
// No strains, stress free configuration
Phi[0] = 0.0;
if (postProc)
Phi[1] = Phi[2] = Phi[3] = 0.0;
if (sigma)
*sigma = 0.0;
if (dSdE)
setIsotropic(*dSdE,C0,Cp);
return true;
}
// Calculate principal strains and the associated directions
Vec3 eps;
std::vector<SymmTensor> M(nsd,SymmTensor(nsd));
{
PROFILE4("Tensor::principal");
if (!epsil.principal(eps,M.data()))
return false;
}
// Split the strain tensor into positive and negative parts
SymmTensor ePos(nsd), eNeg(nsd);
for (a = 0; a < nsd; a++)
if (eps[a] > 0.0)
ePos += eps[a]*M[a];
else if (eps[a] < 0.0)
eNeg += eps[a]*M[a];
if (sigma)
{
// Evaluate the stress tensor
*sigma = C0*trEps;
*sigma += 2.0*mu*(Gc*ePos + eNeg);
}
// Evaluate the tensile energy
Phi[0] = mu*(ePos*ePos).trace();
if (trEps > 0.0) Phi[0] += 0.5*lambda*trEps*trEps;
if (postProc)
{
// Evaluate the compressive energy
Phi[1] = mu*(eNeg*eNeg).trace();
if (trEps < 0.0) Phi[1] += 0.5*lambda*trEps*trEps;
// Evaluate the total strain energy
Phi[2] = Gc*Phi[0] + Phi[1];
// Evaluate the bulk energy
Phi[3] = Gc*(Phi[0] + Phi[1]);
}
else if (sigmaC > 0.0) // Evaluate the crack driving function
Phi[0] = this->MieheCrit56(eps,lambda,mu);
#if INT_DEBUG > 4
std::cout <<"eps_p = "<< eps <<"\n";
for (a = 0; a < nsd; a++)
std::cout <<"M("<< 1+a <<") =\n"<< M[a];
std::cout <<"ePos =\n"<< ePos <<"eNeg =\n"<< eNeg;
if (sigma) std::cout <<"sigma =\n"<< *sigma;
std::cout <<"Phi = "<< Phi[0];
if (postProc) std::cout <<" "<< Phi[1] <<" "<< Phi[2] <<" "<< Phi[3];
std::cout << std::endl;
#else
if (printElm)
{
std::cout <<"g(c) = "<< Gc
<<"\nepsilon =\n"<< epsil <<"eps_p = "<< eps
<<"\nePos =\n"<< ePos <<"eNeg =\n"<< eNeg;
if (sigma) std::cout <<"sigma =\n"<< *sigma;
std::cout <<"Phi = "<< Phi[0];
if (postProc) std::cout <<" "<< Phi[1] <<" "<< Phi[2] <<" "<< Phi[3];
std::cout << std::endl;
}
#endif
if (!dSdE)
return true;
else if (eps[0] == eps[nsd-1])
{
// Hydrostatic pressure
setIsotropic(*dSdE, C0, eps.x > 0.0 ? Cp : mu);
return true;
}
typedef unsigned short int s_ind; // Convenience type definition
// Define a Lambda-function to calculate (lower triangle of) the matrix Qa
auto&& getQ = [this](Matrix& Q, const SymmTensor& Ma, double C)
{
if (C == 0.0) return;
auto&& Mult = [Ma](s_ind i, s_ind j, s_ind k, s_ind l)
{
return Ma(i,j)*Ma(k,l);
};
s_ind i, j, k, l, is, js;
// Normal stresses and strains
for (i = 1; i <= nsd; i++)
for (j = 1; j <= i; j++)
Q(i,j) += C*Mult(i,i,j,j);
is = nsd+1;
for (i = 1; i < nsd; i++)
for (j = i+1; j <= nsd; j++, is++)
{
// Shear stress coupled to normal strain
for (k = 1; k <= nsd; k++)
Q(is,k) += C*Mult(i,j,k,k);
// Shear stress coupled to shear strain
js = nsd+1;
for (k = 1; k < nsd; k++)
for (l = k+1; js <= is; l++, js++)
Q(is,js) += C*Mult(i,j,k,l);
}
};
// Define a Lambda-function to calculate (lower triangle of) the matrix Gab
auto&& getG = [this](Matrix& G, const SymmTensor& Ma,
const SymmTensor& Mb, double C)
{
if (C == 0.0) return;
auto&& Mult = [Ma,Mb](s_ind i, s_ind j, s_ind k, s_ind l)
{
return Ma(i,k)*Mb(j,l) + Ma(i,l)*Mb(j,k) +
Mb(i,k)*Ma(j,l) + Mb(i,l)*Ma(j,k);
};
s_ind i, j, k, l, is, js;
// Normal stresses and strains
for (i = 1; i <= nsd; i++)
for (j = 1; j <= i; j++)
G(i,j) += C*Mult(i,i,j,j);
is = nsd+1;
for (i = 1; i < nsd; i++)
for (j = i+1; j <= nsd; j++, is++)
{
// Shear stress coupled to normal strain
for (k = 1; k <= nsd; k++)
G(is,k) += C*Mult(i,j,k,k);
// Shear stress coupled to shear strain
js = nsd+1;
for (k = 1; k < nsd; k++)
for (l = k+1; js <= is; l++, js++)
G(is,js) += C*Mult(i,j,k,l);
}
};
// Evaluate the stress tangent assuming Voigt notation and symmetry
for (a = 1; a <= nsd; a++)
for (b = 1; b <= a; b++)
(*dSdE)(a,b) = C0;
for (a = 0; a < nsd; a++)
{
double C1 = eps[a] >= 0.0 ? Cp : mu;
getQ(*dSdE, M[a], 2.0*C1);
if (eps[a] != 0.0)
for (b = 0; b < nsd; b++)
if (a != b && eps[a] != eps[b])
getG(*dSdE,M[a],M[b],C1/(1.0-eps[b]/eps[a]));
}
// Account for symmetry
for (b = 2; b <= dSdE->rows(); b++)
for (a = 1; a < b; a++)
(*dSdE)(a,b) = (*dSdE)(b,a);
return true;
}
