Extend the cubic spline form 2D to 3D

/*
 *
 * Algorithm derived from http://blog.ivank.net/interpolation-with-cubic-splines.html
 */

#include <Windows.h>
#include <iostream>
#include <vector>
#include <GL/glut.h>

#define WIDTH 400
#define HEIGHT 400
#define POINT_SIZE 10.0f


// Typedefs to make it easier to refer to our matrices
typedef std::vector<double> MatrixRow;
typedef std::vector<MatrixRow> Matrix;

// Struct for representing a point in 2D space
struct Point {
	double x;
	double y;
};

// Variables for dragging points arround
bool dragging = false;
Point lastPoint = Point{ -1, -1 };

// If the next click should delete a point instead of dragging or adding one
bool shouldDelete = false;

// The number of interpolation steps to do between points
int steps = 5;

// The list of spline points, sorted from lowest x value to highest
std::vector<Point> points;

// Options for the menu
enum MENU_OPTIONS
{
	INCREASE_STEPS,
	DECREASE_STEPS,
	QUIT
};

/**
 * Helper function to a point while still maintaining the vector's sorting
 * Also disallows two points having the same x coordinate
 */
void insertPoint(Point p)
{
	// If the size is 0 just go ahead and insert
	if (points.size() == 0) {
		points.push_back(p);
	}
	else {
		bool inserted = false;
		for (auto it = points.begin(); it != points.end(); ++it) {
			Point cur = *it;
			if (cur.x == p.x) {
				// We can't have overlapping x's for the calculation
				return;
			}
			if (cur.x > p.x) {
				points.insert(it, p);
				inserted = true;
				break;
			}
		}

		if (!inserted) {
			points.push_back(p);
		}
	}
}

/**************************************************************
* Spline functions
**************************************************************/

/**
 * Solve the given Matrix and return the k values
 */
std::vector<double> solveMatrix(Matrix A)
{
	std::vector<double> ks(points.size());

	int m = A.size();

	// Iterate through columns
	for (int k = 0; k < m; k++) {
		// Pivot for column
		int i_max = 0;
		int val_max = MININT;

		for (int i = k; i < m; i++) {
			if (A[i][k] > val_max) {
				i_max = i;
				val_max = A[i][k];
			}
		}

		// Swap row k and the max row
		MatrixRow temp = A[k];
		A[k] = A[i_max];
		A[i_max] = temp;

		// Iterate on all the rows before the pivot
		for (int i = k + 1; i < m; i++) {
			for (int j = k + 1; j < m + 1; j++) {
				// Make sure there's no division by 0
				if (A[k][k] == 0) {
					std::cerr << "Division by 0, can't calculate k values" << std::endl;
					return ks;
				}
				A[i][j] = A[i][j] - A[k][j] * (A[i][k] / A[k][k]);
			}

			A[i][k] = 0;
		}
	}

	// Iterate through columns
	for (int i = m - 1; i >= 0; i--) {
		// Make sure there's no division by 0
		if (A[i][i] == 0) {
			std::cerr << "Division by 0, can't calculate k values" << std::endl;
			return ks;
		}

		double v = A[i][m] / A[i][i];
		ks[i] = v;

		// Iterate over rows
		for (int j = i - 1; j >= 0; j--) {
			A[j][m] -= A[j][i] * v;
			A[j][i] = 0;
		}
	}

	return ks;
}

/**
 * Calculates the k values for the points vector and returns them
 */
