int index(Config config, Position i, Kernel k)
{
int rx = (int)(4 * config.rx + 1);
int ry = (int)(4 * config.ry + 1);
int ox = -(int)(1.5 * config.rx);
int oy = -(int)(1.5 * config.ry);
int x = (int)i.p.x - k.xi - ox;
int y = (int)i.p.y - k.yi - oy;
int w = (int)(4 * config.rx + 1);
int h = (int)(4 * config.ry + 1);
if (0 <= x && x < w && 0 <= y && y < h)
{
return(x + y * (int)(4 * config.rx + 1));
}
return(-1);
}
double calcGaussian(Kernel k, Position i)
{
var dpos = i.p - m[k.index];
var invS = S[k.index].Inverse();
var posTerm = -0.5 * dpos * invS * dpos;
var dcol = c[i.index] - v[k.index];
var colTerm = -Vec3.DistanceSqr(c[i.index], v[k.index]) / (2 * s[k.index] * s[k.index]);
try
{
double val = Math.Max(-1e2, Math.Min(1e2, posTerm + colTerm));
var result = Math.Exp(val);
System.Diagnostics.Debug.Assert(double.IsNaN(result) == false);
return(result);
}
catch (Exception ex)
{
return(0);
}
}
void cStep(Config config)
{
// Spatial constraints
var aveM = new Vec2[config.KernelSize];
For.AllKernels(config, (_, k) =>
{
aveM[k.index] = new Vec2(0, 0);
var neighbors = k.Neighbors4(config);
foreach (var n in neighbors)
{
aveM[k.index] += m[n.index];
}
aveM[k.index] /= neighbors.Count;
});
For.AllKernels(config, (_, k) =>
{
m[k.index] = 0.5 * (aveM[k.index] + m[k.index]);
double halfWidth = 0.25 * config.rx;
double halfHeight = 0.25 * config.ry;
m[k.index] = clampBox(m[k.index], k.xi - halfWidth, k.yi - halfHeight, 2 * halfWidth, 2 * halfHeight);
});
// Constrain spatial variance
For.AllKernels(config, (_, k) =>
{
Mat2x2 _U, _S, _Vt;
S[k.index].SVD(out _U, out _S, out _Vt);
_S.m11 = clamp(_S.m11, 0.05, 0.1);
_S.m22 = clamp(_S.m22, 0.05, 0.1);
var newS = _U * _S * _Vt;
if (double.IsNaN(newS.Inverse().m11) == false)
{
S[k.index] = newS;
}
});
// Shape constraints
For.AllKernels(config, (_, k) =>
{
var neighbors = k.Neighbors8(config);
foreach (var n in neighbors)
{
var d = new Vec2(n.xi - k.xi, n.yi - k.yi);
var sv = sumInRegion(config, k, (i) =>
{
double gki = g(k)[index(config, i, k)];
double dot = (i.p - m[k.index]) * d;
return(gki * Math.Max(0, dot));
});
var f = sumInRegion(config, k, (i) =>
{
try
{
double gki = g(k)[index(config, i, k)];
double gni = index(config, i, n) >= 0 ? g(k)[index(config, i, n)] : 0;
return(gki * gni);
}catch (Exception e)
{
return(0);
}
});
var o = sumInRegion(config, k, (i) =>
{
if (i.p.x >= config.wi)
{
return(new Vec2(0, 0));
}
if (i.p.y >= config.hi)
{
return(new Vec2(0, 0));
}
double gki = g(k)[index(config, i, k)];
double gni = index(config, i, n) >= 0 ? g(k)[index(config, i, n)] : 0;
var p10 = new Position(config, (int)i.p.x + 1, (int)i.p.y);
var p01 = new Position(config, (int)i.p.x, (int)i.p.y + 1);
double gki10 = g(k)[index(config, p10, k)];
double gki01 = g(k)[index(config, p01, k)];
double val00 = gki / (gki + gni);
double val10 = gki10 / (gki10 + gni);
double val01 = gki01 / (gki01 + gni);
return(new Vec2(val10 - val00, val01 - val00));
});
double cos25 = Math.Cos(Math.PI * 25 / 180.0);
if (sv > 0.2 * config.rx || (f < 0.08 && d.NormalSafe() * o.NormalSafe() < cos25))
{
s[k.index] *= 1.1;
s[n.index] *= 1.1;
}
}
});
}
