distmesh/trisurfupd.cpp at master · bfroehle/distmesh · GitHub

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
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
// Copyright (C) 2004-2012 Per-Olof Persson. See COPYRIGHT.TXT for details.

#include "mex.h"
#include <algorithm>
#include <cmath>
#include <cstring>

template<class T> inline T sqr(T x) { return x*x; }
template<class T> inline T length(T *x) { return sqrt(sqr(x[0])+sqr(x[1])+sqr(x[2])); }
template<class T> inline T dot(T *x,T *y) { return x[0]*y[0]+x[1]*y[1]+x[2]*y[2]; }
template<class T> inline void cross(T *v1,T *v2,T *n) { n[0]=v1[1]*v2[2]-v1[2]*v2[1];
                                                        n[1]=v1[2]*v2[0]-v1[0]*v2[2];
                                                        n[2]=v1[0]*v2[1]-v1[1]*v2[0]; }
const char mod3x1[3]={1,2,0};
const char mod3x2[3]={2,0,1};

double triarea(double *p1,double *p2,double *p3)
{
  double d12x=p2[0]-p1[0];
  double d12y=p2[1]-p1[1];
  double d13x=p3[0]-p1[0];
  double d13y=p3[1]-p1[1];
  return (d12x*d13y-d12y*d13x)/2.0;
}

void trinormal3(double *p1,double *p2,double *p3,double *n)
{
  double d12[3]={ p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2] };
  double d13[3]={ p3[0]-p1[0], p3[1]-p1[1], p3[2]-p1[2] };
  cross(d12,d13,n);

  double nnorm=length(n);
  n[0]/=nnorm; n[1]/=nnorm; n[2]/=nnorm;
}

double triqual3(double *p1,double *p2,double *p3)
{
  double n[3];
  double d12[3]={ p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2] };
  double d13[3]={ p3[0]-p1[0], p3[1]-p1[1], p3[2]-p1[2] };
  double d23[3]={ p3[0]-p2[0], p3[1]-p2[1], p3[2]-p2[2] };
  cross(d12,d13,n);
  double vol=length(n)/2.0;
  double den=dot(d12,d12)+dot(d13,d13)+dot(d23,d23);
  return 6.928203230275509*vol/den;
}

void tupdate(double *p,int *t,int *t2t,char *t2n,
             int np,int nt)
{
  for (int t1=0; t1<nt; t1++)
    for (char n1=0; n1<3; n1++) {
      int t2=t2t[n1+3*t1];
      if (t2>=0) {
        double q1=triqual3(&p[3*t[0+3*t1]],&p[3*t[1+3*t1]],&p[3*t[2+3*t1]]);
        double q2=triqual3(&p[3*t[0+3*t2]],&p[3*t[1+3*t2]],&p[3*t[2+3*t2]]);
        double minqold=std::min(q1,q2);
        if (minqold<0.9) {
          char n2=t2n[n1+3*t1];
          char tix11=mod3x1[n1];
          char tix12=mod3x2[n1];
          char tix21=mod3x1[n2];
          char tix22=mod3x2[n2];

          int newt[2][3]={t[0+3*t1],t[1+3*t1],t[2+3*t1],
                          t[0+3*t2],t[1+3*t2],t[2+3*t2]};

          // Swap edge
          newt[0][tix12]=newt[1][n2];
          newt[1][tix22]=newt[0][n1];

          double q3=triqual3(&p[3*newt[0][0]],&p[3*newt[0][1]],&p[3*newt[0][2]]);
          double q4=triqual3(&p[3*newt[1][0]],&p[3*newt[1][1]],&p[3*newt[1][2]]);
          double minqnew=std::min(q3,q4);

          if (minqnew>minqold+.025) {
            bool flip;
            double normal1[3],normal2[3],normal3[3],normal4[3];
            trinormal3(&p[3*t[0+3*t1]],&p[3*t[1+3*t1]],&p[3*t[2+3*t1]],normal1);
            trinormal3(&p[3*t[0+3*t2]],&p[3*t[1+3*t2]],&p[3*t[2+3*t2]],normal2);
            trinormal3(&p[3*newt[0][0]],&p[3*newt[0][1]],&p[3*newt[0][2]],normal3);
            trinormal3(&p[3*newt[1][0]],&p[3*newt[1][1]],&p[3*newt[1][2]],normal4);
            flip=dot(normal1,normal2)>0 && dot(normal3,normal4)>0;
            if (flip) {
              int nbt;
              char nbn;

              // Insert new triangles
              memcpy(t+3*t1,newt[0],3*sizeof(int));
              memcpy(t+3*t2,newt[1],3*sizeof(int));

              // Update t2t and t2n
              nbt=t2t[tix21+3*t2];
              nbn=t2n[tix21+3*t2];
              t2t[n1+3*t1]=nbt;
              t2n[n1+3*t1]=nbn;
              if (nbt>=0) {
                t2t[nbn+3*nbt]=t1;
                t2n[nbn+3*nbt]=n1;
              }

              nbt=t2t[tix11+3*t1];
              nbn=t2n[tix11+3*t1];
              t2t[n2+3*t2]=nbt;
              t2n[n2+3*t2]=nbn;
              if (nbt>=0) {
                t2t[nbn+3*nbt]=t2;
                t2n[nbn+3*nbt]=n2;
              }

              t2t[tix11+3*t1]=t2;
              t2n[tix11+3*t1]=tix21;
              t2t[tix21+3*t2]=t1;
              t2n[tix21+3*t2]=tix11;
            }
          }
        }
      }
    }
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
  plhs[0]=mxDuplicateArray(prhs[0]);
  plhs[1]=mxDuplicateArray(prhs[1]);
  plhs[2]=mxDuplicateArray(prhs[2]);
  int *t=(int*)mxGetPr(plhs[0]);
  int *t2t=(int*)mxGetPr(plhs[1]);
  char *t2n=(char*)mxGetPr(plhs[2]);
  int nt=mxGetN(prhs[0]);

  double *p=mxGetPr(prhs[3]);
  int np=mxGetN(prhs[3]);

  tupdate(p,t,t2t,t2n,np,nt);
}