std::vector<double> getKs()
{
	int n = points.size() - 1;
	std::vector<double> ks;
	
	// The n + 1 by n + 2 matrix that we will solve, represented as nested vectors
	Matrix A(n + 1);
	for (int i = 0; i < A.size(); i++) {
		// Initialize the rows to be n + 2 long
		A[i] = MatrixRow(n + 2);
	}

	// Iterate through the rows (skipping the first one)
	for (int i = 1; i < n; i++) {
		double prevXDiff = points[i].x - points[i - 1].x;
		double nextXDiff = points[i + 1].x - points[i].x;
		double prevYDiff = points[i].y - points[i - 1].y;
		double nextYDiff = points[i + 1].y - points[i].y;

		// If either diff is 0, we can't continue because division would fail
		if (prevXDiff == 0 || nextXDiff == 0) {
			std::cerr << "Two points are directly above each other at x=" << points[i].x
			          << ", unable to calculate k values" << std::endl;
			return ks; // Return exmpty vector
		}

		// Now calculate the columns
		A[i][i - 1] = 1 / prevXDiff;
		A[i][i] = 2 * (1 / prevXDiff + 1 / nextXDiff);
		A[i][i + 1] = 1 / nextXDiff;
		A[i][n + 1] = 3 * ((prevYDiff / (prevXDiff * prevXDiff)) + (nextYDiff / (nextXDiff * nextXDiff)));
	}

	double xdiff = points[1].x - points[0].x;
	double ydiff = points[1].y - points[0].y;

	// If xdiff is 0, we can't continue because division would fail
	if (xdiff == 0) {
		std::cerr << "Two points are directly above each other at x=" << points[0].x
		          << ", unable to calculate k values" << std::endl;
		return ks; // Return empty vector
	}

	// Calculate the first row
	A[0][0] = 2 / xdiff;
	A[0][1] = 1 / xdiff;
	A[0][n + 1] = 3 * ydiff / (xdiff * xdiff);

	xdiff = points[n].x - points[n - 1].x;
	ydiff = points[n].y - points[n - 1].y;

	// If xdiff is 0, we can't continue because division would fail
	if (xdiff == 0) {
		std::cerr << "Two points are directly above each other at x=" << points[0].x
		          << ", unable to calculate k values";
		return ks; // Return empty vector
	}

	// Calculate the last row
	A[n][n - 1] = 1 / xdiff;
	A[n][n] = 2 / xdiff;
	A[n][n + 1] = 3 * ydiff / (xdiff * xdiff);

	ks = solveMatrix(A);

	return ks;
}

/**
 * Given an x coordinate and a vector of k values, return
 *  the y coordinate of the spline. The ks vector should be
 *  the same length as the points vector
 */
double getSplineHeightAt(double x, std::vector<double> ks)
{
	// If we don't have the right amount of K's we can't do anything
	if (ks.size() != points.size()) {
		std::cerr << "We don't have enough K's for the points we have, there might be two points directly above each other" << std::endl;
		return -1;
	}

	// Find the index of the point we will use as one of the points in the spline
	int index;
	for (index = 1; index < points.size(); index++) {
		if (points[index].x >= x) {
			// We found the point we want
			break;
		}
	}

	// If we didn't get a valid index, give up
	if (index == points.size()) {
		return -1;
	}
	
	// If these are equal, the division after this would fail, so show an error and abort
	if (points[index].x == points[index - 1].x) {
		std::cerr << "Two points are directly above each other at x=" << points[index].x
		          << ", unable to calculate spline for that point";
		return -1;
	}

	// The difference between the current point's x and y coordinates and the previous one
	double xdiff = points[index].x - points[index - 1].x;
	double ydiff = points[index].y - points[index - 1].y;

	// Perform the calculations needed to determine the y coordinate
	double t = (x - points[index - 1].x) / xdiff;
	double a = ks[index - 1] * xdiff - ydiff;
	double b = -ks[index] * xdiff + ydiff;

	// Interpolate to the point we want and then return it
	double q = (1 - t) * points[index - 1].y + t * points[index].y + t * (1 - t) * (a * (1 - t) + b * t);
	return q;
}

/**************************************************************
 * Event handlers
 **************************************************************/

void onDisplay(void)
{
	// Clear the buffer
	glClear(GL_COLOR_BUFFER_BIT);

	glColor3f(1.0f, 1.0f, 1.0f);

	glPushMatrix();

	// Draw the spline (if we have at least 3 points)
	if (points.size() >= 3) {
		glBegin(GL_LINE_STRIP);
			std::vector<double> ks = getKs();

			// Loop over all the points (skipping the first
			for (int i = 1; i < points.size(); ++i) {
				Point cur = points[i];
				Point prev = points[i - 1];

				// Iterate over the X's between this point and the previous
				double increment = (cur.x - prev.x) / steps;
				for (double x = prev.x; x <= cur.x; x += increment) {
					double y = getSplineHeightAt(x, ks);
					if (y == -1) {
						// There was an error
						break;
					}
					glVertex2d(x, y);
				}
			}
		glEnd();
	}

	// Draw the points
	glPointSize(POINT_SIZE);
	glBegin(GL_POINTS);
		for (auto it = points.begin(); it != points.end(); ++it) {
			Point point = *it;
			glVertex2d(point.x, point.y);
		}
	glEnd();

	glPopMatrix();

	// Draw now
	glutSwapBuffers();
}

/**
 * Control dragging when the mouse moves with a button down
 */
