LinAlg::Matrix<Type> LinAlg::CaracteristicPolynom (const LinAlg::Matrix<Type>& mat)
{
unsigned n = mat.getNumberOfColumns();
Matrix<Type> z = EigenValues(mat);
Matrix<Type> zi = EigenValues(mat);
Matrix<Type> ret(1,n+1);
std::complex<Type> *tempPoly = new std::complex<Type> [2];
tempPoly[0] = 1;
tempPoly[1] = std::complex<Type>(-z(1,1),-z(1,2));
std::complex<Type> * tempPolyEigenvalue = new std::complex<Type>[2];
unsigned sizeTempPoly = 2;
tempPolyEigenvalue[0] = 1;
for(unsigned i = 2; i <= n ; ++i)
{
tempPolyEigenvalue[1] = std::complex<Type>(-z(i,1),-z(i,2));//apos o templade entre (real,imaginario) atribuição
tempPoly = LinAlg::MultPoly(tempPoly,tempPolyEigenvalue,sizeTempPoly,2);
sizeTempPoly++;
}
for(unsigned i = 0; i < sizeTempPoly ; ++i)
{
ret(1,i+1) = tempPoly[i].real();
}
return ret;
}
std::vector<int> fiedler_reorder(const SymmetricMatrix& m)
{
SymmetricMatrix absm=m;
const int nrows=m.Nrows();
for (int i=0;i<nrows;++i) {
for (int j=0;j<=i;++j){
//absolute value
absm.element(i,j)=std::fabs(absm.element(i,j));
}
}
//laplacian
SymmetricMatrix lap(nrows);
lap=0.;
for (int i=0;i<nrows;++i)
lap.element(i,i)=absm.Row(i+1).Sum();
lap-=absm;
DiagonalMatrix eigs;
Matrix vecs;
EigenValues(lap,eigs,vecs);
ColumnVector fvec=vecs.Column(2);
std::vector<double> fvec_stl(nrows);
//copies over fvec to fvec_stl
std::copy(&fvec.element(0),&fvec.element(0)+nrows,fvec_stl.begin());
std::vector<int> findices;
//sorts the data by eigenvalue in ascending order
sort_data_to_indices(fvec_stl,findices);
return findices;
/* BLOCK works with findices*/
}
bool CalculateNativeAxis(const float* points, int count, unsigned vertex_size, vec3& r, vec3& s, vec3& t)
{
// find covariance matrix
mat3 c = CreateCovarianceMatrix(points, count, vertex_size);
// find eigen values of the covariance matrix
Math::vec3 eigen_values;
if (!EigenValues(c, eigen_values))
return (out_error() << "Can't find eigen values for matrix " << c.ToString() << std::endl, false);
// find eigen vectors of the covariance matrix
Math::vec3 eigen_vectors[3];
if (!EigenVectors(c, eigen_values, eigen_vectors))
return (out_error() << "Can't find eigen values for matrix " << c.ToString() << " with eigen values " << eigen_values.ToString() << std::endl, false);
r = eigen_vectors[0];
s = eigen_vectors[1];
t = eigen_vectors[2];
mat3 a;
a.SetColumn(0, r);
a.SetColumn(1, s);
a.SetColumn(2, t);
out_message() << "Matrix a: " << a.ToString() << std::endl;
mat3 tt = a.Transposed() * c * a;
out_message() << "Matrix tt: " << tt.ToString() << std::endl;
return true;
}
bool DiagonalizeMatrix(const mat3& m, mat3& res)
{
vec3 v;
if (!EigenValues(m, v))
return false;
res.Identity();
res[0] = v[0];
res[4] = v[1];
res[8] = v[2];
return true;
}
void EigenProblemsTest::testSyev()
{
std::cout << "--> Test: syev." <<std::endl;
// turn A to a symmetric matrix
A->randomize_sym();
*Aref = *A;
// Initialize EigenVectors with A
SP::SiconosVector EigenValues(new SiconosVector(size));
SP::SimpleMatrix EigenVectors(new SimpleMatrix(*A));
// *EigenVectors = *A;
Siconos::eigenproblems::syev(*EigenValues, *EigenVectors);
DenseVect error(size);
error *= 0.0;
for( unsigned int i = 0; i < size; ++i )
{
error.plus_assign(ublas::prod( *A->dense(), column(*EigenVectors->dense(), i) ));
error.minus_assign((*EigenValues->dense())(i)*column(*EigenVectors->dense(),i));
}
// Check ...
CPPUNIT_ASSERT_EQUAL_MESSAGE("testSyev 1: ", norm_2(error) < 10 * std::numeric_limits< double >::epsilon() , true);
// Check if A has not been modified
CPPUNIT_ASSERT_EQUAL_MESSAGE("testSyev 2: ", (*A) == (*Aref) , true);
// Now compute only eigenvalues
SP::SiconosVector RefEigenValues(new SiconosVector(*EigenValues));
*EigenVectors = *A;
*EigenValues *= 0.0;
Siconos::eigenproblems::syev(*EigenValues, *EigenVectors, false);
CPPUNIT_ASSERT_EQUAL_MESSAGE("testSyev 3: ", ((*EigenValues) - (*RefEigenValues)).norm2() < 10 * std::numeric_limits< double >::epsilon(), true);
CPPUNIT_ASSERT_EQUAL_MESSAGE("testSyev 4: ", (*A) == (*Aref) , true);
std::cout << "--> Syev test ended with success." <<std::endl;
}
int getNormalModes(double** cartCoords, double* carthessian,
double* masslist, int numCartesians, double* frequencies,
double* normalmodes, int &nfreq, int decontaminate) {
Matrix Hc(numCartesians*3,numCartesians*3);
Hc << carthessian;
// mass weighted cartesian Hessian
int i,j;
Matrix Hmwc(numCartesians*3,numCartesians*3);
for (i=0; i<numCartesians; i++) {
for (j=0; j<numCartesians; j++) {
Hmwc << Hc(i+1,j+1)/sqrt(masslist[i]*masslist[j]);
}
}
#ifdef DEBUG
cout << "Hmwc:\n";
cout << setw(9) << setprecision(3) << (Hmwc);
cout << "\n\n";
#endif
if (!decontaminate) {
Matrix V;
DiagonalMatrix eigval;
EigenValues(Hmwc,eigval,V);
const float twopicinv = 1/(2*PI*LIGHTSPEED); // 2*pi*c
for (i=0; i<numCartesians; i++) {
frequencies[i] = twopicinv*sqrt(abs(eigval(i+1)));
// Complex frequencies are marked with a negative sign
if (eigval(i+1)<0) frequencies[i] *= -1.0f;
}
}
// Moments of inertia tensor
double Ixx=0, Iyy=0, Izz=0, Ixy=0, Ixz=0, Iyz=0, x, y, z, m, mtot=0.0;
double Rcom[3];
for (i=0; i<numCartesians; i++) {
x = cartCoords[i][0];
y = cartCoords[i][1];
z = cartCoords[i][2];
m = masslist[i];
// Center of mass
Rcom[0] += m*x;
Rcom[1] += m*y;
Rcom[2] += m*z;
mtot += m;
Ixx += m*(y*y+z*z);
Iyy += m*(x*x+z*z);
Izz += m*(x*x+y*y);
Ixy -= m*x*y;
Ixz -= m*x*z;
Iyz -= m*y*z;
}
Rcom[0] /= mtot;
Rcom[1] /= mtot;
Rcom[2] /= mtot;
SymmetricMatrix I(3);
I << Ixx << Ixy << Ixz
<< Ixy << Iyy << Iyz
<< Ixz << Iyz << Izz;
// Diagonalize moments of intertia
DiagonalMatrix eigval;
Matrix X;
Jacobi(I, eigval, X);
#ifdef DEBUG
cout << "I:\n";
cout << setw(9) << setprecision(3) << (I);
cout << "\n\n";
cout << "X:\n";
cout << setw(9) << setprecision(3) << (X);
cout << "\n\n";
#endif
// Coords wrt COM
Matrix R(numCartesians, 3);
for (i=0; i<numCartesians; i++) {
R(i,0) = cartCoords[i][0]-Rcom[0];
R(i,1) = cartCoords[i][1]-Rcom[1];
R(i,2) = cartCoords[i][2]-Rcom[2];
}
#ifdef DEBUG
cout << "R:\n";
cout << setw(9) << setprecision(3) << (R);
cout << "\n\n";
#endif
RowVector Px = R*I.Row(1);
RowVector Py = R*I.Row(2);
RowVector Pz = R*I.Row(3);
double sqrm;
IdentityMatrix E(3*numCartesians);
Matrix D(3*numCartesians,3*numCartesians);
D << E;
for (i=0; i<numCartesians; i++) {
sqrm = sqrt(masslist[i]);
D(1,i*3+1) = D(2,i*3+2) = D(3,i*3+3) = sqrm;
D(1,i*3+2) = D(1,i*3+3) = D(2,i*3+1) = D(2,i*3+3) = D(3,i*3+1) = D(3,i*3+2) = 0.0;
for (j=1; j<=3; j++) {
D(4,i*3+j) = (Py(i)*X(j,3)-Pz(i)*X(j,2))/sqrm;
D(5,i*3+j) = (Pz(i)*X(j,1)-Px(i)*X(j,3))/sqrm;
D(6,i*3+j) = (Px(i)*X(j,2)-Py(i)*X(j,1))/sqrm;
}
}
#ifdef DEBUG
cout << "D:\n";
cout << setw(9) << setprecision(3) << (D);
cout << "\n\n";
#endif
// Normalize D
int ntransrot=6;
double s[6];
RowVector Ri, Rj;
for (i=0; i<6; i++) {
//Ri = D.Row(i+1);
//s[i] = DotProduct(Ri, Ri);
s[i] = D.Row(i).NormFrobenius();
if (s[i]<1e-6) ntransrot--;
else D.Column(i+1) *= 1.f/sqrt(s[i]);
}
// Remove the row with zero elements
if (ntransrot<6) {
int k=1;
for (i=0; i<6; i++) {
if (s[i]>=1e-6) {
D.Row(k)=D.Row(i+1);
k++;
}
}
}
#ifdef DEBUG
cout << "D normalized:\n";
cout << setw(9) << setprecision(3) << (D);
cout << "\n\n";
#endif
int Nvib = 3*numCartesians-ntransrot;
cout << "Nvib = 3N-"<<ntransrot<<" = "<<Nvib<<" nonredundant internal coordinates"<<"\n";
// We create a 3N*3N transformation matrix D which transforms from mass weighted
// cartesian coordinates q to internal coordinates S = Dq, where rotation and
// translation have been separated out.
// The first five or six vectors are already linearly independent,
// and we just orthogonalized them above.
// Now the remaining 3N-6 vectors are generated using the stabilized
// Gram-Schmidt algorithm. They are required to be orthogonal to the
// translational rotational vectors.
double dot , norm;
for (i=1; i<=ntransrot; i++) {
for (j=1; j<i; j++) {
// Remove component of D_i that is parallel to D_k:
// D_i = D_i - <D_i, D_k> * D_k
Ri = D.Row(i);
Rj = D.Row(j);
dot = DotProduct(Rj, Ri);
D.Row(i) -= dot*Rj;
}
// Normalize
//norm = DotProduct(D.Row(i), D.Row(i));
norm = D.Row(i).NormFrobenius();
D.Row(i) *= 1/sqrt(norm);
}
int k, cik=0;
for (i=ntransrot; i<=3*numCartesians; i++) {
norm = 0.0;
while (abs(norm) < 1e-12) {
// Initialize i'th row with cik'th coord axis
D.Row(i) = E.Row(cik);
// Stabilized Gram-Schmidt:
for (k=0; k<i; k++) {
// Remove component of D_i that is parallel to D_k (k'th row):
// D_i = D_i - <D_i, D_k> * D_k
// <D_i, D_k> = D_k[cik], since D_i[j]=0 for j!=cik and D_i[cik]=1
D.Row(i) -= D(k,cik)*D.Row(k);
}
// Length of D_i:
norm = D.Row(i).NormFrobenius();
// Check for the linear independence:
// If norm==0 it means that all components parallel to another existing
// vector (row of D) summed up to D_i, i.e the new D_i is linear dependent
// on the other vectors. In this case we choose another D_cik for the
// construction of D_i.
if ( norm < 1e-6) {
cik++; // take another vector
norm = 0.0;
}
}
D.Row(i) *= 1.f/norm;
cik++;
}
#ifdef DEBUG
cout << "D orthonormalized:\n";
cout << setw(9) << setprecision(3) << (D);
cout << "\n\n";
#endif
Matrix Hint;
Hint = D.t() * Hmwc * D;
Matrix V;
EigenValues(Hint,eigval,V);
frequencies = new double[Nvib];
const double scale = 1.f/(2.f*PI*LIGHTSPEED);
for (i=0; i<Nvib; i++) {
frequencies[i] = scale*sqrt(eigval(i+1));
// Complex frequencies are marked with a negative sign
if (eigval(i+1)<0) frequencies[i] *= -1.0f;
}
return Nvib;
}
