void MatrixTest::test_convert_angular_variables_degrees(void)
{
message += "test_convert_angular_variables_degrees\n";
Matrix<double> m;
// Test
m.set(1, 1, 90.0);
m.convert_angular_variables_degrees(0);
assert_true(m.get_rows_number() == 1, LOG);
assert_true(m.get_columns_number() == 2, LOG);
assert_true(fabs(m(0,0) - 1.0) < 1.0e-6, LOG);
assert_true(fabs(m(0,1) - 0.0) < 1.0e-6, LOG);
}
void MatrixTest::test_calculate_eigenvectors(void)
{
message += "test_calculate_eigenvectors";
Matrix<double> eigenvectors;
Matrix<double> m;
// Test
m.set(10,10);
m.randomize_normal();
eigenvectors = m.calculate_eigenvectors();
assert_true(eigenvectors.get_rows_number() == 10, LOG);
assert_true(eigenvectors.get_columns_number() == 10, LOG);
}
// Operator overload for the * sign between two matrices
Matrix Matrix::operator*(Matrix &other)
{
Matrix result;
// This set of loops will go though each field in the result matrix, then calculate using another loop to resuse code for each multiplication between elements
// r is row, c is column, i is row for one element of a multiplication and column for the other element of the same multiplication
for (unsigned int c = 0; c < 4; c++)
for (unsigned int r = 0; r < 4; r++) {
float temp = 0;
for (unsigned int i = 0; i < 4; i++) {
temp = temp + this->get(r,i) * other.get(i,c);
}
result.set(r,c, temp);
}
return result;
}
void RobotKinematics3D::GetCOMJacobian(Matrix& J) const
{
J.resize(3,q.n);
Vector3 dp;
J.set(Zero);
for(int k=0;k<q.n;k++) {
for(int j=k;j!=-1;j=parents[j]) {
GetPositionJacobian(links[k].com,k,j,dp);
dp *= links[k].mass;
J(0,j) += dp.x;
J(1,j) += dp.y;
J(2,j) += dp.z;
}
}
Real mtotal = GetTotalMass();
J /= mtotal;
}
int main(){
ifstream in("in.txt");
int x,y; in>>x>>y;
Matrix ma;
ma.set(x,y);
for(int i=0; i<x; ++i)
for(int j=0; j<y; ++j)
in>>ma.elem(i,j);
in>>x;
Vector ve;
ve.set(x);
for(int i=0; i<x; ++i)
in>>ve[i];
Vector vx = multiply(ma,ve);
vx.display();
ma.remove();
ve.remove();
vx.remove();
}//====================================
Matrix Camera::projectionMatrix() const
{
Matrix projection;
long double h;
long double v;
long double zf = m_farPlaneZ;
long double zn = m_nearPlaneZ;
long double aspectRatio = m_width / (long double)m_height;
long double yScale = 1.0L / std::tan(m_fov / 2.0L);
long double xScale = yScale / aspectRatio;
switch (m_projection)
{
case Perspective:
projection.set(0, 0, xScale);
projection.set(1, 1, yScale);
projection.set(2, 2, (zf + zn) / (zf - zn));
projection.set(2, 3, 1.0L);
projection.set(3, 2, -2 * zf * zn / (zf - zn));
break;
case Parallel:
if (m_width > m_height) {
h = 0.5L * m_zoom;
v = m_height * h / m_width;
} else {
v = 0.5L * m_zoom;
h = m_width * v / m_height;
}
projection.set(0, 0, 1.0L / h);
projection.set(1, 1, 1.0L / v);
projection.set(2, 2, 1.0L / (zf - zn));
projection.set(3, 2, -zn / (zf - zn));
projection.set(3, 3, 1.0L);
break;
}
return projection;
}
void MatrixTest::test_set(void)
{
message += "test_set\n";
std::string file_name = "../data/matrix.dat";
Matrix<double> m;
// Default
m.set();
assert_true(m.get_rows_number() == 0, LOG);
assert_true(m.get_columns_number() == 0, LOG);
// Numbers of rows and columns
m.set(0, 0);
assert_true(m.get_rows_number() == 0, LOG);
assert_true(m.get_columns_number() == 0, LOG);
m.set(2, 3);
assert_true(m.get_rows_number() == 2, LOG);
assert_true(m.get_columns_number() == 3, LOG);
m.set(0, 0);
assert_true(m.get_rows_number() == 0, LOG);
assert_true(m.get_columns_number() == 0, LOG);
// Initialization
m.set(3, 2, 1.0);
assert_true(m.get_rows_number() == 3, LOG);
assert_true(m.get_columns_number() == 2, LOG);
assert_true(m == 1.0, LOG);
// File
m.save(file_name);
m.set(file_name);
assert_true(m.get_rows_number() == 3, LOG);
assert_true(m.get_columns_number() == 2, LOG);
assert_true(m == 1.0, LOG);
}
void serializeMatrix(Archive& ar, Matrix& mat, std::bitset<ATTRIB_NUM>& attribMask, uint8_t& attribIndex, bool useF16)
{
if (attribMask[attribIndex++])
{
if (!attribMask[0])
{
// save
for (int i = 0; i < 16; i++)
{
if (useF16)
{
short val = utils::float32Tofloat16(mat[i]);
ar& val;
}
else
{
float val = mat[i];
ar& val;
}
}
}
else
{
// load
float decompressedMat[16];
for (int i = 0; i < 16; i++)
{
if (useF16)
{
short val;
ar& val;
decompressedMat[i] = utils::float16Tofloat32(val);
}
else
{
ar& decompressedMat[i];
}
}
mat.set(decompressedMat);
}
}
}
void MatrixTest::test_sum_diagonal(void)
{
message += "test_sum_diagonal\n";
Matrix<int> m;
Matrix<int> sum;
Vector<int> diagonal;
// Test
m.set(2, 2, 1);
sum = m.sum_diagonal(1);
diagonal = sum.get_diagonal();
assert_true(diagonal.size() == 2, LOG);
assert_true(diagonal == 2, LOG);
}
void GA::evaluate() {
double minfit = std::numeric_limits<double>::max();
for (int p=0; p<num_pop; p++) {
int nturbines=0;
for (int i=0; i<nt; i++) {
if (pops->get(i,p)>0) {
nturbines++;
}
}
Matrix<double>* layout = new Matrix<double>(nturbines,2);
int l_i = 0;
for (int i=0; i<nt; i++) {
if (pops->get(i,p)>0) {
layout->set(l_i, 0, grid->get(i,0));
layout->set(l_i, 1, grid->get(i,1));
l_i++;
}
}
wfle.evaluate(layout);
double coe = wfle.getEnergyCost();
Matrix<double>* fitnesses = wfle.getTurbineFitnesses();
int n_valid = 0;
for (int i=0; i<nturbines; i++) {
if (fitnesses->get(i,0) > 0.80) {
n_valid++;
}
}
fits[p] = coe; //n_valid;
if (fits[p] < minfit) {
minfit = fits[p];
}
delete layout;
delete fitnesses;
}
printf("%f\n", minfit);
}
void MatrixTest::test_scale_mean_standard_deviation(void)
{
message += "test_scale_mean_standard_deviation\n";
Matrix<double> m;
Vector< Statistics<double> > statistics;
// Test
m.set(2, 2);
m.randomize_uniform();
m.scale_mean_standard_deviation();
statistics = m.calculate_statistics();
assert_true(statistics[0].has_mean_zero_standard_deviation_one(), LOG);
assert_true(statistics[1].has_mean_zero_standard_deviation_one(), LOG);
}
// Generate a uniform scaling matrix from a float value
Matrix Matrix::generateUniformScalingMatrix(float S)
{
Matrix result;
result.set(0,0,S);
result.set(0,1,0);
result.set(0,2,0);
result.set(0,3,0);
result.set(1,0,0);
result.set(1,1,S);
result.set(1,2,0);
result.set(1,3,0);
result.set(2,0,0);
result.set(2,1,0);
result.set(2,2,S);
result.set(2,3,0);
result.set(3,0,0);
result.set(3,1,0);
result.set(3,2,0);
result.set(3,3,1);
return result;
}
TEST(Matrix, writeReadFromFile)
{
std::time_t now = std::chrono::system_clock::to_time_t(
std::chrono::system_clock::now()
);
std::ostringstream sstream;
sstream << ".goblr.test.temp" << now;
const std::string tempFileName = sstream.str();
std::cout << "Using file: " << tempFileName << std::endl;
{
Matrix matrix;
for(unsigned int i = 0; i < goblb::Board::SIZE; ++i)
{
for(unsigned int j = 0; j < goblb::Board::SIZE; ++j)
{
matrix.set(i, j, i + j);
}
}
std::ofstream fstream(tempFileName, std::ofstream::binary);
matrix.writeToFile(fstream);
}
{
Matrix matrix;
std::ifstream fstream(tempFileName, std::ifstream::binary);
matrix.readFromFile(fstream);
for(unsigned int i = 0; i < goblb::Board::SIZE; ++i)
{
for(unsigned int j = 0; j < goblb::Board::SIZE; ++j)
{
EXPECT_EQ(matrix.get(i, j), i + j);
}
}
}
std::remove(tempFileName.c_str());
}
// Generate the identity matrix
Matrix Matrix::generateIdentityMatrix(void)
{
Matrix result;
result.set(0,0,1);
result.set(0,1,0);
result.set(0,2,0);
result.set(0,3,0);
result.set(1,0,0);
result.set(1,1,1);
result.set(1,2,0);
result.set(1,3,0);
result.set(2,0,0);
result.set(2,1,0);
result.set(2,2,1);
result.set(2,3,0);
result.set(3,0,0);
result.set(3,1,0);
result.set(3,2,0);
result.set(3,3,1);
return result;
}
TEST(Matrix, equals)
{
Matrix matrix;
EXPECT_TRUE(matrix == matrix);
EXPECT_FALSE(matrix != matrix);
Matrix matrix2;
EXPECT_TRUE(matrix == matrix2);
EXPECT_FALSE(matrix != matrix2);
matrix2.set(0, 3, 1.0);
EXPECT_FALSE(matrix == matrix2);
EXPECT_TRUE(matrix != matrix2);
matrix.set(0, 3, 1.0);
EXPECT_TRUE(matrix == matrix2);
EXPECT_FALSE(matrix != matrix2);
matrix2.set(4, 3, 7.98);
EXPECT_FALSE(matrix == matrix2);
EXPECT_TRUE(matrix != matrix2);
}
void TestMatrix::testMultiplySquareMatrixAndVector ()
{
bool success = true;
int row, col;
// Try 3x3 matrix and 3x1 vector
Matrix before (3);
QVector<double> vec (3);
int counter = 0;
for (row = 0; row < 3; row++) {
for (col = 0; col < 3; col++) {
before.set (row, col, ++counter);
}
vec [row] = row + 1;
}
// Multiply by itself
QVector<double> afterGot = before * vec;
QVector<double> afterWanted (3);
if (afterGot.size() == afterWanted.size()) {
afterWanted [0] = 1 * 1 + 2 * 2 + 3 * 3;
afterWanted [1] = 4 * 1 + 5 * 2 + 6 * 3;
afterWanted [2] = 7 * 1 + 8 * 2 + 9 * 3;
for (row = 0; row < 3; row++) {
if (qAbs (afterWanted [row] - afterGot [row]) > 0.0001) {
success = false;
break;
}
}
} else {
success = false;
}
QVERIFY (success);
}
void TestMatrix::testMultiplySquareMatrix ()
{
bool success = true;
int row, col;
// Try 3x3 matrix
Matrix before (3);
int counter = 0;
for (row = 0; row < 3; row++) {
for (col = 0; col < 3; col++) {
before.set (row, col, ++counter);
}
}
// Multiply by itself
Matrix afterGot = before * before;
Matrix afterWanted (3);
if (afterGot.rows() == afterWanted.rows() &&
afterGot.cols() == afterWanted.cols()) {
afterWanted.set (0, 0, 1 * 1 + 2 * 4 + 3 * 7);
afterWanted.set (0, 1, 1 * 2 + 2 * 5 + 3 * 8);
afterWanted.set (0, 2, 1 * 3 + 2 * 6 + 3 * 9);
afterWanted.set (1, 0, 4 * 1 + 5 * 4 + 6 * 7);
afterWanted.set (1, 1, 4 * 2 + 5 * 5 + 6 * 8);
afterWanted.set (1, 2, 4 * 3 + 5 * 6 + 6 * 9);
afterWanted.set (2, 0, 7 * 1 + 8 * 4 + 9 * 7);
afterWanted.set (2, 1, 7 * 2 + 8 * 5 + 9 * 8);
afterWanted.set (2, 2, 7 * 3 + 8 * 6 + 9 * 9);
for (row = 0; row < 3; row++) {
for (col = 0; col < 3; col++) {
if (qAbs (afterWanted.get (row, col) - afterGot.get (row, col)) > 0.0001) {
success = false;
break;
}
}
}
} else {
success = false;
}
QVERIFY (success);
}
Matrix matrixOrient(const Vector &x,const Vector &y,const Vector &z)
{
Matrix orient;
orient.set(0,0,x.x());
orient.set(0,1,x.y());
orient.set(0,2,x.z());
orient.set(1,0,y.x());
orient.set(1,1,y.y());
orient.set(1,2,y.z());
orient.set(2,0,z.x());
orient.set(2,1,z.y());
orient.set(2,2,z.z());
return orient;
}
void assemble_Surf2Vol(const Geometry& geo, Matrix& mat, const std::map<const Domain, Vertices> m_points)
{
const double K = 1.0/(4.0*M_PI);
unsigned size = 0; // total number of inside points
for ( std::map<const Domain, Vertices>::const_iterator dvit = m_points.begin(); dvit != m_points.end(); ++dvit) {
size += dvit->second.size();
}
mat = Matrix(size, (geo.size() - geo.outermost_interface().nb_triangles()));
mat.set(0.0);
for ( std::map<const Domain, Vertices>::const_iterator dvit = m_points.begin(); dvit != m_points.end(); ++dvit) {
for ( Geometry::const_iterator mit = geo.begin(); mit != geo.end(); ++mit) {
int orientation = dvit->first.mesh_orientation(*mit);
if ( orientation != 0 ) {
operatorDinternal(*mit, mat, dvit->second, orientation * -1. * K);
if ( !mit->outermost() ) {
operatorSinternal(*mit, mat, dvit->second, orientation * K / geo.sigma(dvit->first));
}
}
}
}
}
Matrix* viewMatrix(Vector3* P, Vector3* N, Vector3* V) {
Matrix *m = new Matrix(4,4);
Vector3 n = *N * (-1.0 / N->magnitude());
Vector3 u = (V->cross(&(*N * -1.0))) * (1 / ((V->cross(N)).magnitude()));
Vector3 v = n.cross(&u);
for (int i = 0; i < 3; ++i)
{
m->set(0, i, u.vector[i]);
m->set(1, i, v.vector[i]);
m->set(2, i, n.vector[i]);
m->set(3, i, 0);
}
m->set(0, 3, -1.0 * u.dot(P));
m->set(1, 3, -1.0 * v.dot(P));
m->set(2, 3, -1.0 * n.dot(P));
m->set(3, 3, 1.0);
return m;
}
void MatrixTest::test_load(void)
{
message += "test_load\n";
std::string file_name = "../data/matrix.dat";
Matrix<int> m;
// Test
m.set();
m.save(file_name);
m.load(file_name);
assert_true(m.get_rows_number() == 0, LOG);
assert_true(m.get_columns_number() == 0, LOG);
// Test
m.set(1, 2, 3);
m.save(file_name);
m.load(file_name);
assert_true(m.get_rows_number() == 1, LOG);
assert_true(m.get_columns_number() == 2, LOG);
assert_true(m == 3, LOG);
// Test
m.set(2, 1, 1);
m.save(file_name);
m.load(file_name);
assert_true(m.get_rows_number() == 2, LOG);
assert_true(m.get_columns_number() == 1, LOG);
// Test
m.set(4, 4, 0);
m.save(file_name);
m.load(file_name);
assert_true(m.get_rows_number() == 4, LOG);
assert_true(m.get_columns_number() == 4, LOG);
assert_true(m == 0, LOG);
// Test
m.set(1, 1, -99);
m.save(file_name);
m.load(file_name);
assert_true(m.get_rows_number() == 1, LOG);
assert_true(m.get_columns_number() == 1, LOG);
assert_true(m == -99, LOG);
// Test
m.set(3, 2);
m(0,0) = 3; m(0,1) = 5;
m(1,0) = 7; m(1,1) = 9;
m(2,0) = 2; m(2,1) = 4;
m.save(file_name);
m.load(file_name);
assert_true(m(0,0) == 3, LOG); assert_true(m(0,1) == 5, LOG);
assert_true(m(1,0) == 7, LOG); assert_true(m(1,1) == 9, LOG);
assert_true(m(2,0) == 2, LOG); assert_true(m(2,1) == 4, LOG);
}
void testMatrix() {
printf("Matrix ");
// Zeros
{
Matrix M(3, 4);
debugAssert(M.rows() == 3);
debugAssert(M.cols() == 4);
debugAssert(M.get(0, 0) == 0);
debugAssert(M.get(1, 1) == 0);
}
// Identity
{
Matrix M = Matrix::identity(4);
debugAssert(M.rows() == 4);
debugAssert(M.cols() == 4);
debugAssert(M.get(0, 0) == 1);
debugAssert(M.get(0, 1) == 0);
}
// Add
{
Matrix A = Matrix::random(2, 3);
Matrix B = Matrix::random(2, 3);
Matrix C = A + B;
for (int r = 0; r < 2; ++r) {
for (int c = 0; c < 3; ++c) {
debugAssert(fuzzyEq(C.get(r, c), A.get(r, c) + B.get(r, c)));
}
}
}
// Matrix multiply
{
Matrix A(2, 2);
Matrix B(2, 2);
A.set(0, 0, 1);
A.set(0, 1, 3);
A.set(1, 0, 4);
A.set(1, 1, 2);
B.set(0, 0, -6);
B.set(0, 1, 9);
B.set(1, 0, 1);
B.set(1, 1, 7);
Matrix C = A * B;
debugAssert(fuzzyEq(C.get(0, 0), -3));
debugAssert(fuzzyEq(C.get(0, 1), 30));
debugAssert(fuzzyEq(C.get(1, 0), -22));
debugAssert(fuzzyEq(C.get(1, 1), 50));
}
// Transpose
{
Matrix A(2, 2);
A.set(0, 0, 1);
A.set(0, 1, 3);
A.set(1, 0, 4);
A.set(1, 1, 2);
Matrix C = A.transpose();
debugAssert(fuzzyEq(C.get(0, 0), 1));
debugAssert(fuzzyEq(C.get(0, 1), 4));
debugAssert(fuzzyEq(C.get(1, 0), 3));
debugAssert(fuzzyEq(C.get(1, 1), 2));
A = Matrix::random(3, 4);
A = A.transpose();
debugAssert(A.rows() == 4);
debugAssert(A.cols() == 3);
}
// Copy-on-mutate
{
Matrix::debugNumCopyOps = Matrix::debugNumAllocOps = 0;
Matrix A = Matrix::identity(2);
debugAssert(Matrix::debugNumAllocOps == 1);
debugAssert(Matrix::debugNumCopyOps == 0);
Matrix B = A;
debugAssert(Matrix::debugNumAllocOps == 1);
debugAssert(Matrix::debugNumCopyOps == 0);
B.set(0,0,4);
debugAssert(B.get(0,0) == 4);
debugAssert(A.get(0,0) == 1);
debugAssert(Matrix::debugNumAllocOps == 2);
debugAssert(Matrix::debugNumCopyOps == 1);
}
// Inverse
{
Matrix A(2, 2);
A.set(0, 0, 1);
A.set(0, 1, 3);
A.set(1, 0, 4);
A.set(1, 1, 2);
Matrix C = A.inverse();
debugAssert(fuzzyEq(C.get(0, 0), -0.2f));
debugAssert(fuzzyEq(C.get(0, 1), 0.3f));
debugAssert(fuzzyEq(C.get(1, 0), 0.4f));
debugAssert(fuzzyEq(C.get(1, 1), -0.1f));
}
{
Matrix A = Matrix::random(10, 10);
Matrix B = A.inverse();
B = B * A;
for (int r = 0; r < B.rows(); ++r) {
for (int c = 0; c < B.cols(); ++c) {
float v = B.get(r, c);
// The precision isn't great on our inverse, so be tolerant
if (r == c) {
debugAssert(abs(v - 1.0f) < 1e-4);
} else {
debugAssert(abs(v) < 1e-4);
}
(void)v;
}
}
}
// Negate
{
Matrix A = Matrix::random(2, 2);
Matrix B = -A;
for (int r = 0; r < A.rows(); ++r) {
for (int c = 0; c < A.cols(); ++c) {
debugAssert(B.get(r, c) == -A.get(r, c));
}
}
}
// Transpose
{
Matrix A = Matrix::random(3,2);
Matrix B = A.transpose();
debugAssert(B.rows() == A.cols());
debugAssert(B.cols() == A.rows());
for (int r = 0; r < A.rows(); ++r) {
for (int c = 0; c < A.cols(); ++c) {
debugAssert(B.get(c, r) == A.get(r, c));
}
}
}
// SVD
{
Matrix A = Matrix(3, 3);
A.set(0, 0, 1.0);
A.set(0, 1, 2.0);
A.set(0, 2, 1.0);
A.set(1, 0, -3.0);
A.set(1, 1, 7.0);
A.set(1, 2, -6.0);
A.set(2, 0, 4.0);
A.set(2, 1, -4.0);
A.set(2, 2, 10.0);
A = Matrix::random(27, 15);
Array<float> D;
Matrix U, V;
A.svd(U, D, V);
// Verify that we can reconstruct
Matrix B = U * Matrix::fromDiagonal(D) * V.transpose();
Matrix test = abs(A - B) < 0.1f;
// A.debugPrint("A");
// U.debugPrint("U");
// D.debugPrint("D");
// V.debugPrint("V");
// (U * D * V.transpose()).debugPrint("UDV'");
debugAssert(test.allNonZero());
float m = float((A - B).norm() / A.norm());
debugAssert(m < 0.01f);
(void)m;
}
testPseudoInverse();
/*
Matrix a(3, 5);
a.set(0,0, 1); a.set(0,1, 2); a.set(0,2, 3); a.set(0,3, 4); a.set(0,4, 5);
a.set(1,0, 3); a.set(1,1, 5); a.set(1,2, 3); a.set(1,3, 1); a.set(1,4, 2);
a.set(2,0, 1); a.set(2,1, 1); a.set(2,2, 1); a.set(2,3, 1); a.set(2,4, 1);
Matrix b = a;
b.set(0,0, 1.8124); b.set(0,1, 0.5341); b.set(0,2, 2.8930); b.set(0,3, 5.2519); b.set(0,4, 4.8829);
b.set(1,0, 2.5930); b.set(1,1, 2.6022); b.set(1,2, 4.2760); b.set(1,3, 5.9497); b.set(1,4, 6.3751);
a.debugPrint("a");
a.debugPrint("b");
Matrix H = b * a.pseudoInverse();
H.debugPrint("H");
*/
printf("passed\n");
}
void GrowingInputsTest::test_perform_inputs_selection(void)
{
message += "test_perform_inputs_selection\n";
DataSet ds;
Matrix<double> data;
NeuralNetwork nn;
LossIndex pf(&nn,&ds);
TrainingStrategy ts(&pf);
GrowingInputs gi(&ts);
GrowingInputs::GrowingInputsResults* gir;
// Test
data.set(20,3);
for (size_t i = 0; i < 20; i++)
{
data(i,0) = (double)i;
data(i,1) = 10.0;
data(i,2) = (double)i;
}
ds.set(data);
//ds.get_instances_pointer()->split_random_indices();
nn.set(2,6,1);
ts.set_display(false);
gi.set_display(false);
gi.set_approximation(true);
gir = gi.perform_inputs_selection();
assert_true(gir->optimal_inputs[0] == 1, LOG);
gi.delete_selection_history();
gi.delete_parameters_history();
gi.delete_loss_history();
// Test
// size_t j = -10;
// for (size_t i = 0; i < 10; i++)
// {
// data(i,0) = (double)i;
// data(i,1) = rand();
// data(i,2) = 1.0;
// j+=1;
// }
// for (size_t i = 10; i < 20; i++)
// {
// data(i,0) = (double)i;
// data(i,1) = rand();
// data(i,2) = 0.0;
// }
// ds.set(data);
// nn.set(2,6,1);
// ts.set_display(false);
// gi.set_display(false);
// gi.set_approximation(false);
// gir = gi.perform_inputs_selection();
//// assert_true(gir->optimal_inputs[0] == 1, LOG);
// gi.delete_selection_history();
// gi.delete_parameters_history();
// gi.delete_loss_history();
}
/*
Naive implementation of Matrix Matrix Multiplication
@param A input matrix
@param B input matrix
@param C output matrix
*/
inline
void naive(const Matrix& A, const Matrix& B, Matrix& C){
//preload dimensions for faster access
int dimM = C.getDimM();
int dimN = C.getDimN();
int dimL = A.getDimN();
for (int m = 0; m < dimM; m+=4){ ///rows of c
for (int n = 0; n < dimN; n+=4){ ///cols of c
//do calculation of a 4x4 block
//std::cout << m << "\t" << n << std::endl;
__m256d* pA = A.get(m, 0);
__m256d* pB = A.get(m+1, 0);
__m256d* pC = A.get(m+2, 0);
__m256d* pD = A.get(m+3, 0);
__m256d* pK = B.getT(0, n);
__m256d* pL = B.getT(0, n+1);
__m256d* pM = B.getT(0, n+2);
__m256d* pN = B.getT(0, n+3);
//std::cout << pA << "\t" << pB << "\t" << pC << "\t" << pD << std::endl;
__m256d K = _mm256_setzero_pd();
__m256d L = _mm256_setzero_pd();
__m256d M = _mm256_setzero_pd();
__m256d N = _mm256_setzero_pd();
__m256d O = _mm256_setzero_pd();
__m256d P = _mm256_setzero_pd();
__m256d Q = _mm256_setzero_pd();
__m256d R = _mm256_setzero_pd();
__m256d S = _mm256_setzero_pd();
__m256d T = _mm256_setzero_pd();
__m256d U = _mm256_setzero_pd();
__m256d V = _mm256_setzero_pd();
__m256d W = _mm256_setzero_pd();
__m256d X = _mm256_setzero_pd();
__m256d Y = _mm256_setzero_pd();
__m256d Z = _mm256_setzero_pd();
for (int l = 0; l < dimL; l+=4){
//std::cout <<"mul" << std::endl;
K = K + (*pA) * (*pK);
L = L + (*pA) * (*pL);
M = M + (*pA) * (*pM);
N = N + (*pA) * (*pN);
O = O + (*pB) * (*pK);
P = P + (*pB) * (*pL);
Q = Q + (*pB) * (*pM);
R = R + (*pB) * (*pN);
S = S + (*pC) * (*pK);
T = T + (*pC) * (*pL);
U = U + (*pC) * (*pM);
V = V + (*pC) * (*pN);
W = W + (*pD) * (*pK);
X = X + (*pD) * (*pL);
Y = Y + (*pD) * (*pM);
Z = Z + (*pD) * (*pN);
//std::cout << "inc" <<std::endl;
pA++;
pB++;
pC++;
pD++;
pK++;
pL++;
pM++;
pN++;
}
// {a[0]+a[1], b[0]+b[1], a[2]+a[3], b[2]+b[3]}
__m256d sumab = _mm256_hadd_pd(K, L);
// {c[0]+c[1], d[0]+d[1], c[2]+c[3], d[2]+d[3]}
__m256d sumcd = _mm256_hadd_pd(M, N);
// {a[0]+a[1], b[0]+b[1], c[2]+c[3], d[2]+d[3]}
__m256d blend = _mm256_blend_pd(sumab, sumcd, 0b1100);
// {a[2]+a[3], b[2]+b[3], c[0]+c[1], d[0]+d[1]}
__m256d perm = _mm256_permute2f128_pd(sumab, sumcd, 0x21);
__m256d sum = _mm256_add_pd(perm, blend);
C.set(m, n, sum);
//C(m , n) = K[0] + K[1] + K[2] + K[3];
//C(m , n+1) = L[0] + L[1] + L[2] + L[3];
//C(m , n+2) = M[0] + M[1] + M[2] + M[3];
//C(m , n+3) = N[0] + N[1] + N[2] + N[3];
// {a[0]+a[1], b[0]+b[1], a[2]+a[3], b[2]+b[3]}
sumab = _mm256_hadd_pd(O, P);
// {c[0]+c[1], d[0]+d[1], c[2]+c[3], d[2]+d[3]}
sumcd = _mm256_hadd_pd(Q, R);
// {a[0]+a[1], b[0]+b[1], c[2]+c[3], d[2]+d[3]}
blend = _mm256_blend_pd(sumab, sumcd, 0b1100);
// {a[2]+a[3], b[2]+b[3], c[0]+c[1], d[0]+d[1]}
perm = _mm256_permute2f128_pd(sumab, sumcd, 0x21);
sum = _mm256_add_pd(perm, blend);
C.set(m+1, n, sum);
//C(m+1, n ) = O[0] + O[1] + O[2] + O[3];
//C(m+1, n+1) = P[0] + P[1] + P[2] + P[3];
//C(m+1, n+2) = Q[0] + Q[1] + Q[2] + Q[3];
//C(m+1, n+3) = R[0] + R[1] + R[2] + R[3];
// {a[0]+a[1], b[0]+b[1], a[2]+a[3], b[2]+b[3]}
sumab = _mm256_hadd_pd(S, T);
// {c[0]+c[1], d[0]+d[1], c[2]+c[3], d[2]+d[3]}
sumcd = _mm256_hadd_pd(U, V);
// {a[0]+a[1], b[0]+b[1], c[2]+c[3], d[2]+d[3]}
blend = _mm256_blend_pd(sumab, sumcd, 0b1100);
// {a[2]+a[3], b[2]+b[3], c[0]+c[1], d[0]+d[1]}
perm = _mm256_permute2f128_pd(sumab, sumcd, 0x21);
sum = _mm256_add_pd(perm, blend);
C.set(m+2, n, sum);
//C(m+2, n ) = S[0] + S[1] + S[2] + S[3];
//C(m+2, n+1) = T[0] + T[1] + T[2] + T[3];
//C(m+2, n+2) = U[0] + U[1] + U[2] + U[3];
//C(m+2, n+3) = V[0] + V[1] + V[2] + V[3];
// {a[0]+a[1], b[0]+b[1], a[2]+a[3], b[2]+b[3]}
sumab = _mm256_hadd_pd(W, X);
// {c[0]+c[1], d[0]+d[1], c[2]+c[3], d[2]+d[3]}
sumcd = _mm256_hadd_pd(Y, Z);
// {a[0]+a[1], b[0]+b[1], c[2]+c[3], d[2]+d[3]}
blend = _mm256_blend_pd(sumab, sumcd, 0b1100);
// {a[2]+a[3], b[2]+b[3], c[0]+c[1], d[0]+d[1]}
perm = _mm256_permute2f128_pd(sumab, sumcd, 0x21);
sum = _mm256_add_pd(perm, blend);
C.set(m+3, n, sum);
//C(m+3, n ) = W[0] + W[1] + W[2] + W[3];
//C(m+3, n+1) = X[0] + X[1] + X[2] + X[3];
//C(m+3, n+2) = Y[0] + Y[1] + Y[2] + Y[3];
//C(m+3, n+3) = Z[0] + Z[1] + Z[2] + Z[3];
}
}
}
bool Matrix::invert(Matrix& out) const
{
Matrix a = Matrix(*this);
out.setSize(n, m);
for (int i = 0; i < n; ++i)
for (int j = 0; j < m; ++j)
out.set(i, j, (i == j) ? 1 : 0);
for (int i = 1; i < n; ++i)
{
// Pivot search
if (true) //a.get(i - 1, i - 1) <= 0.001)
{
double highest = qAbs(a.get(i - 1, i - 1));
int highest_pos = i - 1;
for (int k = i; k < n; ++k)
{
double v = qAbs(a.get(k, i - 1));
if (v > highest)
{
highest = v;
highest_pos = k;
}
}
if (highest == 0)
return false;
if (i - 1 != highest_pos)
{
a.swapRows(i - 1, highest_pos);
out.swapRows(i - 1, highest_pos);
}
}
for (int k = i; k < n; ++k)
{
double factor = -a.get(k, i - 1) / a.get(i - 1, i - 1);
for (int j = 0; j < m; ++j)
{
a.set(k, j, a.get(k, j) + factor * a.get(i - 1, j));
out.set(k, j, out.get(k, j) + factor * out.get(i - 1, j));
}
}
}
for (int i = n - 2; i >= 0; --i)
{
for (int k = i; k >= 0; --k)
{
double factor = -a.get(k, i + 1) / a.get(i + 1, i + 1);
for (int j = 0; j < m; ++j)
{
a.set(k, j, a.get(k, j) + factor * a.get(i + 1, j));
out.set(k, j, out.get(k, j) + factor * out.get(i + 1, j));
}
}
}
for (int i = 0; i < n; ++i)
{
double factor = 1 / a.get(i, i);
for (int j = 0; j < m; ++j)
out.set(i, j, out.get(i, j) * factor);
}
return true;
}
int main(int argc, char *argv[])
{
Buffer buf = Buffer(10);
assert(buf.count() == 10);
Array d = Array(100);
assert(d(55)->getKind() == EMPTY);
d.set(55,HUMAN);
assert(d(55)->getKind() == HUMAN);
Buffer buf2 = Buffer(d);
assert(buf2.count() == 100);
assert((*buf2.toArray())(55)->getKind() == HUMAN);
assert((*buf2.toArray())(54)->getKind() == EMPTY);
Array d2 = Array(2);
d2.set(0,ZOMBIE);
Buffer from = Buffer(d2);
Buffer to = Buffer(d2.getSize());
assert(from.count() == 2);
assert(to.count() == 2);
assert((*from.toArray())(0)->getKind() == ZOMBIE);
error err = MPI_Init(&argc, &argv);
assert(err == MPI_SUCCESS);
int rank;
MPI_Request request;
MPI_Datatype dtype;
err = from.datatype(&dtype);
assert(err == MPI_SUCCESS);
err = MPI_Type_commit(&dtype);
assert(err== MPI_SUCCESS);
err = MPI_Comm_rank(MPI_COMM_WORLD, &rank);
assert(err == MPI_SUCCESS);
if (PROC_HEIGHT*PROC_WIDTH>1){
if (rank == 0){
err = sendToNeighbour(1,&from,&request,dtype,123);
//err = MPI_Isend(from.rawData(), from.count(), dtype, 1, MPI_TAG ,MPI_COMM_WORLD, &request);
assert(err == MPI_SUCCESS);
}else if (rank == 1){
err = recvFromNeighbour(0,&to,dtype,123);
//err = MPI_Recv(to.rawData(), to.count(), dtype, 0, MPI_TAG,MPI_COMM_WORLD, &status);
assert(err == MPI_SUCCESS);
assert((*to.toArray())(0)->getKind() == ZOMBIE);
assert((*to.toArray())(1)->getKind() == EMPTY);
}
}
/* this is what all matrices should do
E E E E E E Z E
E H E E E H E H
E E E E -> E E E E
E E Z E Z E Z E
E E E E E H E E
*/
Matrix myMatr = Matrix(5,4);
myMatr.set(1,1,HUMAN,NULL);
myMatr.set(2,3,ZOMBIE,NULL);
int nbours[4];
MPI_Request reqs[4];
neighbours(toX(rank),toY(rank),PROC_WIDTH,PROC_HEIGHT,nbours);
//neighbours(nbours);
Array **toSend = myMatr.toSend(1,false);
assert(toSend[UP]->getSize() == 2);
assert((*(toSend[UP]))(0)->getKind() == HUMAN);
assert((*(toSend[DOWN]))(1)->getKind() == ZOMBIE);
assert(toSend[LEFT]->getSize() == 3);
Buffer** bufs = (Buffer**)calloc(4,sizeof(Buffer*));
Buffer** bufs2 = (Buffer**)calloc(4,sizeof(Buffer*));
for (int i = 0; i < 4; i++){
bufs[i] = new Buffer(*(toSend[i]));
bufs2[i] = new Buffer(toSend[i]->getSize());
}
assert(bufs[DOWN]->count() == 2);
assert(bufs[LEFT]->count() == 3);
assert(bufs2[LEFT]->count() == 3);
err = sendToAllNeighbours(nbours,bufs,reqs,dtype);
assert(err == MPI_SUCCESS);
err = recvFromAllNeighbours(nbours,bufs2,dtype);
assert(err == MPI_SUCCESS);
assert((*(bufs2[DOWN]->toArray()))(0)->getKind() == HUMAN);
assert((*(bufs2[DOWN]->toArray()))(1)->getKind() == EMPTY);
assert((*(bufs2[UP]->toArray()))(0)->getKind() == EMPTY);
assert((*(bufs2[UP]->toArray()))(1)->getKind() == ZOMBIE);
assert((*(bufs2[LEFT]->toArray()))(1)->getKind() == EMPTY);
assert((*(bufs2[LEFT]->toArray()))(2)->getKind() == ZOMBIE);
assert((*(bufs2[RIGHT]->toArray()))(2)->getKind() == EMPTY);
assert((*(bufs2[RIGHT]->toArray()))(0)->getKind() == HUMAN);
// Full test with matrix
Matrix matrix = Matrix(6,7);
matrix.set(0,2,ZOMBIE,NULL);
matrix.set(1,1,HUMAN,NULL);
matrix.set(4,4,INFECTED,NULL);
/*
E E E E E E E E E E E I E E E E E E I E E
E H E E E E E E H E E E E H E E E E E E E
Z E E E E E E Z E E E E Z E Z H E E E Z H
E E E E E E E -> E E E E E E E --> E E E E E E E
E E E E I E E E E E E I E E E E E E I E E
E E E E E E E E H E E E E E E E E E E E E
*/
assert(matrix.getCount(ZOMBIE) == 1);
assert(matrix.getCount(HUMAN) == 1);
assert(matrix.getCount(INFECTED) == 1);
swapAll(nbours,matrix,false);
assert(matrix.getCount(ZOMBIE) == 2);
assert(matrix.getCount(HUMAN) == 3);
assert(matrix.getCount(INFECTED) == 2);
// Human moving down
matrix.set(1,1,EMPTY,NULL);
matrix.set(1,2,HUMAN,NULL);
matrix(1,2)->setMoveFlag(true);
// Infected moving left
matrix.set(4,4,EMPTY,NULL);
matrix.set(3,4,INFECTED,NULL);
matrix(3,4)->setMoveFlag(true);
// emulating that all of them move
matrix(5,2)->setMoveFlag(true);
assert(matrix.getCount(HUMAN) == 3);
if (rank == 0){
//matrix.printMoveFlags();
}
swapAll(nbours,matrix,true);
if (rank == 0){
//matrix.printMoveFlags();
}
assert(matrix.getCount(HUMAN) == 2);
// Trying to get seg fault by using a lot of collisions
Matrix matrix2 = Matrix(4,4);
matrix2.set(0,0,HUMAN,NULL);
matrix2.set(0,1,HUMAN,NULL);
matrix2.set(0,2,HUMAN,NULL);
matrix2.set(0,3,HUMAN,NULL);
matrix2.set(1,0,HUMAN,NULL);
matrix2.set(1,1,HUMAN,NULL);
matrix2.set(1,2,HUMAN,NULL);
matrix2.set(1,3,HUMAN,NULL);
matrix2.set(2,0,HUMAN,NULL);
matrix2.set(2,1,HUMAN,NULL);
matrix2.set(2,2,HUMAN,NULL);
matrix2.set(2,3,HUMAN,NULL);
matrix2.set(3,0,HUMAN,NULL);
matrix2.set(3,1,HUMAN,NULL);
matrix2.set(3,2,HUMAN,NULL);
matrix2.set(3,3,HUMAN,NULL);
/*
H H H H H H H H
H H H H H H H H
H H H H H H H H
H H H H -> H H H H
*/
assert(matrix2.getCount(HUMAN) == 16);
for (int i = 0; i < 100; i++){
swapAll(nbours,matrix,false);
}
assert(matrix2.getCount(HUMAN) == 16);
// Handling collisions
Matrix matrix3 = Matrix(5,6);
matrix3.set(1,1,INFECTED,NULL);
matrix3.set(1,4,ZOMBIE,NULL);
// Collision handling
/*
E E E E E E E E E E E E
E I E E E E E I E E E I
E E E E E E -> E Z E E E Z
E E E E E E E E E E E E
E Z E E E E E I E E E E
*/
assert(matrix3.getCount(INFECTED) == 1);
assert(matrix3.getCount(ZOMBIE) == 1);
int collisions = swapAll(nbours,matrix3,false);
//cout << "Collisions " << collisions << endl << flush;
//matrix3.print();
//assert(collisions == 1);
//assert(matrix3.getCount(INFECTED) == 3);
//assert(matrix3.getCount(ZOMBIE) == 2);
Matrix matrix4 = Matrix(100,100);
if (rank == 0){
//cout << "looking for deadlocks\n"<<flush;
}
for (int i = 0; i < 100; i++){
if (rank == 0 && i % 10 == 0 ){
//cout << "iteration "<<i<<endl<<flush;
}
for (int x = 0; x < 100; x++){
for (int y = 0; y < 100; y++){
double r = drand48();
if (r < 0.25){
matrix4.set(x,y,HUMAN,NULL);
}else if(r < 0.5){
matrix4.set(x,y,ZOMBIE, NULL);
}
else if(r < 0.75){
matrix4.set(x,y,INFECTED, NULL);
}else{
matrix4.set(x,y,EMPTY, NULL);
}
}
}
// might deadlock, who knows
swapAll(nbours,matrix4,false);
}
err = MPI_Finalize();
assert(err == MPI_SUCCESS);
}
void mover( Matrix& x, Matrix& v, int npart, double L,
double mpv, double vwall, double tau,
Matrix& strikes, Matrix& delv, long& seed ) {
// mover - Function to move particles by free flight
// Also handles collisions with walls
// Inputs
// x Positions of the particles
// v Velocities of the particles
// npart Number of particles in the system
// L System length
// mpv Most probable velocity off the wall
// vwall Wall velocities
// tau Time step
// seed Random number seed
// Outputs
// x,v Updated positions and velocities
// strikes Number of particles striking each wall
// delv Change of y-velocity at each wall
// seed Random number seed
//* Move all particles pretending walls are absent
Matrix x_old(npart);
x_old = x; // Remember original position
int i;
for( i=1; i<= npart; i++ )
x(i) = x_old(i) + v(i,1)*tau;
//* Check each particle to see if it strikes a wall
strikes.set(0.0); delv.set(0.0);
Matrix xwall(2), vw(2), direction(2);
xwall(1) = 0; xwall(2) = L; // Positions of walls
vw(1) = -vwall; vw(2) = vwall; // Velocities of walls
double stdev = mpv/sqrt(2.);
// Direction of particle leaving wall
direction(1) = 1; direction(2) = -1;
for( i=1; i<=npart; i++ ) {
//* Test if particle strikes either wall
int flag = 0;
if( x(i) <= 0 )
flag=1; // Particle strikes left wall
else if( x(i) >= L )
flag=2; // Particle strikes right wall
//* If particle strikes a wall, reset its position
// and velocity. Record velocity change.
if( flag > 0 ) {
strikes(flag)++;
double vyInitial = v(i,2);
//* Reset velocity components as biased Maxwellian,
// Exponential dist. in x; Gaussian in y and z
v(i,1) = direction(flag)*sqrt(-log(1.-rand(seed))) * mpv;
v(i,2) = stdev*randn(seed) + vw(flag); // Add wall velocity
v(i,3) = stdev*randn(seed);
// Time of flight after leaving wall
double dtr = tau*(x(i)-xwall(flag))/(x(i)-x_old(i));
//* Reset position after leaving wall
x(i) = xwall(flag) + v(i,1)*dtr;
//* Record velocity change for force measurement
delv(flag) += (v(i,2) - vyInitial);
}
}
}
void DrawBl()
{
Shader* s = &g_shader[g_curS];
//return;
for(int i=0; i<BUILDINGS; i++)
{
Building* b = &g_building[i];
if(!b->on)
continue;
const BuildingT* t = &g_bltype[b->type];
//const BuildingT* t = &g_bltype[BUILDING_APARTMENT];
Model* m = &g_model[ t->model ];
Vec3f vmin(b->drawpos.x - t->widthx*TILE_SIZE/2, b->drawpos.y, b->drawpos.z - t->widthz*TILE_SIZE/2);
Vec3f vmax(b->drawpos.x + t->widthx*TILE_SIZE/2, b->drawpos.y + (t->widthx+t->widthz)*TILE_SIZE/2, b->drawpos.z + t->widthz*TILE_SIZE/2);
if(!g_frustum.boxin2(vmin.x, vmin.y, vmin.z, vmax.x, vmax.y, vmax.z))
continue;
if(!b->finished)
m = &g_model[ t->cmodel ];
/*
m->draw(0, b->drawpos, 0);
*/
float pitch = 0;
float yaw = 0;
Matrix modelmat;
float radians[] = {(float)DEGTORAD(pitch), (float)DEGTORAD(yaw), 0};
modelmat.translation((const float*)&b->drawpos);
Matrix rotation;
rotation.rotrad(radians);
modelmat.postmult(rotation);
glUniformMatrix4fv(s->m_slot[SSLOT_MODELMAT], 1, 0, modelmat.m_matrix);
Matrix modelview;
#ifdef SPECBUMPSHADOW
modelview.set(g_camview.m_matrix);
#endif
modelview.postmult(modelmat);
glUniformMatrix4fv(s->m_slot[SSLOT_MODELVIEW], 1, 0, modelview.m_matrix);
Matrix mvp;
#if 0
mvp.set(modelview.m_matrix);
mvp.postmult(g_camproj);
#elif 0
mvp.set(g_camproj.m_matrix);
mvp.postmult(modelview);
#else
mvp.set(g_camproj.m_matrix);
mvp.postmult(g_camview);
mvp.postmult(modelmat);
#endif
glUniformMatrix4fv(s->m_slot[SSLOT_MVP], 1, 0, mvp.m_matrix);
Matrix modelviewinv;
Transpose(modelview, modelview);
Inverse2(modelview, modelviewinv);
//Transpose(modelviewinv, modelviewinv);
glUniformMatrix4fv(s->m_slot[SSLOT_NORMALMAT], 1, 0, modelviewinv.m_matrix);
VertexArray* va = &b->drawva;
m->usetex();
glVertexAttribPointer(s->m_slot[SSLOT_POSITION], 3, GL_FLOAT, GL_FALSE, 0, va->vertices);
glVertexAttribPointer(s->m_slot[SSLOT_TEXCOORD0], 2, GL_FLOAT, GL_FALSE, 0, va->texcoords);
if(s->m_slot[SSLOT_NORMAL] != -1)
glVertexAttribPointer(s->m_slot[SSLOT_NORMAL], 3, GL_FLOAT, GL_FALSE, 0, va->normals);
glDrawArrays(GL_TRIANGLES, 0, va->numverts);
}
}
void solve(string file, result* res)
{
//#Create matrix using filename
mat = new Matrix(file);
#ifdef _PRINT_INFO
cout << "Input matrix \n";
printMatrix(mat);
#endif
//#Check that parameters are valid
if(numthreads > mat->getRows())
{
omp_set_num_threads(mat->getRows());
cout << " WARNING! More threads set then rows in matrice of "<< file << ". Threads set to ROW COUNT: " << mat->getRows() << "\n\n";
}
else
{
omp_set_num_threads(numthreads);
cout << " Numthreads is set to: " << numthreads << "\n\n";
}
//#Start timer
long alg_start = get_usecs();
//#Compute vertical prefix sum
ps = new Matrix(mat->getRows(), mat->getCols());
setPrefixSumMatrix();
#ifdef _PRINT_INFO
cout << "Vertical PrefixMatrix \n";
printMatrix(ps);
#endif
//#Start doing some work
res->filename = file;
doWork(0, res);
int bottom = res->bottom;
int top = res->top;
int right = res->right;
int left = res->left;
int max_sum = res->sum;
//#Compose the output matrix
int outmat_row_dim = bottom - top + 1;
int outmat_col_dim = right - left + 1;
Matrix* outmat = new Matrix(outmat_row_dim, outmat_col_dim);
for(int i=top, k=0; i<=bottom; i++, k++) {
for(int j=left, l=0; j<=right ; j++, l++) {
outmat->set(k, l, mat->get(i,j));
}
}
res->matrRows = outmat_row_dim;
res->matrCols = outmat_col_dim;
long alg_end = get_usecs();
res->time = ((double)(alg_end-alg_start))/1000000;
// Print output matrix
#ifdef _PRINT_INFO
cout << "Submatrix found" << "\n";
printMatrix(outmat);
#endif
// Release resources
delete mat;
delete ps;
delete outmat;
}
void assemble_cortical(const Geometry& geo, Matrix& mat, const Head2EEGMat& M, const std::string& domain_name, const unsigned gauss_order, double alpha, double beta, const std::string &filename)
{
// Following the article: M. Clerc, J. Kybic "Cortical mapping by Laplace–Cauchy transmission using a boundary element method".
// Assumptions:
// - domain_name: the domain containing the sources is an innermost domain (defined as the interior of only one interface (called Cortex)
// - Cortex interface is composed of one mesh only (no shared vertices)
// TODO check orders of MxM products for efficiency ... delete intermediate matrices
const Domain& SourceDomain = geo.domain(domain_name);
const Interface& Cortex = SourceDomain.begin()->interface();
const Mesh& cortex = Cortex.begin()->mesh();
// test the assumption
assert(SourceDomain.size() == 1);
assert(Cortex.size() == 1);
// shape of the new matrix:
unsigned Nl = geo.size()-geo.outermost_interface().nb_triangles()-Cortex.nb_vertices()-Cortex.nb_triangles();
unsigned Nc = geo.size()-geo.outermost_interface().nb_triangles();
std::fstream f(filename.c_str());
Matrix P;
if ( !f ) {
// build the HeadMat:
// The following is the same as assemble_HM except N_11, D_11 and S_11 are not computed.
SymMatrix mat_temp(Nc);
mat_temp.set(0.0);
double K = 1.0 / (4.0 * M_PI);
// We iterate over the meshes (or pair of domains) to fill the lower half of the HeadMat (since its symmetry)
for ( Geometry::const_iterator mit1 = geo.begin(); mit1 != geo.end(); ++mit1) {
for ( Geometry::const_iterator mit2 = geo.begin(); (mit2 != (mit1+1)); ++mit2) {
// if mit1 and mit2 communicate, i.e they are used for the definition of a common domain
const int orientation = geo.oriented(*mit1, *mit2); // equals 0, if they don't have any domains in common
// equals 1, if they are both oriented toward the same domain
// equals -1, if they are not
if ( orientation != 0) {
double Scoeff = orientation * geo.sigma_inv(*mit1, *mit2) * K;
double Dcoeff = - orientation * geo.indicator(*mit1, *mit2) * K;
double Ncoeff;
if ( !(mit1->outermost() || mit2->outermost()) && ( (*mit1 != *mit2)||( *mit1 != cortex) ) ) {
// Computing S block first because it's needed for the corresponding N block
operatorS(*mit1, *mit2, mat_temp, Scoeff, gauss_order);
Ncoeff = geo.sigma(*mit1, *mit2)/geo.sigma_inv(*mit1, *mit2);
} else {
Ncoeff = orientation * geo.sigma(*mit1, *mit2) * K;
}
if ( !mit1->outermost() && (( (*mit1 != *mit2)||( *mit1 != cortex) )) ) {
// Computing D block
operatorD(*mit1, *mit2, mat_temp, Dcoeff, gauss_order);
}
if ( ( *mit1 != *mit2 ) && ( !mit2->outermost() ) ) {
// Computing D* block
operatorD(*mit1, *mit2, mat_temp, Dcoeff, gauss_order, true);
}
// Computing N block
if ( (*mit1 != *mit2)||( *mit1 != cortex) ) {
operatorN(*mit1, *mit2, mat_temp, Ncoeff, gauss_order);
}
}
}
}
// Deflate the diagonal block (N33) of 'mat' : (in order to have a zero-mean potential for the outermost interface)
const Interface i = geo.outermost_interface();
unsigned i_first = (*i.begin()->mesh().vertex_begin())->index();
deflat(mat_temp, i, mat_temp(i_first, i_first) / (geo.outermost_interface().nb_vertices()));
mat = Matrix(Nl, Nc);
mat.set(0.0);
// copy mat_temp into mat except the lines for cortex vertices [i_vb_c, i_ve_c] and cortex triangles [i_tb_c, i_te_c].
unsigned iNl = 0;
unsigned i_vb_c = (*cortex.vertex_begin())->index();
unsigned i_ve_c = (*cortex.vertex_rbegin())->index();
unsigned i_tb_c = cortex.begin()->index();
unsigned i_te_c = cortex.rbegin()->index();
for ( unsigned i = 0; i < Nc; ++i) {
if ( !(i_vb_c<=i && i<=i_ve_c) && !(i_tb_c<=i && i<=i_te_c) ) {
mat.setlin(iNl, mat_temp.getlin(i));
++iNl;
}
}
// ** Construct P: the null-space projector **
Matrix W;
{
Matrix U, s;
mat.svd(U, s, W);
}
SparseMatrix S(Nc,Nc);
// we set S to 0 everywhere, except in the last part of the diag:
for ( unsigned i = Nl; i < Nc; ++i) {
S(i, i) = 1.0;
}
P = (W * S) * W.transpose(); // P is a projector: P^2 = P and mat*P*X = 0
if ( filename.length() != 0 ) {
std::cout << "Saving projector P (" << filename << ")." << std::endl;
P.save(filename);
}
} else {
std::cout << "Loading projector P (" << filename << ")." << std::endl;
P.load(filename);
}
// ** Get the gradient of P1&P0 elements on the meshes **
Matrix MM(M.transpose() * M);
SymMatrix RR(Nc, Nc); RR.set(0.);
for ( Geometry::const_iterator mit = geo.begin(); mit != geo.end(); ++mit) {
mit->gradient_norm2(RR);
}
// ** Choose Regularization parameter **
SparseMatrix alphas(Nc,Nc); // diagonal matrix
Matrix Z;
if ( alpha < 0 ) { // try an automatic method... TODO find better estimation
double nRR_v = RR.submat(0, geo.nb_vertices(), 0, geo.nb_vertices()).frobenius_norm();
alphas.set(0.);
alpha = MM.frobenius_norm() / (1.e3*nRR_v);
beta = alpha * 50000.;
for ( Vertices::const_iterator vit = geo.vertex_begin(); vit != geo.vertex_end(); ++vit) {
alphas(vit->index(), vit->index()) = alpha;
}
for ( Meshes::const_iterator mit = geo.begin(); mit != geo.end(); ++mit) {
if ( !mit->outermost() ) {
for ( Mesh::const_iterator tit = mit->begin(); tit != mit->end(); ++tit) {
alphas(tit->index(), tit->index()) = beta;
}
}
}
std::cout << "AUTOMATIC alphas = " << alpha << "\tbeta = " << beta << std::endl;
} else {
for ( Vertices::const_iterator vit = geo.vertex_begin(); vit != geo.vertex_end(); ++vit) {
alphas(vit->index(), vit->index()) = alpha;
}
for ( Meshes::const_iterator mit = geo.begin(); mit != geo.end(); ++mit) {
if ( !mit->outermost() ) {
for ( Mesh::const_iterator tit = mit->begin(); tit != mit->end(); ++tit) {
alphas(tit->index(), tit->index()) = beta;
}
}
}
std::cout << "alphas = " << alpha << "\tbeta = " << beta << std::endl;
}
Z = P.transpose() * (MM + alphas*RR) * P;
// ** PseudoInverse and return **
// X = P * { (M*P)' * (M*P) + (R*P)' * (R*P) }¡(-1) * (M*P)'m
// X = P * { P'*M'*M*P + P'*R'*R*P }¡(-1) * P'*M'm
// X = P * { P'*(MM + a*RR)*P }¡(-1) * P'*M'm
// X = P * Z¡(-1) * P' * M'm
Matrix rhs = P.transpose() * M.transpose();
mat = P * Z.pinverse() * rhs;
}
