syntheyes_lensgrid.c

#include "grid.h"
#include <OpenGL/gl.h>
#include <OpenGL/glu.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>

typedef struct {
    double pixX;
    double pixY;
    double texS;
    double texT;
} point2d;

typedef struct {
    point2d* upperLeft;
    point2d* upperRight;
    point2d* lowerRight;
    point2d* lowerLeft;
} quad2d;

double convertToUV(double absValue, double absSide) {
    double x = (absValue / absSide) - 0.5;
    return x * 2;
}

double convertFromUV(double absolute_side, double uv_value) {
	// First, start from zero (-.1 becomes .4)
    double value_off_corner = (uv_value / 2) + 0.5;
    return absolute_side * value_off_corner - (absolute_side / 2);
}

void applyDistortionTo(point2d * pt, double k, double kcube, double aspect, double widthPix, double heightPix) {
    double u = convertToUV(pt->pixX + (widthPix / 2), widthPix);
    double v = convertToUV(pt->pixY + (heightPix / 2), heightPix);
    
    double r2 = aspect * aspect * u * u + v * v;
    
    double f;
    if (fabs(kcube) > 0.00001) {
        f = 1 + r2*(k + kcube*sqrt(r2));
    } else {
        f = 1 + r2*(k);
    }
	
    double udist = f*u;
    double vdist = f*v;
    pt->pixX = convertFromUV(widthPix, udist);
    pt->pixY = convertFromUV(heightPix, vdist);
}


void drawGridVertex(point2d * pt) {
	glTexCoord2f((GLdouble) pt->texS, (GLdouble) pt->texT);
	glVertex2d((GLdouble) pt->pixX, (GLdouble) pt->pixY);
}

void drawGrid(double width, double height, int subdivs, int enableDistortion, double kFactor, double kCubeFactor) {
    
	// Here we need to clamp the number of subdivs because we are on the stack 
	// to simplify two-dimensional arrays
	// for us. 128 subdivs is plenty enuff.
	if (subdivs > 512) subdivs = 512;
    
    int verticesPerSide = subdivs + 1;
    point2d* meshVertices[verticesPerSide][verticesPerSide];
	
    double aspect = width / height;
    double hUnit = width / subdivs;
    double vUnit = height / subdivs;
    
    double tuUnit = 1.0 / subdivs;
    double tvUnit = 1.0 / subdivs;
    
    int i, vi;

    for (i = 0; i < verticesPerSide; i++) {
        for (vi = 0; vi < verticesPerSide; vi++) {
            
			point2d * p = malloc(sizeof(point2d));
			
			// Assign vertex coords
            p->pixX = (i * hUnit) - (width/2);
            p->pixY = (vi * vUnit) - (height/2);
            
			// Assign ST coords for the 2d texture, using a cutout from the whole
            // texture that maps onto this specific quad. Assume the whole frame is 1x1.
			// GL textures go from 0 bottom left up and clockwise
            p->texS = i * tuUnit;
            p->texT = (tvUnit * subdivs) - (vi * tvUnit);
            if(enableDistortion) {
				applyDistortionTo(p, kFactor, kCubeFactor, aspect, width, height);
			}
			meshVertices[i][vi] = p;
		}
    }
	
	// Sort the vertices into quads
    quad2d * meshQuads[subdivs][subdivs];
	
    for (i = 0; i < subdivs; i++) {
        for(vi = 0; vi < subdivs; vi++) {
            quad2d * q = malloc(sizeof(quad2d));
			q->upperLeft = meshVertices[i][vi];
            q->upperRight = meshVertices[i+1][vi];
            q->lowerRight = meshVertices[i+1][vi+1];
            q->lowerLeft = meshVertices[i][vi+1];
			meshQuads[i][vi] = q;
		}
    }
	
	// Actually draw the quads
	for(i = 0; i < subdivs; i++) {
		for (vi = 0; vi < subdivs; vi++) {
			point2d * quadPts[] = {meshQuads[i][vi]->upperLeft, meshQuads[i][vi]->upperRight, meshQuads[i][vi]->lowerRight, meshQuads[i][vi]->lowerLeft};
			for (int vertexi = 0; vertexi < 4; vertexi++) {
				drawGridVertex(quadPts[vertexi]);
			}
		}
	}
	
	// Release all the vertices
	for (i = 0; i < verticesPerSide; i++) {
        for (vi = 0; vi < verticesPerSide; vi++) {
			free(meshVertices[i][vi]);
		}
	}
	
	// Release all the quads
	for (i = 0; i < subdivs; i++) {
        for (vi = 0; vi < subdivs; vi++) {
			free(meshQuads[i][vi]);
		}
	}
}