a mediocre linear equation solver written in C

eq_solver.c

#include <inttypes.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

enum OpType { OpNull = 0, OpAdd, OpSubtract };

typedef int64_t eqVal;
#define eqPRI PRIi64

typedef struct {
	int type;
	int rowA, rowB;
	eqVal multiplyA, multiplyB;
} operation_t;

#define VAR_COUNT 4
const char *varLetters = "xyzA";

void print_vars(eqVal *vars, int varCount) {
	int i;
	for (i = 0; i < varCount; i++) {
		printf((i == 0) ? "{ %c = %"eqPRI : ", %c = %"eqPRI, varLetters[i], vars[i]);
	}
	printf(" }");
}

void print_equation(eqVal *equation, int varCount) {
	// Print out an equation in neat form
	int i;
	for (i = 0; i < varCount; i++) {
		printf((i == 0) ? "%4"eqPRI"%c" : " + %4"eqPRI"%c", equation[i], varLetters[i]);
	}
	printf(" = %4"eqPRI, equation[varCount]);
}

int solve(eqVal *equations, eqVal *outputVars, int varCount) {
	eqVal *matrix, *prevMatrix;
	int *sortIndices;
	operation_t *operations;
	int stride = varCount + 1;
	int matrixSize = sizeof(eqVal) * stride * varCount;
	int pass, row, row2, flag, i;
	eqVal coef, check, sum;
	int isUnique;

	for (i = 0; i < varCount; i++)
		outputVars[i] = 0xFFFFFFFF;

	matrix = malloc(matrixSize);
	prevMatrix = malloc(matrixSize);
	sortIndices = malloc(sizeof(int) * varCount);
	operations = malloc(sizeof(operation_t) * varCount);

	memcpy(matrix, equations, matrixSize);

	// STAGE 1: Convert the set of equations into triangular form
	for (pass = 0; pass < (varCount - 1); pass++) {
		printf("[ PASS %d ]\n", pass + 1);

		// In pass n, we shall eliminate all coefficients
		// in variable n underneath row n.

		// We must ensure all rows with coefficient 0 in variable n are
		// located underneath all rows with non-zero coefficients.
		// To do this, we scan the matrix twice.
		// The first pass grabs all non-zero rows and the second pass
		// grabs all zero rows;
		flag = 0;
		i = pass;

		for (row = pass; row < varCount; row++) {
			if (matrix[row * stride + pass] != 0) {
				sortIndices[i] = row;
				if (i != row) flag = 1;
				++i;
			}
		}
		for (row = pass; row < varCount; row++) {
			if (matrix[row * stride + pass] == 0) {
				sortIndices[i] = row;
				if (i != row) flag = 1;
				++i;
			}
		}

		if (flag) {
			// If the flag is true, then we must do some reordering.
			// Print every row that comes before this, for consistency
			for (row = 0; row < pass; row++) {
				print_equation(&matrix[row * stride], varCount);
				printf("\n");
			}

			memcpy(prevMatrix, matrix, matrixSize);
			for (row = pass; row < varCount; row++) {
				// Copy the row to its rightful new location
				if (sortIndices[row] != row) {
					for (i = 0; i < stride; i++) {
						matrix[row * stride + i] = prevMatrix[sortIndices[row] * stride + i];
					}
				}
				print_equation(&matrix[row * stride], varCount);
				if (sortIndices[row] != row) {
					printf(" | r'%d = r%d", row + 1, sortIndices[row] + 1);
				}
				printf("\n");
			}

			printf("[ PASS %d.1 ]\n", pass + 1);
		}

		// Figure out what operations must be performed.
		memset(operations, 0, sizeof(operation_t) * varCount);

		for (row = pass + 1; row < varCount; row++) {
			coef = matrix[row * stride + pass];

			// If the coefficient here is already zero,
			// then we don't need to work on this row
			// this time round.
			if (coef == 0)
				continue;

			flag = 0;

			// Find a way to MAKE it zero!
			// Are there any rows with the same coefficient,
			// either positive or negative?
			for (row2 = row - 1; row2 >= pass; row2--) {
				if (row == row2)
					continue;

				check = matrix[row2 * stride + pass];

				if ((coef == check) || (coef == -check)) {
					// Same coef means we don't have to do any multiplication! :D
					operations[row].type = (coef == check) ? OpSubtract : OpAdd;
					operations[row].rowA = row;
					operations[row].multiplyA = 1;
					operations[row].rowB = row2;
					operations[row].multiplyB = 1;
					flag = 1;
					break;
				}
			}

			if (flag) continue;

			// Nope, so we must do some multiplication.
			// For simplicity's sake, rather than trying to find the
			// "simplest" operation, we'll just use the last
			// row with a non-zero coefficient.
			for (row2 = row - 1; row2 >= pass; row2--) {
				check = matrix[row2 * stride + pass];
				if (check == 0)
					continue;

				// If the two coefficients have the same sign,
				// we subtract one from the other. If they're
				// different, we add them both together so they'll
				// cancel each other out.
				if ((coef < 0 && check < 0) || (coef > 0 && check > 0))
					operations[row].type = OpSubtract;
				else
					operations[row].type = OpAdd;

				// Now make them match through multiplication!
				operations[row].rowA = row;
				operations[row].multiplyA = abs(check);
				operations[row].rowB = row2;
				operations[row].multiplyB = abs(coef);
				flag = 1;
				break;
			}
		}

		// Make these operations take effect!
		memcpy(prevMatrix, matrix, matrixSize);

		for (row = 0; row < varCount; row++) {
			if (operations[row].type == OpAdd) {
				for (i = 0; i < stride; i++) {
					matrix[row * stride + i] =
						(operations[row].multiplyA * prevMatrix[operations[row].rowA * stride + i]) +
						(operations[row].multiplyB * prevMatrix[operations[row].rowB * stride + i]);
				}
			} else if (operations[row].type == OpSubtract) {
				for (i = 0; i < stride; i++) {
					matrix[row * stride + i] =
						(operations[row].multiplyA * prevMatrix[operations[row].rowA * stride + i]) -
						(operations[row].multiplyB * prevMatrix[operations[row].rowB * stride + i]);
				}
			}

			print_equation(&matrix[row * stride], varCount);

			if (operations[row].type != OpNull) {
				printf(" | r'%d = ", row + 1);

				if (operations[row].multiplyA == 1)
					printf("r%d", operations[row].rowA + 1);
				else
					printf("%"eqPRI"r%d", operations[row].multiplyA, operations[row].rowA + 1);

				printf((operations[row].type == OpAdd) ? " + " : " - ");

				if (operations[row].multiplyB == 1)
					printf("r%d", operations[row].rowB + 1);
				else
					printf("%"eqPRI"r%d", operations[row].multiplyB, operations[row].rowB + 1);
			}

			printf("\n");
		}
	}


	// STAGE 2: We should now be in triangular form,
	// so work backwards and derive every variable.
	isUnique = 1;

	for (pass = varCount - 1; pass >= 0; pass--) {
		sum = matrix[pass * stride + varCount];

		// Subtract the terms following this one, if any
		for (i = pass + 1; i < varCount; i++)
			sum -= (outputVars[i] * matrix[pass * stride + i]);

		// Now, sum should be equal to coef * var
		coef = matrix[pass * stride + pass];
		if (coef == 0) {
			// This probably means we're not unique
			isUnique = 0;
			outputVars[pass] = 0;
		} else {
			outputVars[pass] = sum / coef;
		}
	}


	free(matrix);
	free(prevMatrix);
	free(operations);
	free(sortIndices);

	return isUnique;
}

