// Validate THREE approaches to the angled-crease sawtooth, measured by the RIGHT
// metric: deviation from the true CREASE LINE (not surface-distance — a sawtooth
// is on-surface but off-crease, which is why the earlier surf-dev oracle missed it).
//
// Geometry: a concave corner — floor (material 1) meets wall (material 2) at a right
// angle, the whole thing ROTATED about Y (off-grid → the hard case). The concave
// crease is the intersection of floor-top and wall-inner = the Y axis (invariant
// under Y-rotation), so crease-deviation = sqrt(vx^2 + vz^2).
//
// Modes compared (all on the SAME crease cells):
// A plane-set snap (current deployed): capture plane-set from brush geometry +
// hidden-face removal + snap + escape-fallback + clamp.
// B exact-Hermite QEF (Cepero "let QEF do it"): plain QEF over the cell's edge
// crossings using EXACT active-face normals. No capture, no snap.
// B2 material-aware QEF: like B, but group edge crossings by MATERIAL; if the cell
// spans 2 materials, solve the crease as the intersection of the two material
// groups' averaged planes (sharp boundary between materials — Cepero).
// C adaptive resolution: mode B at 1x / 2x / 4x voxel resolution.
#include <cstdio>
#include <cmath>
#include <vector>
#include <algorithm>
struct V3{float x,y,z;};
static V3 operator-(V3 a,V3 b){return{a.x-b.x,a.y-b.y,a.z-b.z};}
static V3 operator+(V3 a,V3 b){return{a.x+b.x,a.y+b.y,a.z+b.z};}
static V3 operator*(V3 a,float s){return{a.x*s,a.y*s,a.z*s};}
static float dot(V3 a,V3 b){return a.x*b.x+a.y*b.y+a.z*b.z;}
static V3 cross3(V3 a,V3 b){return{a.y*b.z-a.z*b.y,a.z*b.x-a.x*b.z,a.x*b.y-a.y*b.x};}
static V3 rotY(V3 v,float a){float c=cosf(a),s=sinf(a);return{c*v.x+s*v.z,v.y,-s*v.x+c*v.z};}
struct Plane{V3 n;float d;};
struct Brush{std::vector<Plane> pl;int mat;};
static float bsdf(V3 p,const Brush&b){float m=-1e30f;for(auto&q:b.pl)m=fmaxf(m,dot(q.n,p)-q.d);return m;}
static float usdf(V3 p,const std::vector<Brush>&B){float m=1e30f;for(auto&b:B)m=fminf(m,bsdf(p,b));return m;}
// rotated axis-aligned box brush (rotation about Y through origin). d preserved (orthonormal rot).
static Brush boxBrush(V3 lo,V3 hi,float a,int mat){
Brush b; b.mat=mat;
b.pl={ {rotY({1,0,0},a),hi.x}, {rotY({-1,0,0},a),-lo.x},
{{0,1,0},hi.y}, {{0,-1,0},-lo.y},
{rotY({0,0,1},a),hi.z}, {rotY({0,0,-1},a),-lo.z} };
return b;}
// exact active face at p over the union: the argmax (brush,plane). Returns normal+material.
static void activeFace(V3 p,const std::vector<Brush>&B,V3&n,int&mat){
// union boundary = the brush with min sdf; within it the argmax plane
float best=1e30f;int bi=0;for(size_t i=0;i<B.size();i++){float s=bsdf(p,B[i]);if(s<best){best=s;bi=(int)i;}}
float bp=-1e30f;n={0,0,1};for(auto&q:B[bi].pl){float dp=dot(q.n,p)-q.d;if(dp>bp){bp=dp;n=q.n;}}mat=B[bi].mat;}
// plain QEF over (pos,normal) with mass regularization, clamped to cell
static V3 qef(const std::vector<V3>&P,const std::vector<V3>&N,V3 cmin,V3 cmax,float eps){
float a[6]={0,0,0,0,0,0};V3 b{0,0,0},mass{0,0,0};int n=0;
for(size_t i=0;i<P.size();i++){V3 m=N[i];a[0]+=m.x*m.x;a[1]+=m.x*m.y;a[2]+=m.x*m.z;a[3]+=m.y*m.y;a[4]+=m.y*m.z;a[5]+=m.z*m.z;
float d=dot(m,P[i]);b=b+m*d;mass=mass+P[i];n++;}
if(!n)return mass;mass=mass*(1.0f/n);
float m00=a[0]+eps,m01=a[1],m02=a[2],m11=a[3]+eps,m12=a[4],m22=a[5]+eps;
V3 amp{a[0]*mass.x+a[1]*mass.y+a[2]*mass.z,a[1]*mass.x+a[3]*mass.y+a[4]*mass.z,a[2]*mass.x+a[4]*mass.y+a[5]*mass.z};
V3 rhs=b-amp;
float c00=m11*m22-m12*m12,c01=m02*m12-m01*m22,c02=m01*m12-m02*m11,c11=m00*m22-m02*m02,c12=m01*m02-m00*m12,c22=m00*m11-m01*m01;
float det=m00*c00+m01*c01+m02*c02;V3 r;
if(fabsf(det)>1e-12f){float iv=1.0f/det;r=mass+V3{(c00*rhs.x+c01*rhs.y+c02*rhs.z)*iv,(c01*rhs.x+c11*rhs.y+c12*rhs.z)*iv,(c02*rhs.x+c12*rhs.y+c22*rhs.z)*iv};}else r=mass;
r.x=fmaxf(cmin.x,fminf(cmax.x,r.x));r.y=fmaxf(cmin.y,fminf(cmax.y,r.y));r.z=fmaxf(cmin.z,fminf(cmax.z,r.z));return r;}
static std::vector<Brush> B;
static int edir[3][3]={{1,0,0},{0,1,0},{0,0,1}};
static int ce[12][4]={{0,0,0,0},{0,1,0,0},{0,0,1,0},{0,1,1,0},{0,0,0,1},{1,0,0,1},{0,0,1,1},{1,0,1,1},{0,0,0,2},{1,0,0,2},{0,1,0,2},{1,1,0,2}};
// mesh at voxel size vs, mode: 0=A planeset-snap 1=B exactHermiteQEF 2=B2 material-aware
static void run(const char* label,float vs,int mode){
V3 org{-3,-3,-3};int G=(int)ceilf(6.0f/vs)+1;
auto D=[&](int i,int j,int k){return usdf(org+V3{i*vs,j*vs,k*vs},B);};
std::vector<float> dev;
for(int k=0;k<G-1;k++)for(int j=0;j<G-1;j++)for(int i=0;i<G-1;i++){
bool neg=false,pos=false;for(int a=0;a<2;a++)for(int b2=0;b2<2;b2++)for(int c=0;c<2;c++){float d=D(i+a,j+b2,k+c);if(d<0)neg=true;else pos=true;}
if(!(neg&&pos))continue;
V3 cmin=org+V3{i*vs,j*vs,k*vs},cmax=cmin+V3{vs,vs,vs},cc=cmin+V3{vs*.5f,vs*.5f,vs*.5f};
bool near_crease=(cc.x*cc.x+cc.z*cc.z)<(1.6f*vs)*(1.6f*vs); if(!near_crease)continue;
// gather edge crossings: pos + exact active-face normal + material
std::vector<V3>P,N;std::vector<int>M;
for(int e=0;e<12;e++){int ci=i+ce[e][0],cj=j+ce[e][1],ck=k+ce[e][2],dr=ce[e][3];
float d0=D(ci,cj,ck),d1=D(ci+edir[dr][0],cj+edir[dr][1],ck+edir[dr][2]);if((d0<0)==(d1<0))continue;
V3 p0=org+V3{ci*vs,cj*vs,ck*vs},p1=p0+V3{edir[dr][0]*vs,edir[dr][1]*vs,edir[dr][2]*vs};
float t=d0/(d0-d1);V3 ps=p0+(p1-p0)*t;V3 nm;int mt;activeFace(ps,B,nm,mt);P.push_back(ps);N.push_back(nm);M.push_back(mt);}
if(P.empty())continue;
V3 v;
if(mode==1){ v=qef(P,N,cmin,cmax,0.01f); } // B: plain exact-Hermite QEF
else if(mode==2){ // B2: material-aware
// count distinct materials among edges
int m0=M[0];bool two=false;for(int m:M)if(m!=m0){two=true;break;}
if(!two){ v=qef(P,N,cmin,cmax,0.01f); }
else { // average a plane per material, solve the 2-plane crease
V3 n1{0,0,0},n2{0,0,0},p1c{0,0,0},p2c{0,0,0};int c1=0,c2=0;
for(size_t q=0;q<P.size();q++){ if(M[q]==m0){n1=n1+N[q];p1c=p1c+P[q];c1++;} else {n2=n2+N[q];p2c=p2c+P[q];c2++;} }
if(c1&&c2){ n1=n1*(1.0f/c1);float l1=sqrtf(dot(n1,n1));if(l1>1e-6f)n1=n1*(1/l1);p1c=p1c*(1.0f/c1);
n2=n2*(1.0f/c2);float l2=sqrtf(dot(n2,n2));if(l2>1e-6f)n2=n2*(1/l2);p2c=p2c*(1.0f/c2);
float d1=dot(n1,p1c),d2=dot(n2,p2c);
// project cell centre onto the crease line of the two material planes
V3 r=cc;float cco=dot(n1,n2),det=1-cco*cco;
if(det>1e-4f){float r0=d1-dot(n1,r),r1=d2-dot(n2,r);float t0=(r0-cco*r1)/det,t1=(r1-cco*r0)/det;r=r+n1*t0+n2*t1;}
r.x=fmaxf(cmin.x,fminf(cmax.x,r.x));r.y=fmaxf(cmin.y,fminf(cmax.y,r.y));r.z=fmaxf(cmin.z,fminf(cmax.z,r.z));v=r; }
else v=qef(P,N,cmin,cmax,0.01f);
}
}
else { // A: plane-set snap (capture from brush geometry + hidden-face + snap + fallback + clamp)
v=qef(P,N,cmin,cmax,0.01f);
V3 cl_n[3];float cl_d[3];int cl=0;float T=vs*1.2f;
for(size_t bi=0;bi<B.size();bi++)for(size_t pi=0;pi<B[bi].pl.size();pi++){Plane pl=B[bi].pl[pi];
float fd=dot(pl.n,cc)-pl.d;if(fabsf(fd)>=T)continue;bool ob=true;
for(size_t pj=0;pj<B[bi].pl.size();pj++){if(pj==pi)continue;if(dot(B[bi].pl[pj].n,cc)-B[bi].pl[pj].d>T){ob=false;break;}}
if(!ob)continue;V3 fpt=cc-pl.n*fd,qp=fpt+pl.n*(vs*.5f);bool bur=false;
for(size_t bk=0;bk<B.size();bk++){if(bk==bi)continue;if(bsdf(qp,B[bk])<0){bur=true;break;}}if(bur)continue;
int mm=-1;for(int q=0;q<cl;q++)if(fabsf(dot(pl.n,cl_n[q]))>0.999f){mm=q;break;}
if(mm<0&&cl<3){cl_n[cl]=pl.n;cl_d[cl]=pl.d;cl++;}}
V3 sn=v;
if(cl==1)sn=v-cl_n[0]*(dot(cl_n[0],v)-cl_d[0]);
else if(cl==2){float cco=dot(cl_n[0],cl_n[1]),det=1-cco*cco;if(det>1e-4f){float r0=cl_d[0]-dot(cl_n[0],v),r1=cl_d[1]-dot(cl_n[1],v);sn=v+cl_n[0]*((r0-cco*r1)/det)+cl_n[1]*((r1-cco*r0)/det);}}
else if(cl==3){V3 a=cl_n[0],b3=cl_n[1],cn=cl_n[2];float den=dot(a,cross3(b3,cn));if(fabsf(den)>1e-5f)sn=(cross3(b3,cn)*cl_d[0]+cross3(cn,a)*cl_d[1]+cross3(a,b3)*cl_d[2])*(1.0f/den);}
if(cl>=2){V3 lo=cmin-V3{vs*.5f,vs*.5f,vs*.5f},hi=cmax+V3{vs*.5f,vs*.5f,vs*.5f};
if(sn.x<lo.x||sn.y<lo.y||sn.z<lo.z||sn.x>hi.x||sn.y>hi.y||sn.z>hi.z){int nb=0;float ba=1e30f;for(int q=0;q<cl;q++){float aa=fabsf(dot(cl_n[q],cc)-cl_d[q]);if(aa<ba){ba=aa;nb=q;}}sn=v-cl_n[nb]*(dot(cl_n[nb],v)-cl_d[nb]);}}
sn.x=fmaxf(cmin.x,fminf(cmax.x,sn.x));sn.y=fmaxf(cmin.y,fminf(cmax.y,sn.y));sn.z=fmaxf(cmin.z,fminf(cmax.z,sn.z));
if(cl>=1)v=sn;
}
dev.push_back(sqrtf(v.x*v.x+v.z*v.z)); // distance to crease line (Y axis)
}
// diagnostic: how many crease cells actually SEE both materials in their edges?
if(mode==2){int both=0,tot=0;
for(int k=0;k<G-1;k++)for(int j=0;j<G-1;j++)for(int i=0;i<G-1;i++){
bool neg=false,pos=false;for(int a=0;a<2;a++)for(int b2=0;b2<2;b2++)for(int c=0;c<2;c++){float d=D(i+a,j+b2,k+c);if(d<0)neg=true;else pos=true;}
if(!(neg&&pos))continue;V3 cc=org+V3{(i+.5f)*vs,(j+.5f)*vs,(k+.5f)*vs};
if((cc.x*cc.x+cc.z*cc.z)>=(1.6f*vs)*(1.6f*vs))continue;
int m0=-1;bool two=false,any=false;
for(int e=0;e<12;e++){int ci=i+ce[e][0],cj=j+ce[e][1],ck=k+ce[e][2],dr=ce[e][3];
float d0=D(ci,cj,ck),d1=D(ci+edir[dr][0],cj+edir[dr][1],ck+edir[dr][2]);if((d0<0)==(d1<0))continue;
V3 p0=org+V3{ci*vs,cj*vs,ck*vs},p1=p0+V3{edir[dr][0]*vs,edir[dr][1]*vs,edir[dr][2]*vs};float t=d0/(d0-d1);V3 ps=p0+(p1-p0)*t;V3 nm;int mt;activeFace(ps,B,nm,mt);
any=true;if(m0<0)m0=mt;else if(mt!=m0)two=true;}
if(any){tot++;if(two)both++;}}
printf(" [material diag: %d/%d crease cells see BOTH materials in their edges]\n",both,tot);}
std::sort(dev.begin(),dev.end());
printf(" %-38s cells=%zu crease-dev p50=%.4f p95=%.4f MAX=%.4f (voxel=%.3f)\n",label,dev.size(),
dev.empty()?0:dev[dev.size()/2],dev.empty()?0:dev[(size_t)(dev.size()*.95)],dev.empty()?0:dev.back(),vs);
}
int main(){
float a=0.4f; // tilt off-grid (worst case)
B.push_back(boxBrush({-2,-2,-2},{2,2,0},a,1)); // floor, material 1, top face z=0
B.push_back(boxBrush({0,-2,0},{2,2,3},a,2)); // wall, material 2, inner face x=0
float vs=0.224f;
printf("Concave floor|wall crease, tilted %.2f rad (crease = Y axis). Lower dev = sharper.\n",a);
printf("== The three approaches ==\n");
run("A plane-set snap (current deployed)",vs,0);
run("B exact-Hermite QEF (let QEF do it)",vs,1);
run("B2 material-aware QEF",vs,2);
printf("== Option C: adaptive resolution (mode B at finer voxels) ==\n");
run("C exact-Hermite QEF @ 1x",vs,1);
run("C exact-Hermite QEF @ 2x",vs*0.5f,1);
run("C exact-Hermite QEF @ 4x",vs*0.25f,1);
return 0;
}