I have recently been trying to get myself a ray tracer to calculate a given position's longtitude and latitude values. My sphere is at the origin {0, 0, 0} and my camera always points to the origin.
#include <array>
#include <vector>
#include <iostream>
#include "glm/glm.hpp"
#include "glm/gtc/matrix_transform.hpp"
std::array<double, 3> ecef_to_geo(std::array<double, 3> ecef) {
std::array<double, 3> geo{ 0, 0, 0 }; //Results go here (Lat, Lon, Altitude)
double a = 6378137.0; //WGS-84 semi-major axis
double e2 = 6.6943799901377997e-3; //WGS-84 first eccentricity squared
double a1 = 4.2697672707157535e+4; //a1 = a*e2
double a2 = 1.8230912546075455e+9; //a2 = a1*a1
double a3 = 1.4291722289812413e+2; //a3 = a1*e2/2
double a4 = 4.5577281365188637e+9; //a4 = 2.5*a2
double a5 = 4.2840589930055659e+4; //a5 = a1+a3
double a6 = 9.9330562000986220e-1; //a6 = 1-e2
double zp, w2, w, r2, r, s2, c2, s, c, ss;
double g, rg, rf, u, v, m, f, p, x, y, z;
double n, lat, lon, alt;
x = ecef[0];
y = ecef[1];
z = ecef[2];
zp = std::abs(z);
w2 = x * x + y * y;
w = std::sqrt(w2);
r2 = w2 + z * z;
r = std::sqrt(r2);
geo[1] = std::atan2(y, x); //Lon (final)
s2 = z * z / r2;
c2 = w2 / r2;
u = a2 / r;
v = a3 - a4 / r;
if (c2 > 0.3) {
s = (zp / r) * (1.0 + c2 * (a1 + u + s2 * v) / r);
geo[0] = std::asin(s); //Lat
ss = s * s;
c = std::sqrt(1.0 - ss);
}
else {
c = (w / r) * (1.0 - s2 * (a5 - u - c2 * v) / r);
geo[0] = std::acos(c); //Lat
ss = 1.0 - c * c;
s = std::sqrt(ss);
}
g = 1.0 - e2 * ss;
rg = a / std::sqrt(g);
rf = a6 * rg;
u = w - rg * c;
v = zp - rf * s;
f = c * u + s * v;
m = c * v - s * u;
p = m / (rf / g + f);
geo[0] = geo[0] + p; //Lat
geo[2] = f + m * p / 2.0; //Altitude
if (z < 0.0) {
geo[0] *= -1.0; //Lat
}
geo[0] = geo[0] * 180 / glm::pi<double>();
geo[1] = geo[1] * 180 / glm::pi<double>();
return(geo); //Return Lat, Lon, Altitude in that order
}
bool solve_quadratic(float a, float b, float c, float& t0, float& t1) {
float discriminant = (b * b) - (4 * a * c);
if (discriminant < 0) //no solution
return false;
if (discriminant == 0) { //1 solution
t0 = -b / 2.0f * a;
}
else if (discriminant > 0) {//2 solutions;
auto droot = std::sqrt(discriminant);
t0 = (-b - droot) / (2.0f * a);
t1 = (-b + droot) / (2.0f * a);
}
return true;
}
void sphere_intersection(glm::vec3 ray_origin, glm::vec3 ray_direction) {
double r = 6378137.0;
float a = glm::dot(ray_direction, ray_direction);
float b = glm::dot(ray_origin, ray_direction) * 2.0f;
float c = glm::dot(ray_origin, ray_origin) - (r * r);
float t0{ 0 }, t1{ 0 };
if (!solve_quadratic(a, b, c, t0, t1))
return;
glm::vec3 hit1 = ray_origin * ray_direction * t0;
glm::vec3 hit2 = ray_origin * ray_direction * t1;
auto r1 = ecef_to_geo({ hit1.x, hit1.y, hit1.z });
auto r2 = ecef_to_geo({ hit2.x, hit2.y, hit2.z });
std::cout << "------results for ray_direction(" << ray_direction.x << ", " << ray_direction.y << ", " << ray_direction.z << ") ---------- " << std::endl;
std::cout << "t0 -> " << t0 << " | hit1 -> (" << r1[0] << ", " << r1[1] << ")" << std::endl;
std::cout << "t1 -> " << t1 << " | hit2 -> (" << r2[0] << ", " << r2[1] << ")" << std::endl;
}
int main() {
//-x=anti meridian
//+x=meridian
//+y=india
//-y=panama
//-z=south pole
//+z=north pole
std::array<double, 3> v{0, 1000000, -1000000};
for (size_t i = 0; i < 3; i++)
{
for (size_t x = 0; x < 3; x++)
{
for (size_t t = 0; t < 3; t++)
{
glm::vec3 cp{v[i], v[x], v[t]};
sphere_intersection(cp, glm::normalize(cp));
auto result1_e = ecef_to_geo(std::array<double, 3>{cp.x, cp.y, cp.z});
std::cout << "ecef_to_geo -> (" << result1_e[0] << ", " << result1_e[1] << ")" << std::endl << std::endl;
}
}
}
}
As we can see in the output window; for all zero/positive x values t1 looks like the correct solution and for negative x values t0 looks like the correct solution(sometimes with a wrong sign). We can confirm that because ecef_to_geo function always yields the correct lat/lon values(slight differences arises from sphere/spheroid difference). I always expect positive t value to be the solution. My mistake might be lying in the ray direction since it is same as ray origin but I can't say for sure.