int do_random_test() {
	eqVal realVars[VAR_COUNT], calculatedVars[VAR_COUNT];
	eqVal equations[VAR_COUNT][VAR_COUNT + 1];
	eqVal sum, coeff;
	int i, j, isUnique;

	// Generate some random values for x, y, z, ...
	for (i = 0; i < VAR_COUNT; i++)
		realVars[i] = (rand() % 40) - 20;

	print_vars(realVars, VAR_COUNT);
	printf("\n");

	// Generate some random equations that we'll use
	for (i = 0; i < VAR_COUNT; i++) {
		// random coefficients
		sum = 0;

		for (j = 0; j < VAR_COUNT; j++) {
			coeff = (rand() % 15) + 1;
			sum += (coeff * realVars[j]);
			equations[i][j] = coeff;
		}

		equations[i][VAR_COUNT] = sum;

		print_equation(equations[i], VAR_COUNT);
		printf("\n");
	}

	isUnique = solve(equations[0], calculatedVars, VAR_COUNT);
	print_vars(calculatedVars, VAR_COUNT);
	if (!isUnique) printf(" (NOT UNIQUE!!!)");
	printf("\n");

	// Check the results for correctness
	for (i = 0; i < VAR_COUNT; i++)
		if (realVars[i] != calculatedVars[i])
			return 0;
	return 1;
}

int main(int argc, char **argv) {
	int i, successes = 0, failures = 0, failure_log[100];

	srand(500);
	for (i = 0; i < 100; i++) {
		if (do_random_test()) {
			++successes;
		} else {
			failure_log[failures] = i;
			++failures;
		}
	}

	printf("successes: %d\n", successes);
	printf("failures: %d\n", failures);
	for (i = 0; i < failures; i++) {
		printf((i == 0) ? "[ %d" : ", %d", failure_log[i]);
	}
	if (failures) printf(" ]\n");

	return EXIT_SUCCESS;
}