private void ts_random_init(ts_randomizer_t d, ulong jsrseed)
{
const double m1 = 2147483648.0;
double dn = 3.442619855899;
double tn = dn;
double vn = 9.91256303526217e-3;
double q;
int i;
d.jsr = jsrseed;
// Set up tables for Normal
q = vn / Math.Exp(-0.5 * dn * dn);
d.kn[0] = (ulong)((dn / q) * m1);
d.kn[1] = 0;
d.wn[0] = q / m1;
d.wnt[0] = q;
d.wn[127] = dn / m1;
d.wnt[127] = dn;
d.fn[0] = 1.0;
d.fn[127] = Math.Exp(-0.5 * dn * dn);
for (i = 126; i >= 1; i--)
{
dn = Math.Sqrt(-2.0 * Math.Log(vn / dn + Math.Exp(-0.5 * dn * dn), Math.E));
d.kn[i + 1] = (ulong)((dn / tn) * m1);
tn = dn;
d.fn[i] = Math.Exp(-0.5 * dn * dn);
d.wn[i] = dn / m1;
d.wnt[i] = dn;
}
}
public double ts_random_normal_fix()
{
ts_randomizer_t d = random;
const double r = 3.442620; // The starting of the right tail
double x, y;
for (; ;)
{
x = d.hz * d.wn[d.iz];
if (d.iz == 0)
{ // iz==0, handle the base strip
do
{
x = -Math.Log(UNI(d), Math.E) * 0.2904764;
// .2904764 is 1/r
y = -Math.Log(UNI(d), Math.E);
} while (y + y < x * x);
return((d.hz > 0) ? r + x : -r - x);
}
// iz>0, handle the wedges of other strips
if (d.fn[d.iz] + UNI(d) * (d.fn[d.iz - 1] - d.fn[d.iz]) < Math.Exp(-.5 * x * x))
{
return(x);
}
// Start all over
d.hz = (long)SHR3(d);
d.iz = (ulong)d.hz & 127;
if ((ulong)Math.Abs(d.hz) < d.kn[d.iz])
{
return(d.hz * d.wn[d.iz]);
}
}
}
public SLAMAlgorithm()
{
montecarlo_position = new ts_position_t();
random = new ts_randomizer_t();
this.ts_random_init(random, 0xdead);
this.ts_map_init();
}
private void ts_random_init(ts_randomizer_t d, ulong jsrseed)
{
const double m1 = 2147483648.0;
double dn = 3.442619855899;
double tn = dn;
double vn = 9.91256303526217e-3;
double q;
int i;
d.jsr = jsrseed;
// Set up tables for Normal
q = vn / Math.Exp(-0.5 * dn * dn);
d.kn[0] = (ulong)((dn / q) * m1);
d.kn[1] = 0;
d.wn[0] = q / m1;
d.wnt[0] = q;
d.wn[127] = dn / m1;
d.wnt[127] = dn;
d.fn[0] = 1.0;
d.fn[127] = Math.Exp(-0.5 * dn * dn);
for (i = 126; i >= 1; i--)
{
dn = Math.Sqrt(-2.0 * Math.Log(vn / dn + Math.Exp(-0.5 * dn * dn), Math.E));
d.kn[i + 1] = (ulong)((dn / tn) * m1);
tn = dn;
d.fn[i] = Math.Exp(-0.5 * dn * dn);
d.wn[i] = dn / m1;
d.wnt[i] = dn;
}
}
public ulong SHR3(ts_randomizer_t d)
{
d.jz = d.jsr;
d.jsr ^= (d.jsr << 13);
d.jsr ^= (d.jsr >> 17);
d.jsr ^= (d.jsr << 5);
return(d.jz + d.jsr);
}
public ulong SHR3(ts_randomizer_t d)
{
d.jz = d.jsr;
d.jsr ^= (d.jsr << 13);
d.jsr ^= (d.jsr >> 17);
d.jsr ^= (d.jsr << 5);
return d.jz + d.jsr;
}
public double UNI(ts_randomizer_t d)
{
return(.5 + SHR3(d) * .2328306e-9);
}
public double UNI(ts_randomizer_t d)
{
return .5 + SHR3(d) * .2328306e-9;
}
public SLAMAlgorithm()
{
montecarlo_position = new ts_position_t();
random = new ts_randomizer_t();
this.ts_random_init(random, 0xdead);
this.ts_map_init();
}