void onActiveMouseMotion(int x, int y)
{
	if (!dragging) {
		// We only care about this if we're dragging
		return;
	}

	// Remove the previous point (if it exists)
	if (!(lastPoint.x == -1 && lastPoint.y == -1)) {
		for (auto it = points.begin(); it != points.end(); ++it) {
			Point p = *it;
			if (p.x == lastPoint.x && p.y == lastPoint.y) {
				points.erase(it);
				break;
			}
		}
	}

	// Make a new point for the current location and insert it
	lastPoint = Point{ (double)x, (double)y };
	insertPoint(lastPoint);

	// Since we changed something we want to redraw
	glutPostRedisplay();
}

/**
 * If a mouse button is pressed, see if we need to drag or create a new point
 */
void onMouseButton(int button, int state, int x, int y)
{
	// Do something with the mouse
	std::cout << "Click: " << button << " " << state << " " << x << " " << y << std::endl;

	if (button != GLUT_LEFT_BUTTON) {
		// We only care about the left button
		return;
	}

	if (state == 0) {
		// See if they are trying to choose a point
		for (auto it = points.begin(); it != points.end(); ++it) {
			Point p = *it;

			// See if the mouse is on the square for the point
			if (x >= p.x - POINT_SIZE / 2 && x <= p.x + POINT_SIZE / 2 && y >= p.y - POINT_SIZE / 2 && y <= p.y + POINT_SIZE / 2) {
				if (!shouldDelete) {
					// We found a point to drag
					dragging = true;
					lastPoint = p;
				}
				else {
					// We want to delete, so delete this point
					points.erase(it);
				}
				break;
			}
		}
		if (!dragging) {
			lastPoint = Point{ -1, -1 };
		}
	}


	if (state == 1) {
		if (!dragging && !shouldDelete) {
			// If we weren't dragging or deleting anything, insert a point on mouse up
			insertPoint(Point{ (double)x,  (double)y });
		}
		else {
			// Otherwise, say we're done dragging
			dragging = false;
			lastPoint = Point{ -1, -1 };
		}
	}

	// Since we changed something we want to redraw
	glutPostRedisplay();
}

void onKeyDown(unsigned char key, int x, int y)
{
	// Do something with the keyboard
	std::cout << "Key down: " << key << " " << x << " " << y << std::endl;
	switch (key) {
	case 'd':
	case 'D':
		shouldDelete = true;
		break;
	}
}

void onKeyUp(unsigned char key, int x, int y)
{
	// Do something with the keyboard
	std::cout << "Key up: " << key << " " << x << " " << y << std::endl;

	switch (key) {
	case 'q':
	case 'Q':
		exit(0);
		break;
	case 'd':
	case 'D':
		shouldDelete = false;
		break;
	case '+':
		steps += 1;
		if (steps > 15) steps = 15;
		break;
	case '-':
		steps -= 1;
		if (steps <= 0) steps = 1;
		break;
	}

	// Since we changed something we want to redraw
	glutPostRedisplay();
}

void onMenu(int option)
{
	switch (option) {
	case INCREASE_STEPS:
		steps += 1;
		if (steps > 15) steps = 15;
		break;
	case DECREASE_STEPS:
		steps -= 1;
		if (steps <= 0) steps = 1;
		break;
	case QUIT:
		exit(0);
		break;
	}

	// Since we changed something we want to redraw
	glutPostRedisplay();
}

/**************************************************************
* Main/init functions
**************************************************************/

void initGL(void)
{
	// Set clear color
	glClearColor(0.0, 0.0, 0.0, 0.0);

	// Initialize view
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluOrtho2D(0, WIDTH, HEIGHT, 0);
}


/**
 * Declare initial window size, position, and display mode
 * Set up callback functions for glut events
 */
int main(int argc, char** argv)
{
	// Init everything
	glutInit(&argc, argv);
	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB);
	glutInitWindowSize(WIDTH, HEIGHT);
	glutCreateWindow("Cubic Spline");
	initGL();

	// Hook up callbacks
	glutDisplayFunc(onDisplay);
	glutMouseFunc(onMouseButton);
	glutMotionFunc(onActiveMouseMotion);
	glutKeyboardFunc(onKeyDown);
	glutKeyboardUpFunc(onKeyUp);

	// Set up the menu
	glutCreateMenu(onMenu);
	glutAddMenuEntry("Increase # of interpolation steps", INCREASE_STEPS);
	glutAddMenuEntry("Decrease # of interpolation steps", DECREASE_STEPS);
	glutAddMenuEntry("Quit", QUIT);
	glutAttachMenu(GLUT_RIGHT_BUTTON);

	// Go!
	glutMainLoop();

	return 0;
}

Select allOpen in new window