Blame view

src/ExternLib/Tetrahedron/common/tetrahedron.cc 3.82 KB
73157db7   Benjamin Renard   Add tetrahedron f...
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
#include<vector>
#include<iostream>
#include<cmath>
#include<algorithm>

using Vector = std::vector<double>;
using Matrix = std::vector<Vector>;

#ifdef DEBUG
void print_matrix(Matrix);
#endif

Matrix concatenate_vectors(Vector p1, Vector p2, Vector p3, Vector p4) {
	Matrix mat;
	mat.push_back(p1);
	mat.push_back(p2);
	mat.push_back(p3);
	mat.push_back(p4);
	return mat;
}

Matrix transpose(Matrix mat) {
	Matrix t; // new matrix
	int n = mat.size();
	int m = mat[0].size();
	for(int i=0;i<m;i++) {
		// new row of size n
		Vector r;
		for(int j=0;j<n;j++) {
			r.push_back(mat[j][i]);
		}
		t.push_back(r);
	}
	return t;
}

double scalar_product(Vector vect1, Vector vect2) {
	double ans=0;
05101f8e   Benjamin Renard   Fix Tetrahedron l...
39
	for(int i=0;i<(int)vect1.size();i++) {
73157db7   Benjamin Renard   Add tetrahedron f...
40
41
42
43
44
45
46
47
48
49
50
51
52
		ans += vect1[i] * vect2[i];
	}
	return ans;
}

Matrix matrix_multiply(Matrix mat1, Matrix mat2) {
	Matrix mat2t = transpose(mat2);
	Matrix r;
	int n = mat1.size();
	int m = mat1[0].size();
	for(int i=0;i<n;i++) {
		// new row
		Vector row;
05101f8e   Benjamin Renard   Fix Tetrahedron l...
53
		for(int j=0;j<m;j++) row.push_back(scalar_product(mat1[i], mat2t[j]));
73157db7   Benjamin Renard   Add tetrahedron f...
54
55
56
57
58
59
60
61
			r.push_back(row);
	}
	return r;
		
}

#ifdef DEBUG
void print_vector(Vector vect) {
05101f8e   Benjamin Renard   Fix Tetrahedron l...
62
	for(int i=0;i<(int)vect.size();i++) std::cout << vect[i] << " ";
73157db7   Benjamin Renard   Add tetrahedron f...
63
64
65
66
	std::cout << std::endl;
}

void print_matrix(Matrix mat) {
05101f8e   Benjamin Renard   Fix Tetrahedron l...
67
	for(int i=0;i<(int)mat.size();i++) {
73157db7   Benjamin Renard   Add tetrahedron f...
68
69
70
71
72
73
74
		print_vector(mat[i]);
	}
}
#endif

Vector vector_substract(Vector vect1, Vector vect2) {
	Vector ans;
05101f8e   Benjamin Renard   Fix Tetrahedron l...
75
	for(int i=0;i<(int)vect1.size();i++) {
73157db7   Benjamin Renard   Add tetrahedron f...
76
77
78
79
80
81
82
83
84
		ans.push_back(vect1[i] - vect2[i]);
	}
	return ans;
}

Vector vector_projection(Vector v, Vector u) {
	Vector ans;
	double a = scalar_product(u,v);
	double b = scalar_product(u,u);
05101f8e   Benjamin Renard   Fix Tetrahedron l...
85
	for(int i=0;i<(int)v.size();i++) {
73157db7   Benjamin Renard   Add tetrahedron f...
86
87
88
89
90
91
92
93
		ans.push_back((a/b) * u[i]);
	}
	return ans;
}

Vector vector_normalize(Vector vect) {
	Vector ans;
	double norm = scalar_product(vect, vect);
05101f8e   Benjamin Renard   Fix Tetrahedron l...
94
	for(int i=0;i<(int)vect.size();i++) ans.push_back(vect[i] / sqrt(norm));
73157db7   Benjamin Renard   Add tetrahedron f...
95
96
97
98
99
100
101
	return ans;
}

Matrix gram_schmidt(Matrix mat) {
	Matrix matt = transpose(mat);
	Matrix basis;
	Matrix Us;
05101f8e   Benjamin Renard   Fix Tetrahedron l...
102
	for(int i=0;i<(int)mat.size();i++) {
73157db7   Benjamin Renard   Add tetrahedron f...
103
104
105
106
107
108
109
110
111
112
113
114
115
116
		// new basis vector
		Vector nb = matt[i];
		for(int j=0;j<=i-1;j++) {
			nb = vector_substract(nb, vector_projection(matt[i], Us[j]));
			Vector vv = vector_projection(matt[i], Us[j]);
		}
		Us.push_back(nb);
		nb = vector_normalize(nb);
		basis.push_back(vector_normalize(nb));
	}
	return transpose(basis);
}

bool is_upper_triangular(Matrix mat) {
05101f8e   Benjamin Renard   Fix Tetrahedron l...
117
	for(int i=0;i<(int)mat.size();i++) {
73157db7   Benjamin Renard   Add tetrahedron f...
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
		for(int j=0;j<i;j++) {
			if(mat[i][j] > 1e-16) return false;
		}
	}
	return true;
}

Vector matrix_eigenvalues(Matrix mat) {
	Matrix A = mat;
	for(int k=0;k<1000000;k++) {
		Matrix basis = gram_schmidt(A);
		Matrix basisT = transpose(basis);
		Matrix t1 = matrix_multiply(A, basis);
		Matrix t2 = matrix_multiply(basisT, t1);
		if(is_upper_triangular(t2)) break;

		A = t2;
	}
 	Vector eig;
05101f8e   Benjamin Renard   Fix Tetrahedron l...
137
	for(int i=0;i<(int)mat.size();i++) eig.push_back(A[i][i]);
73157db7   Benjamin Renard   Add tetrahedron f...
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
	return eig;
}

void compute_tetrahedron(Vector p1, Vector p2, Vector p3, Vector p4, double& elongation, double& planarity) {
	// make matrix by concatenating vectors
	Matrix mat = concatenate_vectors(p1,p2,p3,p4);

	Matrix matt = transpose(mat);

	// matrix product
	Matrix pp = matrix_multiply(matt, mat);

	Vector eig = matrix_eigenvalues(pp);

	// sort eigenvalues
	sort(eig.begin(), eig.end());

	elongation = 1. - sqrt(eig[0] / eig[1]);
	planarity = 1. - sqrt(eig[1] / eig[2]);
}

#ifdef DEBUG
Vector random_vector() {
	Vector v;
	for(int i=0;i<3;i++) v.push_back((double)(rand()%7));

	return v;
}

int main(int argc, char ** argv) {
	// create some random data and compute the elongation
	for(int i=0;i<1000000;i++) {
		// generate twelve random numbers
		Vector p1 = random_vector();
		Vector p2 = random_vector();
	     	Vector p3 = random_vector();
	       	Vector p4 = random_vector();
		std::cout << "Elongation " << elongation(p1,p2,p3,p4) << ", planarity: " << planarity(p1,p2,p3,p4) << std::endl;
	}
	return 0;
}
#endif