void orthonormalize( Matrix& S, Matrix& u ) {
if (S.size1() == 0 || S.size2() == 0) return;
int i, j, k;
int n = S.size1();
Vector eval( n );
Matrix evec( n, n );
u.reset( n, n );
u.set(0);
// diagonalize S, and then calculate 1/sqrt(S).
diagonalize( S, evec, eval);
for(i=0; i<n; i++)
eval[i] = 1.0/sqrt(eval[i]);
double sum;
for(i=0; i<n; i++)
for(j=0; j<n; j++) {
sum = 0;
for(k=0; k<n; k++)
sum += evec(j,k)*eval[k]*evec(i,k);
u(i,j) = sum;
}
}
Real
calcPrincipleValues(const SymmTensor & symm_tensor, unsigned int index, RealVectorValue & direction)
{
ColumnMajorMatrix eval(3,1);
ColumnMajorMatrix evec(3,3);
symm_tensor.columnMajorMatrix().eigen(eval, evec);
// Eigen computes low to high. We want high first.
direction(0) = evec(0,index);
direction(1) = evec(1,index);
direction(2) = evec(2,index);
return eval(index);
}
void
RankTwoTensor::getRUDecompositionRotation(RankTwoTensor & rot) const
{
const RankTwoTensor &a = *this;
RankTwoTensor c, diag, evec;
PetscScalar cmat[N][N], work[10];
PetscReal w[N];
// prepare data for the LAPACKsyev_ routine (which comes from petscblaslapack.h)
PetscBLASInt nd = N,
lwork = 10,
info;
c = a.transpose() * a;
for (unsigned int i = 0; i < N; ++i)
for (unsigned int j = 0; j < N; ++j)
cmat[i][j] = c(i,j);
LAPACKsyev_("V", "U", &nd, &cmat[0][0], &nd, w, work, &lwork, &info);
if (info != 0)
mooseError("In computing the eigenvalues and eigenvectors of a symmetric rank-2 tensor, the PETSC LAPACK syev routine returned error code " << info);
diag.zero();
for (unsigned int i = 0; i < N; ++i)
diag(i,i) = std::pow(w[i], 0.5);
for (unsigned int i = 0; i < N; ++i)
for (unsigned int j = 0; j < N; ++j)
evec(i,j) = cmat[i][j];
rot = a * ((evec.transpose() * diag * evec).inverse());
}
//----- Calculation of eigen vectors and eigen values -----
int calcEigenVectors(const dmatrix &_a, dmatrix &_evec, dvector &_eval)
{
assert( _a.cols() == _a.rows() );
typedef dmatrix mlapack;
typedef dvector vlapack;
mlapack a = _a; // <-
mlapack evec = _evec;
vlapack eval = _eval;
int n = (int)_a.cols();
double *wi = new double[n];
double *vl = new double[n*n];
double *work = new double[4*n];
int lwork = 4*n;
int info;
dgeev_("N","V", &n, &(a(0,0)), &n, &(eval(0)), wi, vl, &n, &(evec(0,0)), &n, work, &lwork, &info);
_evec = evec.transpose();
_eval = eval;
delete [] wi;
delete [] vl;
delete [] work;
return info;
}
Real
MaterialTensorAux::principalValue( const SymmTensor & tensor, unsigned int index )
{
ColumnMajorMatrix eval(3,1);
ColumnMajorMatrix evec(3,3);
tensor.columnMajorMatrix().eigen(eval, evec);
// Eigen computes low to high. We want high first.
int i = -index + 2;
return eval(i);
}
Matrix44 Camera::createCamera()
{
m_cameraForward.normalize();
m_cameraUp.normalize();
cross(m_cameraRight, m_cameraUp, m_cameraForward);
m_cameraRight.normalize();
cross(m_cameraUp, m_cameraForward, m_cameraRight);
m_cameraUp.normalize();
Vector3 evec(m_eye);
m_camera = Matrix44(m_cameraRight.x(), m_cameraRight.y(), m_cameraRight.z(), -evec.dot(m_cameraRight),
m_cameraUp.x(), m_cameraUp.y(), m_cameraUp.z(), -evec.dot(m_cameraUp),
m_cameraForward.x(), m_cameraForward.y(), m_cameraForward.z(), -evec.dot(m_cameraForward),
0.0f, 0.0f, 0.0f, 1.0f);
m_camera.transpose(); //We want row major matrices
m_invcamera = m_camera.inverted();
return m_camera;
}
Matrix<double> RealtimeAPCSCP::calcExactLagrangian() {
// std::chrono::time_point<std::chrono::system_clock> start, end, startf, endf;
// std::chrono::duration<double> duration;
std::map<std::string, Matrix<double> > eval;
std::vector<std::map<std::string, Matrix<double> > > evec(G.size());
std::map<std::string, Matrix<double>> map = make_map("x", x_act);
Matrix<double> result;
// start = std::chrono::system_clock::now();
eval = hess_fi(map);
result = eval["jac"];
// startf = std::chrono::system_clock::now();
for (int i = 0; i < G.size(); i++) {
eval = this->hess_gi[i](map);
result += y_act[i] * (eval)["jac"];
}
// endf = std::chrono::system_clock::now();
// duration = endf - startf;
// std::cout << "Duration of for Loop: " << duration.count()
// << std::endl;
return result;
}
/////////////////////////////////////////////////////////////////////////////////
// calculate the intersection of a plane the given ray
// the ray has an origin and a direction, ray = origin + t*direction
// find the t parameter, return true if it is between 0.0 and 1.0, false
// otherwise, write the results in following variables:
// depth - t \in [0.0 1.0]
// posX - x position of intersection point, nothing if no intersection
// posY - y position of intersection point, nothing if no intersection
// posZ - z position of intersection point, nothing if no intersection
// normalX - x component of normal at intersection point, nothing if no intersection
// normalX - y component of normal at intersection point, nothing if no intersection
// normalX - z component of normal at intersection point, nothing if no intersection
//
/////////////////////////////////////////////////////////////////////////////////
bool
Plane::intersect(Ray ray, double *depth,
double *posX, double *posY, double *posZ,
double *normalX, double *normalY, double *normalZ)
{
//////////*********** START OF CODE TO CHANGE *******////////////
Vec3 evec( ray.origin[0],
ray.origin[1],
ray.origin[2]);
Vec3 nvec( this->params[0],
this->params[1],
this->params[2]);
Vec3 dvec( ray.direction[0],
ray.direction[1],
ray.direction[2]);
double d = this->params[3] * sqrt(nvec[0] * nvec[0] + nvec[1] * nvec[1] + nvec[2] * nvec[2]);
double t = -1;
double denom = dvec.dot(nvec);
if (denom != 0) {
t = (-d - (evec.dot(nvec))) / dvec.dot(nvec);
}
if (t <= 0) {
return false;
} else {
*depth = t;
*posX = (dvec[0] * t) + evec[0];
*posY = (dvec[1] * t) + evec[1];
*posZ = (dvec[2] * t) + evec[2];
if (denom > 0) {
*normalX = -nvec[0];
*normalY = -nvec[1];
*normalZ = -nvec[2];
} else {
*normalX = nvec[0];
*normalY = nvec[1];
*normalZ = nvec[2];
}
}
//////////*********** END OF CODE TO CHANGE *******////////////
//printf("dvec[0], dvec[1], dvec[2]: (%f, %f, %f) \n", dvec[0], dvec[1], dvec[2]);
//printf("evec[0], evec[1], evec[2]: (%f, %f, %f) \n", evec[0], evec[1], evec[2]);
//printf("Plane interesection at t:%f (%f, %f, %f)\n", t, *posX, *posY, *posZ);
return true;
}
/////////////////////////////////////////////////////////////////////////////////
// calculate the intersection of a sphere the given ray
// the ray has an origin and a direction, ray = origin + t*direction
// find the t parameter, return true if it is between 0.0 and 1.0, false
// otherwise, write the results in following variables:
// depth - t \in [0.0 1.0]
// posX - x position of intersection point, nothing if no intersection
// posY - y position of intersection point, nothing if no intersection
// posZ - z position of intersection point, nothing if no intersection
// normalX - x component of normal at intersection point, nothing if no intersection
// normalX - y component of normal at intersection point, nothing if no intersection
// normalX - z component of normal at intersection point, nothing if no intersection
//
// attention: a sphere has usually two intersection points make sure to return
// the one that is closest to the ray's origin and still in the viewing frustum
//
/////////////////////////////////////////////////////////////////////////////////
bool
Sphere::intersect(Ray ray, double *depth,
double *posX, double *posY, double *posZ,
double *normalX, double *normalY, double *normalZ)
{
//////////*********** START OF CODE TO CHANGE *******////////////
// from slides:
// (cx + t * vx)^2 + (cy + t * vy)^2 + (cz + t * vy)^2 = r^2
// text:
// (e+td−c)·(e+td−c)−R2 = 0
// (d·d)t^2 +2d·(e−c)t+(e−c)·(e−c)−R^2 = 0
// d: the direction vector of the ray
// e: point at which the ray starts
// c: center point of the sphere
Vec3 dvec( ray.direction[0],
ray.direction[1],
ray.direction[2]);
Vec3 evec( ray.origin[0],
ray.origin[1],
ray.origin[2]);
Vec3 cvec( this->center[0],
this->center[1],
this->center[2]);
// use the quadratic equation, since we have the form At^2 + Bt + C = 0.
double a = dvec.dot(dvec);
double b = dvec.scale(2).dot(evec.subtract(cvec));
Vec3 eMinusCvec = evec.subtract(cvec);
double c = eMinusCvec.dot(eMinusCvec) - (this->radius * this->radius);
// discriminant: b^2 - 4ac
double discriminant = (b * b) - (4 * a * c);
// From text: If the discriminant is negative, its square root
// is imaginary and the line and sphere do not intersect.
if (discriminant < 0) {
//printf("No intersection with sphere - 1\n");
return false;
} else {
// there is at least one intersection point
double t1 = (-b + sqrt(discriminant)) / (2 * a);
double t2 = (-b - sqrt(discriminant)) / (2 * a);
double tmin = fminf(t1, t2);
double tmax = fmaxf(t1, t2);
double t = 0; // t is set to either tmin or tmax (or the function returns false)
if (tmin >= 0) { //} && tmin <= 1) {
t = tmin;
} else if (tmax >= 0) { //} && tmax <= 1) {
t = tmax;
} else {
// return false if neither interestion point is within [0, 1]
//printf("No intersection with sphere. t values (%f, %f)\n", t1, t2);
return false;
}
*depth = t;
// position: (e + td)
Vec3 posvec = dvec.scale(t).add(evec);
*posX = posvec[0];
*posY = posvec[1];
*posZ = posvec[2];
// normal: 2(p - c)
Vec3 normalvec = posvec.subtract(cvec).scale(2);
normalvec.normalize();
*normalX = normalvec[0];
*normalY = normalvec[1];
*normalZ = normalvec[2];
}
//////////*********** END OF CODE TO CHANGE *******////////////
//printf("Sphere intersection found (%f, %f, %f) \n", *posX, *posY, *posZ);
return true;
}
static void NUMmaximizeCongruence (double **b, double **a, long nr, long nc, double **t, long maximumNumberOfIterations, double tolerance) {
long numberOfIterations = 0;
Melder_assert (nr > 0 && nc > 0);
Melder_assert (t);
if (nc == 1) {
t[1][1] = 1; return;
}
autoNUMmatrix<double> c (1, nc, 1, nc);
autoNUMmatrix<double> w (1, nc, 1, nc);
autoNUMmatrix<double> u (1, nc, 1, nc);
autoNUMvector<double> evec (1, nc);
autoSVD svd = SVD_create (nc, nc);
// Steps 1 & 2: C = A'A and W = A'B
double checkc = 0, checkw = 0;
for (long i = 1; i <= nc; i++) {
for (long j = 1; j <= nc; j++) {
for (long k = 1; k <= nr; k++) {
c[i][j] += a[k][i] * a[k][j];
w[i][j] += a[k][i] * b[k][j];
}
checkc += c[i][j]; checkw += w[i][j];
}
}
if (checkc == 0.0 || checkw == 0.0) {
Melder_throw (U"NUMmaximizeCongruence: we cannot rotate a zero matrix.");
}
// Scale W by (diag(B'B))^-1/2
for (long j = 1; j <= nc; j++) {
double scale = 0.0;
for (long k = 1; k <= nr; k++) {
scale += b[k][j] * b[k][j];
}
if (scale > 0.0) {
scale = 1.0 / sqrt (scale);
}
for (long i = 1; i <= nc; i++) {
w[i][j] *= scale;
}
}
// Step 3: largest eigenvalue of C
evec[1] = 1.0;
double rho, f, f_old;
NUMdominantEigenvector (c.peek(), nc, evec.peek(), &rho, 1.0e-6);
do_steps45 (w.peek(), t, c.peek(), nc, &f);
do {
for (long j = 1; j <= nc; j++) {
// Step 7.a
double p = 0.0;
for (long k = 1; k <= nc; k++) {
for (long i = 1; i <= nc; i++) {
p += t[k][j] * c[k][i] * t[i][j];
}
}
// Step 7.b
double q = 0.0;
for (long k = 1; k <= nc; k++) {
q += w[k][j] * t[k][j];
}
// Step 7.c
if (q == 0.0) {
for (long i = 1; i <= nc; i++) {
u[i][j] = 0.0;
}
} else {
double ww = 0.0;
for (long k = 1; k <= nc; k++) {
ww += w[k][j] * w[k][j];
}
for (long i = 1; i <= nc; i++) {
double ct = 0.0;
for (long k = 1; k <= nc; k++) {
ct += c[i][k] * t[k][j];
}
u[i][j] = (q * (ct - rho * t[i][j]) / p - 2.0 * ww * t[i][j] / q - w[i][j]) / sqrt (p);
}
}
}
// Step 8
SVD_svd_d (svd.get(), u.peek());
// Step 9
for (long i = 1; i <= nc; i++) {
for (long j = 1; j <= nc; j++) {
t[i][j] = 0.0;
for (long k = 1; k <= nc; k++) {
t[i][j] -= svd -> u[i][k] * svd -> v[j][k];
}
}
}
numberOfIterations++;
f_old = f;
// Steps 10 & 11 equal steps 4 & 5
do_steps45 (w.peek(), t, c.peek(), nc, &f);
} while (fabs (f_old - f) / f_old > tolerance && numberOfIterations < maximumNumberOfIterations);
}
vector<complex<float> > PMS(vector<vector<complex<float> >> CSC_array)
{
int CSC_size = (int)CSC_array.size();
int array_size = 0;
int i;
int m,n;
vector<vector< complex<float>> > Product_mat;
vector<vector< complex<float>> > S_Z;
vector<complex<float> > temp_mat;
vector<complex<float> > Major_eigen_vector;
complex<float> mat_ij_1;
complex<float> mat_ij_2;
complex<float> scalar(0,0);
S_Z.resize(40, vector< complex<float>>(40, 0));
for (m = 0; m<40; m++)
{
for (n = 0; n<40; n++)
{
S_Z[m][n] = 0;
}
}
for(i=0; i<period; i++)
{
array_size = (int)(CSC_array[i]).size();
for(m=0; m<array_size; m++)
{
mat_ij_1 = CSC_array[i][m];
scalar = scalar + mat_ij_1*conj(mat_ij_1);
for(n=0; n<array_size; n++)
{
mat_ij_2 = CSC_array[i][n];
mat_ij_2 = conj(mat_ij_2);
temp_mat.push_back(mat_ij_1*mat_ij_2);
}
Product_mat.push_back(temp_mat);
temp_mat.clear();
}
for(m=0; m<array_size; m++)
{
for(n=0; n<array_size; n++)
{
Product_mat[m][n] = Product_mat[m][n]/scalar;
}
}
for(m=0; m<array_size; m++)
{
for(n=0; n<array_size; n++)
{
S_Z[m][n] = S_Z[m][n] + Product_mat[m][n];
}
}
scalar = 0;
Product_mat.clear();
}
MatrixXcf temp(array_size,array_size);
MatrixXcf evec(array_size,array_size);
MatrixXcf eval(array_size,1);
for(m=0; m<array_size; m++)
{
for(n=0; n<array_size; n++)
{
temp(m,n) = S_Z[m][n];
}
}
ComplexEigenSolver<MatrixXcf> ces;
ces.compute(temp);
evec = ces.eigenvectors();
eval = ces.eigenvalues();
//cout << " Eigen " << eval(0,0) << endl;
//cout << " EE " << evec(0,0) << endl;
float Max_eigen=0.0;
int Max_eigen_index=0;
float temp_value=0.0;
for(i=0; i<array_size; i++)
{
temp_value = norm( (i,0));
if(temp_value > Max_eigen)
{
Max_eigen = temp_value;
Max_eigen_index = i;
}
}
/*
cout << "The eigenvector of temp are" << endl << evec << endl;
cout << "The eigenvalue of temp are" << endl << eval << endl;
cout << "Max eigenvalue " << endl << Max_eigen << endl;
cout << "Max eigenvalue index " << endl << Max_eigen_index << endl;
cout << "Max eigenvector " << endl << Max_eigen*evec(i) << endl;
*/
for(i=0; i<array_size; i++)
{
Major_eigen_vector.push_back(evec(i, Max_eigen_index));
//Major_eigen_vector.push_back(Max_eigen*evec(i,Max_eigen_index));
//cout << Major_eigen_vector[i] << endl;
}
return Major_eigen_vector;
}
