mesh_find.c 6.95 KB
Newer Older
1
2
3
4
5
#include <math.h>
#include <stdlib.h>
#include "quadtree.h"
#include "mesh.h"
#include "vector.h"
6
7
8
9
10
11
12
#include "mesh_find.h"

struct  MeshTree_{
  Cell *tree;
  double *elements;
  Mesh *mesh;
};
13
14
15
16
17
18
19
20
21
22
23
24
25

static void _bboxtol(double *bbmin, double *bbmax) {
  double lmax = 0;
  for (int i = 0; i < DIMENSION; ++i) {
    lmax = fmax(lmax, bbmax[i] - bbmin[i]);
  }
  double tol = 1e-8 * lmax;
  for (int i = 0; i < DIMENSION; ++i) {
    bbmax[i] += tol;
    bbmin[i] -= tol;
  }
}

26
const double volTet(const double x0[3], const double x1[3], const double x2[3], const double x3[3]) {
27
28
29
30
31
32
33
34
35
  // x1 * (y2 * (z3 - z4) - z2 * (y3 - y4) + y3 * z4 - y4 * z3)
  //- y1 * (x2 * (z3 - z4) - z2 * (x3 - x4) + x3 * z4 - x4 * z3)
  //+ z1 * (x2 * (y3 - y4) - y2 * (x3 - x4) + x3 * y4 - x4 * y3)
  //- (x2 * (y3 * z4 - y4 * z3) - y2 * (x3 * z4 - x4 * z3) + z2 * (x3 * y4 - x4 * y3))
  return  x0[0] * (x1[1] * (x2[2] - x3[2]) - x1[2] * (x2[1] - x3[1]) + x2[1] * x3[2] - x3[1] * x2[2])
    - x0[1] * (x1[0] * (x2[2] - x3[2]) - x1[2] * (x2[0] - x3[0]) + x2[0] * x3[2] - x3[0] * x2[2])
    + x0[2] * (x1[0] * (x2[1] - x3[1]) - x1[1] * (x2[0] - x3[0]) + x2[0] * x3[1] - x3[0] * x2[1])
    - (x1[0] * (x2[1] * x3[2] - x3[1] * x2[2]) - x1[1] * (x2[0] * x3[2] - x3[0] * x2[2]) + x1[2] * (x2[0] * x3[1] - x3[0] * x2[1]));
}
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
static inline int isInsideTet(const double x[3], const double x0[3], const double x1[3], const double x2[3], const double x3[3], double *xi){
  for (int i = 0; i< 3; ++i) {
    const double xx = x[i];
    if ((xx < x0[i] && xx< x1[i] && xx < x2[i] && xx < x3[i])
        ||(xx > x0[i] && xx> x1[i] && xx > x2[i] && xx > x3[i]))
      return 0;
  }
  double v = volTet(x0, x1, x2, x3);
  double v0 = volTet(x, x1, x2, x3);
  if (v0 / v < -1e-8)
    return 0;
  double v1 = volTet(x0, x, x2, x3);
  if (v1 / v < -1e-8)
    return 0;
  double v2 = volTet(x0, x1, x, x3);
  if (v2 / v < -1e-8)
    return 0;
  double v3 = volTet(x0, x1, x2, x);
  if (v3 / v < -1e-8)
    return 0;

  xi[0] = v1 / v;
  xi[1] = v2 / v;
  xi[2] = v3 / v;
  return 1;
}
62

