/* See {CoherentEnergy.h} */ #include #include #define _GNU_SOURCE #include #include #include #include #include #include CONST Zero == 0.0; Epsilon == 0.0000000001; TYPE bool_vec_t == ARRAY OF bool_t; double_vec_t == double_vec_t; REVEAL T == Public BRANDED OBJECT double K; /* The energy normalization factor */ ElemTableRec_t *top; /* The map elements. */ bool_vec_t vVar; /* TRUE if node is variable */ bool_vec_t wallRelevant; /* TRUE if wall is relevant */ prodet: REF double_vec_t; /* The product of the determinants of the two tetrahedra incident in each internal wall */ eDprodet: REF double_vec_t; /* (Work) Derivate of energy relative to the product of determinants. */ Wa,Wb,Wc,Wd : r4_t; /* 4D viewing matrix */ Data4Radius: double; /* Radius of nodes in R^{4} */ OVERRIDES init = Init; defTop = DefTop; defVar = DefVar; eval = Eval; name = Name; } SELF_Init(SELF_T *erg) { return erg; } /* END Init */ void SELF_DefTop(SELF_T *erg, ElemTableRec_t *top) { erg->K = 1.0/((double)top->wall.nel); erg->top = top; erg->vVar = bool_vec_new(top->node.nel); erg->wallRelevant = bool_vec_new(top->wall.nel); /* Allocate product determinant tables: */ erg->prodet = double_vec_new(top->wall.nel); erg->eDprodet = double_vec_new(top->wall.nel); /* Just in case the client forgets to call "defVar": */ for (i = 0; i < top->node.nel; i++){ erg->vVar.el[i] = FALSE; } for (i = 0; i < top->wall.nel; i++){ erg->wallRelevant.el[i] = FALSE; } /* Compute the 4D matrix of viewing */ CalcV4Matrix(erg); erg->Data4Radius = 1.43215593075614510; } /* END DefTop */ void SELF_DefVar(SELF_T *erg, bool_vec_t *variable) { /* Decide which triangular walls are relevant to "Coherent" energy. A wall is relevant iff it exists, has at least one variable corner and is an internal wall (i.e. has exactly two tetrahedra incident to it) and least one tetrahedra is an existing tetrahedron. */ int NV = erg->top->node.nel; int NF = erg->top->wall.nel; bool_vec_t vVar = erg->vVar^; ??? wall = erg->top->wall^; ??? wallRelevant = erg->wallRelevant^; { assert((variable.nel) == NV); vVar = variable; /* Find the relevant walls: */ for (i = 0; i < NP; i++){ polyRelevant[i] = FALSE; } for (i = 0; i < NP; i++) { ??? p = cell[i]; ??? r = Srot(cell[i]); Place_t a = Tors(r); Place_t r1 = Clock(PrevE(r)); Place_t a1 = Tors(r1); Node_t u = OrgV(a); Node_t v = OrgV(NextE(a)); Node_t w = OrgV(PrevE(a)); Node_t x = OrgV(PrevE(a1)); ??? vvar = vVar.el[u->num]) || (vVar.el[v->num]) || (vVar.el[w->num]) || (vVar.el[x->num]; Wall_t f1 = PWall(a); Wall_t f2 = PWall(PrevF(a)); Wall_t f3 = PWall(PrevF(NextE(a))); Wall_t f4 = PWall(PrevF(PrevE(a))); { assert(f1 != f2) && (f2 != f3) && (f3 != f4); if ((p->exists) && (vvar)) { assert(u->exists) && (v->exists) && (w->exists) && (x->exists); polyRelevant[i] = TRUE; } } } } } /* END DefVar */ void CalcV4Matrix(erg: T) /* This procedure computes the four basis vectors for the 4D viewing matrix, Wa,Wb,Wc, and Wd. Note that the Up vector transforms to Wb, the Over vector transforms to Wc, and the line of sight transforms to Wd. The Wa vector is then computed from Wb,Wc and Wd. */ VAR double norm; { ??? From4 = erg->From4; ??? To4 = erg->To4; ??? Up4 = erg->Up4; ??? Over4 = erg->Over4; ??? Wa = erg->Wa; ??? Wb = erg->Wb; ??? Wc = erg->Wc; ??? Wd = erg->Wd; { /* Calculate Wd, the 4th coordinate basis vector and line-of-sight. */ Wd = r4_Sub(To4,From4); norm = r4_Norm(Wd); if (norm < Epsilon) { fprintf(stderr,"4D To Point and From Point are the same\n"); Process.ExitŠ(1); } Wd = r4_Scale(1.0/norm, Wd); /* Calculate Wa, the X-axis basis vector. */ Wa = r4_cross(Up4,Over4,Wd); norm = r4_Norm(Wa); if (norm < Epsilon) { fprintf(stderr, "4D up, over and view vectors are not perpendicular\n"); Process.ExitŠ(1); } Wa = r4_Scale(1.0/norm, Wa); /* Calculate Wb, the perpendicularized Up vector. */ Wb = r4_cross(Over4,Wd,Wa); norm = r4_Norm(Wb); if (norm < Epsilon) { fprintf(stderr,"Invalid 4D over vector\n"); Process.ExitŠ(1); } Wb = r4_Scale(1.0/norm, Wb); /* Calculate Wc, the perpendicularized Over vector. Note that the resulting vector is already normalized, since Wa, Wb and Wd are all unit vectors. */ Wc = r4_cross(Wd,Wa,Wb); } } CalcV4Matrix; void SELF_Eval( SELF_T erg; Coords_t *c; double *e; bool_t grad; Gradient *eDc; ) { int NV = erg->top->node.nel; ??? NP = erg->top->cell.nel; ??? cell = erg->top->cell^; bool_vec_t vVar = erg->vVar^; ??? polyRelevant = erg->polyRelevant^; ??? det = erg->det^; ??? eDdet = erg->eDdet^; ??? K = erg->K; ??? Slope = Math.pow(10.0, 2.0); ??? Data4Radius = erg->Data4Radius; ??? Wa = erg->Wa; ??? Wb = erg->Wb; ??? Wc = erg->Wc; { r3_t ProjectTo3D(num: uint) r3_t c3 ; { ??? TempV = r4_Sub(c[num], erg->From4); double rtemp = 1.0 / Data4Radius; { c3[0] = rtemp * r4_dot(TempV, erg->Wa); c3[1] = rtemp * r4_dot(TempV, erg->Wb); c3[2] = rtemp * r4_dot(TempV, erg->Wc); return c3; } } ProjectTo3D; void Accum_det(*U, V, W, X: uint; VAR dto: double) /* Compute the determinat of the tetrahedron "u v w x" in R^{3}. */ { ??? pu = ProjectTo3D(U); ??? pv = ProjectTo3D(V); ??? pw = ProjectTo3D(W); ??? px = ProjectTo3D(X); r4_t a == (r4_t){pu[0], pu[1], pu[2], 1.0}; r4_t b == (r4_t){pv[0], pv[1], pv[2], 1.0}; r4_t c == (r4_t){pw[0], pw[1], pw[2], 1.0}; r4_t d == (r4_t){px[0], px[1], px[2], 1.0}; { dto = dto + r4_det(a,b,c,d); } } Accum_det; void Accum_e_from_det(det: double; double e; VAR VAR eDdet: double) /* Adds to "e" the energy term corresponding to a cell with determinant "det". Also, if "grad" is true, stores in "eDdet" the derivative of that term. */ { ??? d2 = det * det; ??? sq = sqrt(1.0+d2); ??? h = Slope * (sq-det); { e = e + K * h; if (grad) { eDdet = K * (Slope*(det/sq-1.0) + 2.0 * det); } else { eDdet = 0.0; } } } Accum_e_from_det; void Distribute_eDdet( uint *iu,iv,iw,ix; eDdet: double) /* Accumulates in "eDc" the gradient of "e" relative to the corners of the tetrahedron "iu iv iw ix", given the derivative "eDdet" of "e" relative to the tetrahedron's determinant "det". */ { double rtemp = 1.0 / Data4Radius; double wa0 = Wa[0], wa1 = Wa[1], wa2 = Wa[2], wa3 = Wa[3]; double wb0 = Wb[0], wb1 = Wb[1], wb2 = Wb[2], wb3 = Wb[3]; double wc0 = Wc[0], wc1 = Wc[1], wc2 = Wc[2], wc3 = Wc[3]; r3_t pu = ProjectTo3D(iu); r3_t pv = ProjectTo3D(iv); r3_t pw = ProjectTo3D(iw); r3_t px = ProjectTo3D(ix); double pu0 = pu[0], pu1 = pu[1], pu2 = pu[2]; double pv0 = pv[0], pv1 = pv[1], pv2 = pv[2]; double pw0 = pw[0], pw1 = pw[1], pw2 = pw[2]; double px0 = px[0], px1 = px[1], px2 = px[2]; double A0 = pw2 - px2; double A1 = pw1 - px1; double A2 = pw1 * px2 - px1 * pw2; double A = pv1 * A0 - pv2 * A1 + A2; double B0 = pw2 - px2; double B1 = pw0 - px0; double B2 = pw0 * px2 - px0 * pw2; double B = pv0 * B0 - pv2 * B1 + B2; double C0 = pw1 - px1; double C1 = pw0 - px0; double C2 = pw0 * px1 - px0 * pw1; double C = pv0 * C0 - pv1 * C1 + C2; double D0 = pw1 * px2 - px1 * pw2; double D1 = pw0 * px2 - px0 * pw2; double D2 = pw0 * px1 - px0 * pw1; double d1Du0 = A * rtemp * wa0; double d1Du1 = A * rtemp * wa1; double d1Du2 = A * rtemp * wa2; double d1Du3 = A * rtemp * wa3; double d2Du0 = B * rtemp * wb0; double d2Du1 = B * rtemp * wb1; double d2Du2 = B * rtemp * wb2; double d2Du3 = B * rtemp * wb3; double d3Du0 = C * rtemp * wc0; double d3Du1 = C * rtemp * wc1; double d3Du2 = C * rtemp * wc2; double d3Du3 = C * rtemp * wc3; double d4Du0 = Zero; double d4Du1 = Zero; double d4Du2 = Zero; double d4Du3 = Zero; double detDu0 = d1Du0 - d2Du0 + d3Du0 - d4Du0; double detDu1 = d1Du1 - d2Du1 + d3Du1 - d4Du1; double detDu2 = d1Du2 - d2Du2 + d3Du2 - d4Du2; double detDu3 = d1Du3 - d2Du3 + d3Du3 - d4Du3; double eDu0 = eDdet * detDu0; double eDu1 = eDdet * detDu1; double eDu2 = eDdet * detDu2; double eDu3 = eDdet * detDu3; double d1Dv0 = pu0 * rtemp * ( A0 * wb0 - A1 * wc0); double d1Dv1 = pu0 * rtemp * ( A0 * wb1 - A1 * wc1); double d1Dv2 = pu0 * rtemp * ( A0 * wb2 - A1 * wc2); double d1Dv3 = pu0 * rtemp * ( A0 * wb3 - A1 * wc3); double d2Dv0 = pu1 * rtemp * ( B0 * wa0 - B1 * wc0); double d2Dv1 = pu1 * rtemp * ( B0 * wa1 - B1 * wc1); double d2Dv2 = pu1 * rtemp * ( B0 * wa2 - B1 * wc2); double d2Dv3 = pu1 * rtemp * ( B0 * wa3 - B1 * wc3); double d3Dv0 = pu2 * rtemp * ( C0 * wa0 - C1 * wb0); double d3Dv1 = pu2 * rtemp * ( C0 * wa1 - C1 * wb1); double d3Dv2 = pu2 * rtemp * ( C0 * wa2 - C1 * wb2); double d3Dv3 = pu2 * rtemp * ( C0 * wa3 - C1 * wb3); double d4Dv0 = rtemp * ( D0 * wa0 - D1 * wb0 + D2 * wc0); double d4Dv1 = rtemp * ( D0 * wa1 - D1 * wb1 + D2 * wc1); double d4Dv2 = rtemp * ( D0 * wa2 - D1 * wb2 + D2 * wc2); double d4Dv3 = rtemp * ( D0 * wa3 - D1 * wb3 + D2 * wc3); double detDv0 = d1Dv0 - d2Dv0 + d3Dv0 - d4Dv0; double detDv1 = d1Dv1 - d2Dv1 + d3Dv1 - d4Dv1; double detDv2 = d1Dv2 - d2Dv2 + d3Dv2 - d4Dv2; double detDv3 = d1Dv3 - d2Dv3 + d3Dv3 - d4Dv3; double eDv0 = eDdet * detDv0; double eDv1 = eDdet * detDv1; double eDv2 = eDdet * detDv2; double eDv3 = eDdet * detDv3; double d1Dw0 = pu0 * rtemp * ( wc0 * (pv1-px1) + wb0 * (px2-pv2)); double d1Dw1 = pu0 * rtemp * ( wc1 * (pv1-px1) + wb1 * (px2-pv2)); double d1Dw2 = pu0 * rtemp * ( wc2 * (pv1-px1) + wb2 * (px2-pv2)); double d1Dw3 = pu0 * rtemp * ( wc3 * (pv1-px1) + wb3 * (px2-pv2)); double d2Dw0 = pu1 * rtemp * ( wc0 * (pv0-px0) + wa0 * (px2-pv2)); double d2Dw1 = pu1 * rtemp * ( wc1 * (pv0-px0) + wa1 * (px2-pv2)); double d2Dw2 = pu1 * rtemp * ( wc2 * (pv0-px0) + wa2 * (px2-pv2)); double d2Dw3 = pu1 * rtemp * ( wc3 * (pv0-px0) + wa3 * (px2-pv2)); double d3Dw0 = pu2 * rtemp * ( wb0 * (pv0-px0) + wa0 * (px1-pv1)); double d3Dw1 = pu2 * rtemp * ( wb1 * (pv0-px0) + wa1 * (px1-pv1)); double d3Dw2 = pu2 * rtemp * ( wb2 * (pv0-px0) + wa2 * (px1-pv1)); double d3Dw3 = pu2 * rtemp * ( wb3 * (pv0-px0) + wa3 * (px1-pv1)); double d4Dw0 = rtemp * ( pv0 * (px2*wb0-px1*wc0) + pv1 * (px0*wc0-px2*wa0) + pv2 * (px1*wa0-px0*wb0)); double d4Dw1 = rtemp * ( pv0 * (px2*wb1-px1*wc1) + pv1 * (px0*wc1-px2*wa1) + pv2 * (px1*wa1-px0*wb1)); double d4Dw2 = rtemp * ( pv0 * (px2*wb2-px1*wc2) + pv1 * (px0*wc2-px2*wa2) + pv2 * (px1*wa2-px0*wb2)); double d4Dw3 = rtemp * ( pv0 * (px2*wb3-px1*wc3) + pv1 * (px0*wc3-px2*wa3) + pv2 * (px1*wa3-px0*wb3)); double detDw0 = d1Dw0 - d2Dw0 + d3Dw0 - d4Dw0; double detDw1 = d1Dw1 - d2Dw1 + d3Dw1 - d4Dw1; double detDw2 = d1Dw2 - d2Dw2 + d3Dw2 - d4Dw2; double detDw3 = d1Dw3 - d2Dw3 + d3Dw3 - d4Dw3; double eDw0 = eDdet * detDw0; double eDw1 = eDdet * detDw1; double eDw2 = eDdet * detDw2; double eDw3 = eDdet * detDw3; double d1Dx0 = pu0 * rtemp * ( wc0 * (pw1-pv1) + wb0 * (pv2-pw2)); double d1Dx1 = pu0 * rtemp * ( wc1 * (pw1-pv1) + wb1 * (pv2-pw2)); double d1Dx2 = pu0 * rtemp * ( wc2 * (pw1-pv1) + wb2 * (pv2-pw2)); double d1Dx3 = pu0 * rtemp * ( wc3 * (pw1-pv1) + wb3 * (pv2-pw2)); double d2Dx0 = pu1 * rtemp * ( wc0 * (pw0-pv0) + wa0 * (pv2-pw2)); double d2Dx1 = pu1 * rtemp * ( wc1 * (pw0-pv0) + wa1 * (pv2-pw2)); double d2Dx2 = pu1 * rtemp * ( wc2 * (pw0-pv0) + wa2 * (pv2-pw2)); double d2Dx3 = pu1 * rtemp * ( wc3 * (pw0-pv0) + wa3 * (pv2-pw2)); double d3Dx0 = pu2 * rtemp * ( wb0 * (pw0-pv0) + wa0 * (pv1-pw1)); double d3Dx1 = pu2 * rtemp * ( wb1 * (pw0-pv0) + wa1 * (pv1-pw1)); double d3Dx2 = pu2 * rtemp * ( wb2 * (pw0-pv0) + wa2 * (pv1-pw1)); double d3Dx3 = pu2 * rtemp * ( wb3 * (pw0-pv0) + wa3 * (pv1-pw1)); double d4Dx0 = rtemp * ( pv0 * (pw1*wc0-pw2*wb0) + pv1 * (pw2*wa0-pw0*wc0) + pv2 * (pw0*wb0-pw1*wa0)); double d4Dx1 = rtemp * ( pv0 * (pw1*wc1-pw2*wb1) + pv1 * (pw2*wa1-pw0*wc1) + pv2 * (pw0*wb1-pw1*wa1)); double d4Dx2 = rtemp * ( pv0 * (pw1*wc2-pw2*wb2) + pv1 * (pw2*wa2-pw0*wc2) + pv2 * (pw0*wb2-pw1*wa2)); double d4Dx3 = rtemp * ( pv0 * (pw1*wc3-pw2*wb3) + pv1 * (pw2*wa3-pw0*wc3) + pv2 * (pw0*wb3-pw1*wa3)); double detDx0 = d1Dx0 - d2Dx0 + d3Dx0 - d4Dx0; double detDx1 = d1Dx1 - d2Dx1 + d3Dx1 - d4Dx1; double detDx2 = d1Dx2 - d2Dx2 + d3Dx2 - d4Dx2; double detDx3 = d1Dx3 - d2Dx3 + d3Dx3 - d4Dx3; double eDx0 = eDdet * detDx0; double eDx1 = eDdet * detDx1; double eDx2 = eDdet * detDx2; double eDx3 = eDdet * detDx3; { if (vVar.el[iu]) { eDc[iu,0] = eDc[iu,0] + eDu0; eDc[iu,1] = eDc[iu,1] + eDu1; eDc[iu,2] = eDc[iu,2] + eDu2; eDc[iu,3] = eDc[iu,3] + eDu3; } if (vVar.el[iv]) { eDc[iv,0] = eDc[iv,0] + eDv0; eDc[iv,1] = eDc[iv,1] + eDv1; eDc[iv,2] = eDc[iv,2] + eDv2; eDc[iv,3] = eDc[iv,3] + eDv3; } if (vVar.el[iw]) { eDc[iw,0] = eDc[iw,0] + eDw0; eDc[iw,1] = eDc[iw,1] + eDw1; eDc[iw,2] = eDc[iw,2] + eDw2; eDc[iw,3] = eDc[iw,3] + eDw3; } if (vVar.el[ix]) { eDc[ix,0] = eDc[ix,0] + eDx0; eDc[ix,1] = eDc[ix,1] + eDx1; eDc[ix,2] = eDc[ix,2] + eDx2; eDc[ix,3] = eDc[ix,3] + eDx3; } } } Distribute_eDdet; { /* Clear determinant accumulators: */ for (j = 0; j < NP; j++){ det[j] = 0.0; } /* Enumerate cells and accumulate cell determinants: */ for (j = 0; j < NP; j++) { if (polyRelevant[j]) { ??? t = Srot(cell[j]); Place_t a = Tors(t); Place_t t1 = Clock(PrevE(t)); Place_t a1 = Tors(t1); Node_t un = OrgV(a)->num; Node_t vn = OrgV(NextE(a))->num; Node_t wn = OrgV(PrevE(a))->num; Node_t xn = OrgV(PrevE(a1))->num; { Accum_det(un, vn, wn, xn, det[j]) } } } /* Compute energy "e" from cells determinants, and the gradient "eDdet": */ e = 0.0; for (p = 0; p < NP; p++) { if (polyRelevant[p]) { Accum_e_from_det(det[p], e, eDdet[p]) } else { eDdet[p] = 0.0 } } /* Now distribute "eDdet" over "eDc": */ for (i = 0; i < NV; i++){ eDc[i] = (r4_t){0.0, ..}; } if (grad) { for (j = 0; j < NP; j++) { if (polyRelevant[j]) { ??? t = Srot(cell[j]); Place_t a = Tors(t); Place_t t1 = Clock(PrevE(t)); Place_t a1 = Tors(t1); Node_t un = OrgV(a)->num; Node_t vn = OrgV(NextE(a))->num; Node_t wn = OrgV(PrevE(a))->num; Node_t xn = OrgV(PrevE(a1))->num; { Distribute_eDdet(un, vn, wn, xn, eDdet[j]) } } } } } } } /* END Eval */ PROCEDURE Name(/* UNUSED */ erg: T): char *== { return "Coherent()" } /* END Name */ { } CoherentEnergy.