2 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
4 // + This file is part of enGrid. +
6 // + Copyright 2008,2009 Oliver Gloth +
8 // + enGrid is free software: you can redistribute it and/or modify +
9 // + it under the terms of the GNU General Public License as published by +
10 // + the Free Software Foundation, either version 3 of the License, or +
11 // + (at your option) any later version. +
13 // + enGrid is distributed in the hope that it will be useful, +
14 // + but WITHOUT ANY WARRANTY; without even the implied warranty of +
15 // + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +
16 // + GNU General Public License for more details. +
18 // + You should have received a copy of the GNU General Public License +
19 // + along with enGrid. If not, see <http://www.gnu.org/licenses/>. +
21 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
23 #include "geometrytools.h"
24 #include "containertricks.h"
26 #include <vtkCellType.h>
29 namespace GeometryTools
{
31 double rad2deg( double rad
)
36 double deg2rad( double deg
)
41 void rotate(vec3_t g1
, vec3_t g2
,vec3_t g3
, vec3_t
&b
, double theta
)
48 mat3_t g
= g_t
.transp();
52 mat3_t rot
= mat3_t::identity();
53 rot
[0][0] = cos(theta
);
54 rot
[0][1] = -sin(theta
);
55 rot
[1][0] = sin(theta
);
56 rot
[1][1] = cos(theta
);
57 //cout << rot << endl;
59 //cout << bbb << endl;
64 vec3_t
rotate(vec3_t v
, vec3_t axis
, double theta
)
68 // transposed base of rotate system
71 // compute projection of v in axis direction
72 vec3_t v_axis
= (axis
*v
)*axis
;
74 // compute first orthogonal vector (first base vector)
77 //In case of points on the rotation axis, do nothing
78 if(g_t
[0].abs()==0) return v
;
82 // second base vector is the normalised axis
85 // compute second orthogonal vector (third base vector)
86 g_t
[2] = g_t
[0].cross(g_t
[1]);
88 // base of rotate system
89 mat3_t g
= g_t
.transp();
91 // matrix for rotation around g_t[1];
92 mat3_t rot
= mat3_t::identity();
93 rot
[0][0] = cos(theta
);
94 rot
[0][2] = sin(theta
);
95 rot
[2][0] = -sin(theta
);
96 rot
[2][2] = cos(theta
);
98 // transfer v to rotate system
101 // rotate the vector and transfer it back
108 vec3_t
orthogonalVector(vec3_t v
)
114 for (int i
= 1; i
< 3; ++i
) {
115 if (v
[i
] > v
[i_max
]) i_max
= i
;
116 if (v
[i
] < v
[i_min
]) i_min
= i
;
127 double intersection(vec3_t x_straight
, vec3_t v_straight
, vec3_t x_plane
, vec3_t u_plane
, vec3_t v_plane
)
129 vec3_t n
= u_plane
.cross(v_plane
);
130 return intersection(x_straight
,v_straight
,x_plane
,n
);
133 double intersection(vec3_t x_straight
, vec3_t v_straight
, vec3_t x_plane
, vec3_t n_plane
)
135 double k
= (x_plane
*n_plane
- x_straight
*n_plane
)/(v_straight
*n_plane
);
139 bool intersection (double &k1
, double &k2
, vec2_t r1
, vec2_t u1
, vec2_t r2
, vec2_t u2
)
141 double ave_length
= .5*(sqrt(u1
[0]*u1
[0]+u1
[1]*u1
[1]) + sqrt(u2
[0]*u2
[0]+u2
[1]*u2
[1]));
142 double DET
= (u1
[0]*u2
[1]-u1
[1]*u2
[0]);
143 if (fabs(DET
) > 1e-6*ave_length
) {
144 k1
= -(u2
[0]*r2
[1]-u2
[0]*r1
[1]-r2
[0]*u2
[1]+r1
[0]*u2
[1])/DET
;
145 k2
= -(-u1
[1]*r2
[0]+u1
[0]*r2
[1]-u1
[0]*r1
[1]+u1
[1]*r1
[0])/DET
;
152 void sliceTriangle(const vector
<vec3_t
> &Tin
, vec3_t x
, vec3_t n
, vector
<vector
<vec3_t
> > &Tout
)
157 double kab
= intersection(a
,b
-a
,x
,n
);
158 double kbc
= intersection(b
,c
-b
,x
,n
);
159 double kca
= intersection(c
,a
-c
,x
,n
);
160 bool ab_cut
= ((kab
>= 0) && (kab
<= 1));
161 bool bc_cut
= ((kbc
>= 0) && (kbc
<= 1));
162 bool ca_cut
= ((kca
>= 0) && (kca
<= 1));
163 if (ab_cut
&& bc_cut
&& ca_cut
) {
164 //cerr << "invalid triangle (SliceTriangle) A" << endl;
165 //exit(EXIT_FAILURE);
166 if ((kab
<= kbc
) && (kab
<= kca
)) ab_cut
= false;
167 else if ((kbc
<= kab
) && (kbc
<= kca
)) bc_cut
= false;
170 if (ab_cut
&& bc_cut
) {
171 vec3_t ab
= a
+ kab
*(b
-a
);
172 vec3_t bc
= b
+ kbc
*(c
-b
);
173 Tout
.resize(3,vector
<vec3_t
>(3));
174 clinit(Tout
[0]) = a
,ab
,bc
;
175 clinit(Tout
[1]) = ab
,b
,bc
;
176 clinit(Tout
[2]) = bc
,c
,a
;
177 } else if (bc_cut
&& ca_cut
) {
178 vec3_t bc
= b
+ kbc
*(c
-b
);
179 vec3_t ca
= c
+ kca
*(a
-c
);
180 Tout
.resize(3,vector
<vec3_t
>(3));
181 clinit(Tout
[0]) = a
,bc
,ca
;
182 clinit(Tout
[1]) = a
,b
,bc
;
183 clinit(Tout
[2]) = bc
,c
,ca
;
184 } else if (ca_cut
&& ab_cut
) {
185 vec3_t ca
= c
+ kca
*(a
-c
);
186 vec3_t ab
= a
+ kab
*(b
-a
);
187 Tout
.resize(3,vector
<vec3_t
>(3));
188 clinit(Tout
[0]) = a
,ab
,ca
;
189 clinit(Tout
[1]) = ab
,b
,ca
;
190 clinit(Tout
[2]) = b
,c
,ca
;
192 Tout
.resize(1,vector
<vec3_t
>(3));
193 clinit(Tout
[0]) = a
,b
,c
;
198 double tetraVol(vec3_t x0
, vec3_t x1
, vec3_t x2
, vec3_t x3
, bool neg
)
203 double V
= v1
*(v2
.cross(v3
));
205 if (!neg
&& (V
< 0)) {
211 Lmin
= min(Lmin
,v1
.abs());
212 Lmin
= min(Lmin
,v2
.abs());
213 Lmin
= min(Lmin
,v3
.abs());
214 Lmin
= min(Lmin
,v4
.abs());
215 Lmin
= min(Lmin
,v5
.abs());
216 Lmin
= min(Lmin
,v6
.abs());
217 Lmax
= max(Lmax
,v1
.abs());
218 Lmax
= max(Lmax
,v2
.abs());
219 Lmax
= max(Lmax
,v3
.abs());
220 Lmax
= max(Lmax
,v4
.abs());
221 Lmax
= max(Lmax
,v5
.abs());
222 Lmax
= max(Lmax
,v6
.abs());
223 if (Lmin
/Lmax
< 1e-6) {
234 double pyraVol(vec3_t x0
, vec3_t x1
, vec3_t x2
, vec3_t x3
, vec3_t x4
, bool neg
)
236 double V1
= tetraVol(x0
, x1
, x3
, x4
, neg
) + tetraVol(x1
, x2
, x3
, x4
, neg
);
237 double V2
= tetraVol(x0
, x1
, x2
, x4
, neg
) + tetraVol(x2
, x3
, x0
, x4
, neg
);
241 vec3_t m0 = .25*(x0+x1+x2+x3);
242 V += tetraVol(x0, x1, m0, x4, neg);
243 V += tetraVol(x1, x2, m0, x4, neg);
244 V += tetraVol(x2, x3, m0, x4, neg);
245 V += tetraVol(x3, x0, m0, x4, neg);
250 double prismVol(vec3_t x0
, vec3_t x1
, vec3_t x2
, vec3_t x3
, vec3_t x4
, vec3_t x5
, bool neg
)
253 vec3_t p
= 1.0/6.0*(x0
+x1
+x2
+x3
+x4
+x5
);
254 V
+= tetraVol(x0
, x2
, x1
, p
, neg
);
255 V
+= tetraVol(x3
, x4
, x5
, p
, neg
);
256 V
+= pyraVol (x0
, x1
, x4
, x3
, p
, neg
);
257 V
+= pyraVol (x1
, x2
, x5
, x4
, p
, neg
);
258 V
+= pyraVol (x0
, x3
, x5
, x2
, p
, neg
);
262 double hexaVol(vec3_t x0
, vec3_t x1
, vec3_t x2
, vec3_t x3
, vec3_t x4
, vec3_t x5
, vec3_t x6
, vec3_t x7
, bool neg
)
265 vec3_t p
= 1.0/8.0*(x0
+x1
+x2
+x3
+x4
+x5
+x6
+x7
);
266 V
+= pyraVol(x0
, x1
, x3
, x2
, p
, neg
);
267 V
+= pyraVol(x0
, x4
, x5
, x1
, p
, neg
);
268 V
+= pyraVol(x4
, x6
, x7
, x5
, p
, neg
);
269 V
+= pyraVol(x2
, x3
, x7
, x6
, p
, neg
);
270 V
+= pyraVol(x1
, x5
, x7
, x3
, p
, neg
);
271 V
+= pyraVol(x0
, x2
, x6
, x4
, p
, neg
);
275 double triArea(vec3_t x0
, vec3_t x1
, vec3_t x2
)
279 double A
= 0.5*((a
.cross(b
)).abs());
283 double quadArea(vec3_t x0
, vec3_t x1
, vec3_t x2
, vec3_t x3
)
286 vec3_t p
= .25*(x0
+x1
+x2
+x3
);
287 A
+= triArea(x0
,x1
,p
);
288 A
+= triArea(x1
,x2
,p
);
289 A
+= triArea(x2
,x3
,p
);
290 A
+= triArea(x3
,x0
,p
);
294 vec3_t
triNormal(vec3_t x0
, vec3_t x1
, vec3_t x2
)
298 vec3_t n
= 0.5*(a
.cross(b
));
302 vec3_t
quadNormal(vec3_t x0
, vec3_t x1
, vec3_t x2
, vec3_t x3
)
306 vec3_t p
= .25*(x0
+x1
+x2
+x3
);
307 n
+= triNormal(x0
,x1
,p
);
308 n
+= triNormal(x1
,x2
,p
);
309 n
+= triNormal(x2
,x3
,p
);
310 n
+= triNormal(x3
,x0
,p
);
314 vec3_t
triNormal(vtkUnstructuredGrid
*grid
, vtkIdType p1
, vtkIdType p2
, vtkIdType p3
)
317 grid
->GetPoint(p1
,x1
.data());
318 grid
->GetPoint(p2
,x2
.data());
319 grid
->GetPoint(p3
,x3
.data());
320 return triNormal(x1
,x2
,x3
);
323 vec3_t
quadNormal(vtkUnstructuredGrid
*grid
, vtkIdType p1
, vtkIdType p2
, vtkIdType p3
, vtkIdType p4
)
325 vec3_t x1
, x2
, x3
, x4
;
326 grid
->GetPoint(p1
,x1
.data());
327 grid
->GetPoint(p2
,x2
.data());
328 grid
->GetPoint(p3
,x3
.data());
329 grid
->GetPoint(p4
,x4
.data());
330 return quadNormal(x1
,x2
,x3
,x4
);
333 vec3_t
cellNormal(vtkUnstructuredGrid
*grid
, vtkIdType i
)
339 grid
->GetCellPoints(i
,npts
,pts
);
340 if (npts
== 3) return triNormal(grid
,pts
[0],pts
[1],pts
[2]);
341 else if (npts
== 4) return quadNormal(grid
,pts
[0],pts
[1],pts
[2],pts
[3]);
345 double cellVA(vtkUnstructuredGrid
*grid
, vtkIdType cellId
, bool neg
)
350 grid
->GetCellPoints(cellId
, Npts
, pts
);
351 for (int i_pts
= 0; i_pts
< Npts
; ++i_pts
) {
352 grid
->GetPoints()->GetPoint(pts
[i_pts
], p
[i_pts
].data());
354 vtkIdType cellType
= grid
->GetCellType(cellId
);
355 if (cellType
== VTK_TRIANGLE
) return triArea (p
[0], p
[1], p
[2]);
356 else if (cellType
== VTK_QUAD
) return quadArea(p
[0], p
[1], p
[2], p
[3]);
357 else if (cellType
== VTK_TETRA
) return tetraVol(p
[0], p
[1], p
[2], p
[3], neg
);
358 else if (cellType
== VTK_PYRAMID
) return pyraVol (p
[0], p
[1], p
[2], p
[3], p
[4], neg
);
359 else if (cellType
== VTK_WEDGE
) return prismVol(p
[0], p
[1], p
[2], p
[3], p
[4], p
[5], neg
);
360 else if (cellType
== VTK_HEXAHEDRON
) return hexaVol (p
[0], p
[1], p
[2], p
[3], p
[4], p
[5], p
[6], p
[7], neg
);
364 double angle(const vec3_t
& u
, const vec3_t
& v
)
366 // return the angle w.r.t. another 3-vector
367 double ptot2
= u
.abs2()*v
.abs2();
371 double arg
= (u
*v
)/sqrt(ptot2
);
372 if(arg
> 1.0) arg
= 1.0;
373 if(arg
< -1.0) arg
= -1.0;