int32 _intersect_line_plane(float64 *pt, float64 p00[3], float64 p01[3],
float64 pp[3], float64 pn[3], int32 dim)
{
int32 ii;
float64 dot1, dot2;
float64 v0[3], r[3];
_mul_c_add_v3(v0, p01, -1.0, p00, dim);
dot1 = 0.0;
for (ii = 0; ii < dim; ii++) {
dot1 += fabs(v0[ii]);
}
if (dot1 < 1e-10) {
*pt = 0.0;
goto end_label;
}
_mul_c_add_v3(r, p00, -1.0, pp, dim);
gtr_dot_v3(&dot1, pn, v0, dim);
if (fabs(dot1) <= 1e-10) {
*pt = 1e10;
goto end_label;
}
gtr_dot_v3(&dot2, pn, r, dim);
*pt = - dot2 / dot1;
end_label:
return(RET_OK);
}
int32 _intersect_line_triangle(float64 *pt, float64 p00[3], float64 p01[3],
float64 pps[9], float64 pn[3])
{
float64 duv, dwv, dwu, duu, dvv, aux, t1, t2;
float64 u[3], v[3], w[3], tmp[3];
_intersect_line_plane(pt, p00, p01, pps, pn, 3);
// u = pps[1] - pps[0].
_mul_c_add_v3(u, pps + 3, -1.0, pps, 3);
// v = pps[2] - pps[0].
_mul_c_add_v3(v, pps + 6, -1.0, pps, 3);
// w = p00 + t * (p01 - p00) - pps[0].
_mul_c_add_v3(tmp, p01, -1.0, p00, 3);
_mul_c_add_v3(w, p00, *pt, tmp, 3);
_mul_c_add_v3(w, w, -1.0, pps, 3);
gtr_dot_v3(&duv, u, v, 3);
gtr_dot_v3(&dwv, w, v, 3);
gtr_dot_v3(&dwu, w, u, 3);
gtr_dot_v3(&duu, u, u, 3);
gtr_dot_v3(&dvv, v, v, 3);
aux = duv * duv - duu * dvv;
if (fabs(aux/duu) < 1e-14) {
*pt = 1e10;
goto end_label;
}
t1 = (duv * dwv - dvv * dwu) / aux;
t2 = (duv * dwu - duu * dwv) / aux;
if ((t1 < -1e-10) || (t2 < -1e-10) || ((t1 + t2) > (1.0 + 1e-10))) {
*pt = 1e10;
}
end_label:
return(RET_OK);
}
/*!
For 3D and 2D.
@par Revision history:
- 08.06.2006, c
- 02.08.2006
*/
int32 orient_elements( int32 *flag, int32 flag_n_row,
int32 *conn, int32 conn_n_row, int32 conn_n_col,
float64 *coors, int32 coors_n_row, int32 coors_n_col,
int32 *v_roots, int32 v_roots_n_row,
int32 *v_vecs, int32 v_vecs_n_row, int32 v_vecs_n_col,
int32 *swap_from, int32 swap_from_n_row, int32 swap_from_n_col,
int32 *swap_to, int32 swap_to_n_row, int32 swap_to_n_col )
{
#define IR( iel, ir ) (conn[conn_n_col*(iel)+v_roots[ir]])
#define IV( iel, ir, iv ) (conn[conn_n_col*(iel)+v_vecs[v_vecs_n_col*ir+iv]])
#define CONN( iel, ip ) (conn[conn_n_col*(iel)+ip])
#define SWF( ir, is ) (swap_from[swap_from_n_col*ir+is])
#define SWT( ir, is ) (swap_to[swap_to_n_col*ir+is])
int32 ir, iel, ii, ip0, ip1, ip2, ip3, tmp, nc;
float64 v0[3], v1[3], v2[3], v3[3], cross[3], dot[1];
nc = coors_n_col;
if (nc == 4) { // 3D.
for (iel = 0; iel < conn_n_row; iel++) {
flag[iel] = 0;
for (ir = 0; ir < v_roots_n_row; ir++) {
ip0 = IR( iel, ir );
ip1 = IV( iel, ir, 0 );
ip2 = IV( iel, ir, 1 );
ip3 = IV( iel, ir, 2 );
for (ii = 0; ii < 3; ii++) {
v0[ii] = coors[nc*ip0+ii];
v1[ii] = coors[nc*ip1+ii] - v0[ii];
v2[ii] = coors[nc*ip2+ii] - v0[ii];
v3[ii] = coors[nc*ip3+ii] - v0[ii];
}
gtr_cross_product( cross, v1, v2 );
gtr_dot_v3( dot, v3, cross );
/* output( "%d %d -> %d %d %d %d %e\n", iel, ir, ip0, ip1, ip2, ip3, */
/* dot[0] ); */
if (dot[0] < CONST_MachEps) {
flag[iel]++;
for (ii = 0; ii < swap_from_n_col; ii++) {
SwapValues( CONN( iel, SWF( ir, ii ) ),
CONN( iel, SWT( ir, ii ) ), tmp );
/* output( "%d %d\n", SWF( ir, ii ), SWT( ir, ii ) ); */
}
}
}
/* sys_pause(); */
}
} else if (nc == 3) { // 2D.
for (iel = 0; iel < conn_n_row; iel++) {
flag[iel] = 0;
for (ir = 0; ir < v_roots_n_row; ir++) {
ip0 = IR( iel, ir );
ip1 = IV( iel, ir, 0 );
ip2 = IV( iel, ir, 1 );
for (ii = 0; ii < 2; ii++) {
v0[ii] = coors[nc*ip0+ii];
v1[ii] = coors[nc*ip1+ii] - v0[ii];
v2[ii] = coors[nc*ip2+ii] - v0[ii];
}
v1[2] = v2[2] = 0.0;
gtr_cross_product( cross, v1, v2 );
if (cross[2] < CONST_MachEps) {
flag[iel]++;
for (ii = 0; ii < swap_from_n_col; ii++) {
SwapValues( CONN( iel, SWF( ir, ii ) ),
CONN( iel, SWT( ir, ii ) ), tmp );
}
}
}
}
}
return( RET_OK );
#undef IR
#undef IV
#undef CONN
#undef SWF
#undef SWT
}
// `volumes` must be preallocated.
int32 mesh_get_volumes(Mesh *mesh, float64 *volumes, int32 dim)
{
#define VS(ic, id) (coors[nc*entity_vertices->indices[ic] + id])
int32 ret = RET_OK;
uint32 D = mesh->topology->max_dim;
uint32 nc = mesh->geometry->dim;
uint32 id;
uint32 indx2[6];
float64 vol, aux, vv0, vv1, vv2, vv3;
float64 *ptr = volumes;
float64 *coors = mesh->geometry->coors;
float64 v0[3], v1[3], v2[3], ndir[3];
Indices entity_vertices[1];
MeshEntityIterator it0[1];
MeshConnectivity *cd0 = 0; // d -> 0
if (!dim) {
errput("vertices have no volume!\n");
ERR_CheckGo(ret);
}
cd0 = mesh->topology->conn[IJ(D, dim, 0)];
for (mei_init(it0, mesh, dim); mei_go(it0); mei_next(it0)) {
me_get_incident2(it0->entity, entity_vertices, cd0);
/* mei_print(it0, stdout); */
/* ind_print(entity_vertices, stdout); */
vol = 0;
if (dim == 1) { // Edges.
for (id = 0; id < nc; id++) {
aux = VS(1, id) - VS(0, id);
vol += aux * aux;
}
ptr[0] = sqrt(vol);
} else if (entity_vertices->num == 3) { // Triangles.
ptr[0] = _tri_area(coors, entity_vertices->indices, nc);
} else if (nc == 2) { // Quadrilateral cells.
ptr[0] = _tri_area(coors, entity_vertices->indices, nc);
indx2[0] = entity_vertices->indices[2];
indx2[1] = entity_vertices->indices[3];
indx2[2] = entity_vertices->indices[0];
ptr[0] += _tri_area(coors, indx2, nc);
} else if (nc == 3) { // 3D.
if (entity_vertices->num == 4) {
if (dim == 2) { // Quadrilateral faces (approximate).
indx2[0] = entity_vertices->indices[0];
indx2[1] = entity_vertices->indices[1];
indx2[2] = entity_vertices->indices[2];
indx2[3] = entity_vertices->indices[3];
indx2[4] = entity_vertices->indices[0];
indx2[5] = entity_vertices->indices[1];
aux = _tri_area(coors, indx2, nc);
aux += _tri_area(coors, indx2 + 1, nc);
aux += _tri_area(coors, indx2 + 2, nc);
aux += _tri_area(coors, indx2 + 3, nc);
ptr[0] = 0.5 * aux;
} else { // Tetrahedral cells.
for (id = 0; id < nc; id++) {
vv0 = VS(0, id);
vv1 = VS(1, id);
vv2 = VS(2, id);
vv3 = VS(3, id);
v0[id] = vv1 - vv0;
v1[id] = vv2 - vv0;
v2[id] = vv3 - vv2;
}
gtr_cross_product(ndir, v0, v1);
gtr_dot_v3(ptr, v2, ndir, 3);
ptr[0] /= 6.0;
}
} else { // Hexahedral cells with trilinear interpolation.
// See https://math.stackexchange.com/questions/1628540/what-is-the-enclosed-volume-of-an-irregular-cube-given-the-x-y-z-coordinates-of
// Uses 0 1 3 2 4 5 7 6 ordering w.r.t. sfepy.
aux = _aux_hex(coors, entity_vertices->indices, nc, 0, 1, 3, 2);
aux -= _aux_hex(coors, entity_vertices->indices, nc, 4, 5, 7, 6);
aux -= _aux_hex(coors, entity_vertices->indices, nc, 0, 1, 4, 5);
aux += _aux_hex(coors, entity_vertices->indices, nc, 3, 2, 7, 6);
aux += _aux_hex(coors, entity_vertices->indices, nc, 0, 3, 4, 7);
aux -= _aux_hex(coors, entity_vertices->indices, nc, 1, 2, 5, 6);
ptr[0] = aux / 12.0;
}
}
ptr += 1;
}
end_label:
return(ret);
#undef VS
}
