Vector3f cTrackball::ballVector(Vector2f screen) const
{
// normalize and centre the screen coordinates first
screen -= m_center;
screen /= m_radius;
float lsqared = screen.squaredNorm();
Vector3f ball;
// if we are grabbing outside the bounds of the virtual hemisphere,
// take a point on the edge
if (lsqared > 1.0)
{
screen.normalize();
ball = Vector3f(screen[0], screen[1], 0.0f);
}
// otherwise we are on the protruding hemisphere
else
{
float z = sqrtf(1.0f - lsqared);
ball = Vector3f(screen[0], screen[1], z);
}
// return the ball vector, taking into account the camera's orientation
return m_inverseCamera * ball;
}
Vector4f*
Atmosphere::computeInscatterTable() const
{
// Rg - "ground radius"
// Rt - "transparent radius", i.e. radius of the atmosphere at some point
// where it is visually undetectable.
float Rg = planetRadius;
float Rg2 = Rg * Rg;
float Rt = shellRadius();
float Rt2 = Rt * Rt;
// Avoid numerical precision problems by choosing a first viewer
// position just *above* the planet surface.
float baseHeight = Rg * 1.0e-6f;
unsigned int sampleCount = HeightSamples * ViewAngleSamples * SunAngleSamples;
Vector4f* inscatter = new Vector4f[sampleCount];
for (unsigned int i = 0; i < HeightSamples; ++i)
{
float w = float(i) / float(HeightSamples);
float h = w * w * (Rt - Rg) + baseHeight;
float r = Rg + h;
float r2 = r * r;
cout << "layer " << i << ", height=" << h << "km\n";
Vector2f eye(0.0f, r);
for (unsigned int j = 0; j < ViewAngleSamples; ++j)
{
float v = float(j) / float(ViewAngleSamples - 1);
float mu = max(-1.0f, min(1.0f, toMu(v)));
float cosTheta = mu;
float sinTheta2 = 1.0f - cosTheta * cosTheta;
float sinTheta = sqrt(sinTheta2);
Vector2f view(sinTheta, cosTheta);
float pathLength;
float d = Rg2 - r2 * sinTheta2;
if (d > 0.0f && -r * cosTheta - sqrt(d) > 0.0f)
{
// Ray hits the planet
pathLength = -r * cosTheta - sqrt(Rg2 - r2 * sinTheta2);
}
else
{
// Ray hits the sky
pathLength = -r * cosTheta + sqrt(Rt2 - r2 * sinTheta2);
}
for (unsigned int k = 0; k < SunAngleSamples; ++k)
{
float w = float(k) / float(SunAngleSamples - 1);
float muS = toMuS(w);
float cosPhi = muS;
float sinPhi = sqrt(max(0.0f, 1.0f - cosPhi * cosPhi));
Vector2f sun(sinPhi, cosPhi);
float stepLength = pathLength / float(ScatteringIntegrationSteps);
Vector2f step = view * stepLength;
Vector3f rayleigh = Vector3f::Zero();
float mie = 0.0f;
for (unsigned int m = 0; m < ScatteringIntegrationSteps; ++m)
{
Vector2f x = eye + step * m;
float distanceToViewer = stepLength * m;
float rx2 = x.squaredNorm();
float rx = sqrt(rx2);
// Compute the transmittance along the path to the viewer
Vector3f viewPathTransmittance = transmittance(r, mu, distanceToViewer, *this);
// Compute the cosine and sine of the angle between the
// sun direction and zenith at the current sample.
float c = x.dot(sun) / rx;
float s2 = 1.0f - c * c;
// Compute the transmittance along the path to the sun
// and the total transmittance t.
Vector3f t;
if (Rg2 - rx2 * s2 < 0.0f || -rx * c - sqrt(Rg2 - rx2 * s2) < 0.0f)
{
// Compute the distance through the atmosphere
// in the direction of the sun
float sunPathLength = -rx * c + sqrt(Rt2 - rx2 * s2);
Vector3f sunPathTransmittance = transmittance(rx, c, sunPathLength, *this);
t = viewPathTransmittance.cwise() * sunPathTransmittance;
}
else
{
// Ray to sun intersects the planet; no inscattered
// light at this point.
t = Vector3f::Zero();
}
// Accumulate Rayleigh and Mie scattering
float hx = rx - Rg;
rayleigh += (exp(-hx / rayleighScaleHeight) * stepLength) * t;
mie += exp(-hx / mieScaleHeight) * stepLength * t.x();
}
unsigned int index = (i * ViewAngleSamples + j) * SunAngleSamples + k;
inscatter[index] << rayleigh.cwise() * rayleighCoeff,
mie * mieCoeff;
if (i == HeightSamples - 1 && k == 0)
{
cout << acos(muS) * 180.0/M_PI << ", "
<< acos(mu) * 180.0/M_PI << ", "
<< inscatter[index].transpose() << endl;
}
#if 0
// Emit warnings about NaNs in scatter table
if (isNaN(rayleigh.x()))
{
cout << "NaN in inscatter table at (" << k << ", " << j << ", " << i << ")\n";
}
#endif
}
}
}
return inscatter;
}
float VectorLocalization2D::observationWeightLidar(vector2f loc, float angle, const LidarParams &lidarParams, const vector<Vector2f> &laserPoints) const
{
static const bool debug = false;
float numRays = lidarParams.numRays;
float minRange = lidarParams.minRange;
float maxRange = lidarParams.maxRange;
float logOutOfRangeProb = lidarParams.logOutOfRangeProb;
float logObstacleProb = lidarParams.logObstacleProb;
float logShortHitProb = lidarParams.logShortHitProb;
vector2f laserLoc = loc + vector2f(lidarParams.laserToBaseTrans.x(),lidarParams.laserToBaseTrans.y()).rotate(angle);
vector<line2f> lines;
vector<int> lineCorrespondences;
if(UseAnalyticRender){
lineCorrespondences = currentMap->getRayToLineCorrespondences(laserLoc, angle, lidarParams.angleResolution, numRays, 0.0, maxRange, true, &lines);
}else{
lineCorrespondences = currentMap->getRayToLineCorrespondences(laserLoc, angle, lidarParams.angleResolution, numRays, 0.0, maxRange, false, NULL);
}
float *scanRays = lidarParams.laserScan;
float curAngle;
Vector2f attraction;
float midScan = float(numRays)/2.0;
Matrix2f robotAngle;
robotAngle = Rotation2Df(angle);
Vector2f heading, scanPoint, laserLocE(V2COMP(laserLoc));
float logTotalWeight = 0.0;
Vector2f scanPoint2, scanDir, lineDir;
Matrix2f robotAngle2;
robotAngle2 = Rotation2Df(angle+lidarParams.angleResolution);
for(int i=0; i<numRays-1; i++){
if(scanRays[i]>maxRange || scanRays[i]<minRange){
logTotalWeight += logObstacleProb*lidarParams.correlationFactor;
continue;
}
if(lineCorrespondences[i]>=0){
scanPoint = laserLocE + robotAngle*laserPoints[i];
scanPoint2 = laserLocE + robotAngle2*laserPoints[i+1];
scanDir = (scanPoint2-scanPoint).normalized();
if(UseAnalyticRender){
lineDir = Vector2f(V2COMP(lines[lineCorrespondences[i]].Dir()));
}else{
lineDir = Vector2f(V2COMP(currentMap->lines[lineCorrespondences[i]].Dir()));
}
if(fabs(lineDir.dot(scanDir))>lidarParams.minCosAngleError){
if(UseAnalyticRender)
attraction = observationFunction(lines[lineCorrespondences[i]], scanPoint);
else
attraction = observationFunction(currentMap->lines[lineCorrespondences[i]], scanPoint);
logTotalWeight += -min(attraction.squaredNorm()/lidarParams.lidarStdDev, -logShortHitProb)*lidarParams.correlationFactor;
}else{
logTotalWeight += logObstacleProb*lidarParams.correlationFactor;
}
}else if(scanRays[i]<maxRange){
logTotalWeight += logObstacleProb*lidarParams.correlationFactor;
}else{
logTotalWeight += logOutOfRangeProb*lidarParams.correlationFactor;
}
}
//if(debug) printf("numCorrespondences: %d\n",numCorrespondences);
//exit(-1);
//return numCorrespondences;
return exp(logTotalWeight);
}