63
void mesh_tree_particle_to_mesh(MeshTree *mt, size_t nParticles, double *particles, int *elid, double *Xi)
64
65
66
67
{
  int *found = NULL;
  vectorPushN(&found, 100);
  vectorClear(found);
68
69
  int N = DIMENSION + 1;
  const double *elements = mt->elements;
70
71
  for (size_t i = 0; i < nParticles; ++i) {
    double *x = &particles[i * DIMENSION];
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
72
    if (elid[i]!=-1) {
73
      const double *el = &elements[elid[i] * N * DIMENSION];
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
74
#if DIMENSION == 2
75
      const double *X[3] = {el, el + DIMENSION, el + DIMENSION * 2};
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
76
77
78
79
80
81
82
83
84
85
86
87
88
      double dx = x[0] - X[0][0], dy = x[1] - X[0][1];
      double DX[2] = {X[1][0] - X[0][0], X[2][0] - X[0][0]};
      double DY[2] = {X[1][1] - X[0][1], X[2][1] - X[0][1]};
      double det = DX[1] * DY[0] - DX[0] * DY[1];
      double xi  = (DX[1] * dy - DY[1] * dx) / det;
      double eta = -(DX[0] * dy - DY[0] * dx) / det;
      if (xi > -1e-8 && eta > -1e-8 && 1 - xi - eta > -1e-8) {
        Xi[i * DIMENSION + 0] = xi;
        Xi[i * DIMENSION + 1] = eta;
      }
      else
        elid[i] = -1;
#else
89
90
91
92
      const double *X[4] = {el, el + DIMENSION, el + DIMENSION * 2, el + DIMENSION * 3};
      double xi[3];
      if (!isInsideTet(x,el,el+3,el+6,el+9,Xi+i*DIMENSION))
        elid[i] = -1;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
93
94
95
96
#endif
    }
    if (elid[i] != -1)
    continue;
97
    cellSearch(mt->tree, x, x, &found);
98
    for (size_t j = 0; j < vectorSize(found); ++j) {
99
      const double *el = &elements[found[j] * N * DIMENSION];
100
      if (DIMENSION == 2)  {
101
        const double *X[3] = {el, el + DIMENSION, el + DIMENSION * 2};
102
103
104
105
106
107
108
109
110
111
112
113
114
115
        double dx = x[0] - X[0][0], dy = x[1] - X[0][1];
        double DX[2] = {X[1][0] - X[0][0], X[2][0] - X[0][0]};
        double DY[2] = {X[1][1] - X[0][1], X[2][1] - X[0][1]};
        double det = DX[1] * DY[0] - DX[0] * DY[1];
        double xi  = (DX[1] * dy - DY[1] * dx) / det;
        double eta = -(DX[0] * dy - DY[0] * dx) / det;
        if (xi > -1e-8 && eta > -1e-8 && 1 - xi - eta > -1e-8) {
          Xi[i * DIMENSION + 0] = xi;
          Xi[i * DIMENSION + 1] = eta;
          elid[i] = found[j];
          break;
        }
      }
      else {
116
117
118
119
        if (isInsideTet(x,el,el+3,el+6,el+9,Xi+i*DIMENSION)){
          elid[i] = found[j];
          break;
        }
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
      }
    }
    /*if (elid[i] == -1) {
      printf(" PARTICLE %i OUTSIDE DOMAIN N2 search!!!\n", i);
      double toll = -1000;
      for (size_t j = 0; j < nElements; ++j) {
        double *el = &elements[j * N * DIMENSION];
        double *X[3] = {el, el + DIMENSION, el + DIMENSION * 2};
        double dx = x[0] - X[0][0], dy = x[1] - X[0][1];
        double DX[2] = {X[1][0] - X[0][0], X[2][0] - X[0][0]};
        double DY[2] = {X[1][1] - X[0][1], X[2][1] - X[0][1]};
        double det = DX[1] * DY[0] - DX[0] * DY[1];
        double xi  = (DX[1] * dy - DY[1] * dx) / det;
        double eta = -(DX[0] * dy - DY[0] * dx) / det;
        toll = fmax(toll, fmin(-xi, fmin(-eta, xi + eta -1)));
        if (xi > -1e-8 && eta > -1e-8 && 1 - xi - eta > -1e-8) {
          Xi[i * DIMENSION + 0] = xi;
          Xi[i * DIMENSION + 1] = eta;
          elid[i] = j;
          break;
        }
      }*/
      if (elid[i] == -1) {
        //printf(" PARTICLE %lu OUTSIDE DOMAIN!!! %g %g\n", i, x[0], x[1]);
        //exit(1);
      }
    //}
147
    vectorClear(found);
148
149
150
151
  }
  vectorFree(found);
}

152
153
154
155
156
157

MeshTree *mesh_tree_create(Mesh *mesh) {
  MeshTree *mt = malloc(sizeof(MeshTree));
  mt->mesh = mesh;
  size_t nElements = mesh->n_elements;
#if DIMENSION == 2
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
158
  double *elements = malloc(sizeof(double)*6*mesh->n_elements);
159
  for (int i = 0; i < nElements*3; ++i) {
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
160
    int iv = mesh->elements[i];
161
162
163
    elements[i*2+0] = mesh->x[iv*3+0];
    elements[i*2+1] = mesh->x[iv*3+1];
  }
164
#else
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
165
166
167
168
169
170
171
  double *elements = malloc(sizeof(double)*12*mesh->n_elements);
  for (int i = 0; i < mesh->n_elements*4; ++i) {
    int iv = mesh->elements[i];
    elements[i*3+0] = mesh->x[iv*3+0];
    elements[i*3+1] = mesh->x[iv*3+1];
    elements[i*3+2] = mesh->x[iv*3+2];
  }
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
#endif
  mt->elements = elements;
  double bbmin[DIMENSION], bbmax[DIMENSION];
  int N = DIMENSION + 1;
  for (int i = 0; i < DIMENSION; ++i) {
    bbmin[i] = elements[i];
    bbmax[i] = elements[i];
  }
  for (int i = 0; i < N * nElements; ++i) {
    for (int d = 0; d < DIMENSION; ++d) {
      bbmin[d] = fmin(bbmin[d], elements[DIMENSION * i + d]);
      bbmax[d] = fmax(bbmax[d], elements[DIMENSION * i + d]);
    }
  }
  _bboxtol(bbmin, bbmax);
  Cell *tree = cellNew(bbmin, bbmax);
  double amin[DIMENSION], amax[DIMENSION];
  for (int i = 0; i < nElements; ++i) {
    double *el = elements + (DIMENSION * N) * i;
    for (int d = 0; d < DIMENSION; ++d) {
      amin[d] = el[d];
      amax[d] = el[d];
    }
    for (int v = 1; v < N; ++v) {//only simplices
      for (int d = 0; d < DIMENSION; ++d) {
        amin[d] = fmin(amin[d], el[v * DIMENSION + d]);
        amax[d] = fmax(amax[d], el[v * DIMENSION + d]);
      }
    }
    _bboxtol(amin, amax);
    cellAdd(tree, amin, amax, i, NULL);
  }
  mt->tree = tree;
  return mt;
}


void mesh_tree_free(MeshTree *mt) {
  cellFree(mt->tree);
  free(mt->elements);
  free(mt);
213
}