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); }