Stereographic projection

#include <n2DLib.h> // not important, that's just for display
#include <math.h>

typedef struct
{
    float x, y;
} Vec2;

typedef struct
{
    float x, y, z;
} Vec3;

Vec3 operator+(Vec3 a, Vec3 b)
{
    Vec3 r;
    r.x = a.x + b.x;
    r.y = a.y + b.y;
    r.z = a.z + b.z;
    return r;
}

Vec3 operator-(Vec3 a, Vec3 b)
{
    Vec3 r;
    r.x = a.x - b.x;
    r.y = a.y - b.y;
    r.z = a.z - b.z;
    return r;
}

Vec3 operator-(Vec3 v)
{
    Vec3 r;
    r.x = -v.x;
    r.y = -v.y;
    r.z = -v.z;
    return r;
}

Vec3 operator*(float t, Vec3 v)
{
    Vec3 r;
    r.x = t * v.x;
    r.y = t * v.y;
    r.z = t * v.z;
    return r;
}

Vec3 operator/(Vec3 v, float t)
{
    Vec3 r;
    r.x = v.x / t;
    r.y = v.y / t;
    r.z = v.z / t;
    return r;
}

float sqlen(Vec3 v)
{
    return v.x * v.x + v.y * v.y + v.z * v.z;
}

float dot(Vec3 a, Vec3 b)
{
    return a.x * b.x + a.y * b.y + a.z * b.z;
}

Vec3 rotate(Vec3 v, float angle)
{
    Vec3 r;
    r.x = v.x;
    r.y = v.y * cos(angle) + v.z * sin(angle);
    r.z = -v.y * sin(angle) + v.z * cos(angle);
    return r;
}

Vec3 projOnSphere(Vec3 v, Vec3 center, float r)
{
    // Direction vector of the line from v to the center of the sphere
    Vec3 u = center - v;
    u = u / sqrt(sqlen(u));

    // Calculate the coefficients for the 2nd degree polynomial on t
    float a = sqlen(u);
    float b = 2 * dot(u, v - center);
    float c = sqlen(v - center) - r * r;
    float d = b * b - 4 * a * c; // is always > 0
    // Actual t
    float t1 = (-b - sqrt(d)) / (2 * a), t2 = (-b + sqrt(d)) / (2 * a);
    // The point is always the closest intersection, even if t < 0
    return min(t1, t2) * u + v;
}

// v : point to project (must be on the sphere), center : of the sphere, r : radius, n : normal of the plane (must not be 0), p : a point of the plane
Vec3 stereographicProj(Vec3 v, Vec3 center, float r, Vec3 _n, Vec3 p)
{
    // Calculate f, the point of the sphere the farthest from the plane
    Vec3 n = dot(center - p, _n) > 0 ? _n : -_n;
    Vec3 f = r * n + center;
    // Direction vector of the line from f to v
    Vec3 u = v - f;
    u = u / sqrt(sqlen(u));

    // Constant of the plane's equation
    float c = dot(p, n);

    // Actual t
    if (dot(u, n) == 0) return v;
    float t = (c - dot(f, n)) / dot(u, n);
    return t * u + f;
}

Vec2 spaceToScreen(Vec3 v, Vec3 camera, float zoom)
{
    Vec3 tv = v - camera;
    Vec2 p;
    p.x = tv.x * zoom / tv.z + 160;
    p.y = tv.y * zoom / tv.z + 120;
    return p;
}

int main(int argc, char *argv[])
{
    Vec3 camera = { 0, 0, -100 };
    Vec3 cube[] = {
            { 30, 30, 30 },
            { 30, -30, 30 },
            { -30, -30, 30 },
            { -30, 30, 30 },
            { 30, 30, -30 },
            { 30, -30, -30 },
            { -30, -30, -30 },
            { -30, 30, -30 },
    };

    int lines[] = {
            0, 1, 1, 2, 2, 3, 3, 0,
            4, 5, 5, 6, 6, 7, 7, 4,
            0, 4, 1, 5, 2, 6, 3, 7
    };

    Vec3 center = { 0, 0, 0 };
    float r = 20;
    Vec3 u = { 0 };
    float zoom = 100;
    float theta = 0;
    float tx;

    initBuffering("Stereographical projection");

    clearBufferB();

    while (!G_keys[SDLK_ESCAPE] && !updateScreen())
    {
        Vec3 tx = { sin(theta * 3.141592 / 180) * 60, 0, 0 };
        constrainFrameRate(60);
        clearBufferB();

        // Draw the sphere first
        Vec3 test = { r, center.y, center.z };
        float pr = spaceToScreen(test, camera, zoom).x - 160;
        Vec2 ps = spaceToScreen(center, camera, zoom);
        fillCircle(ps.x, ps.y, pr, 0xffff);


        Vec2 pts[8];
        Vec3 ppts[8];
        // Draw the cube
        for (int i = 0; i < 8; i++)
        {
            Vec3 rp = rotate(cube[i], theta * 3.141592 / 180); // rotate by theta degrees around the X axis
            Vec2 sp = spaceToScreen(rp + tx, camera, zoom); // display the actual point
            pts[i] = sp;
            if (rp.z <= center.z || sq(sp.x - 160) + sq(sp.y - 120) >= sq(pr)) // stupid occlusion
                fillCircle(sp.x, sp.y, 1, 0x07e0);
            Vec3 spherePoint = projOnSphere(rp + tx, center, r); // project on a sphere
            Vec2 spp = spaceToScreen(spherePoint, camera, zoom); // display the projection
            if (spherePoint.z <= center.z)
                fillCircle(spp.x, spp.y, 1, 0xf800);
            drawLine(sp.x, sp.y, spp.x, spp.y, 0xf800);
            Vec3 planePoint = stereographicProj(spherePoint, center, r, { 0, 1, 0 }, { 0, 80, 0 }); // project the projection on the xOz plane with y = 80
            sp = spaceToScreen(planePoint, camera, zoom); // display it
            ppts[i].x = sp.x;
            ppts[i].y = sp.y;
            ppts[i].z = planePoint.z;
            fillCircle(sp.x, sp.y, 1, 0x001f);
        }

        // Draw lines
        for (int i = 0; i < 12; i++)
        {
            drawLine(pts[lines[i * 2]].x, pts[lines[i * 2]].y, pts[lines[i * 2 + 1]].x, pts[lines[i * 2 + 1]].y, 0x07e0);
            if(ppts[lines[i * 2]].z - camera.z > 0 && ppts[lines[i * 2 + 1]].z - camera.z > 0) drawLine(ppts[lines[i * 2]].x, ppts[lines[i * 2]].y, ppts[lines[i * 2 + 1]].x, ppts[lines[i * 2 + 1]].y, 0x001f);
        }

        theta += 2;

        // Move the sphere
        if (G_keys[SDLK_LEFT]) center.x -= 2;
        if (G_keys[SDLK_RIGHT]) center.x += 2;
        if (G_keys[SDLK_w]) center.z += 2;
        if (G_keys[SDLK_s]) center.z -= 2;
        if (G_keys[SDLK_UP]) center.y -= 2;
        if (G_keys[SDLK_DOWN]) center.y += 2;
    }

    deinitBuffering();

    return 0;
}