Add support for post solve assembling

7 files changed, 155 insertions, 65 deletions

diff --git a/examples/box_and_lines.c b/examples/box_and_lines.c
index 7026268..d90f208 100644
--- a/examples/box_and_lines.c
+++ b/examples/box_and_lines.c
@@ -135,7 +135,9 @@ void a_box(struct box *box, struct topology *topo, struct constraints *constr) {
 
 void draw_from_constraints(struct constraints *c, struct topology *t, struct canvas *cv) {
 	struct drawing drawing = {};
-	bool rc = solve_constraints(c, &drawing);
+	struct subassembly assemblies[16] = {};
+	size_t assemblies_num;
+	bool rc = solve_constraints(c, &drawing, assemblies, &assemblies_num);
 	assert(rc);
 
 	// Copy over all the parameter values to a new array
diff --git a/examples/stacked.c b/examples/stacked.c
index 8f3d09b..aeee9ee 100644
--- a/examples/stacked.c
+++ b/examples/stacked.c
@@ -63,7 +63,9 @@ void a_box(struct box *box, struct topology *topo, struct constraints *constr) {
 
 void draw_from_constraints(struct constraints *c, struct topology *t, struct canvas *cv) {
 	struct drawing drawing = {};
-	bool rc = solve_constraints(c, &drawing);
+	struct subassembly assemblies[16] = {};
+	size_t assemblies_num;
+	bool rc = solve_constraints(c, &drawing, assemblies, &assemblies_num);
 	assert(rc);
 
 	// Copy over all the parameter values to a new array
diff --git a/examples/triangle_tip.c b/examples/triangle_tip.c
index 6b05b10..9d2d5ce 100644
--- a/examples/triangle_tip.c
+++ b/examples/triangle_tip.c
@@ -2,7 +2,9 @@
 
 void draw_from_constraints(struct constraints *c, struct topology *t, struct canvas *cv) {
 	struct drawing drawing = {};
-	bool rc = solve_constraints(c, &drawing);
+	struct subassembly assemblies[16] = {};
+	size_t assemblies_num;
+	bool rc = solve_constraints(c, &drawing, assemblies, &assemblies_num);
 	assert(rc);
 
 	// Copy over all the parameter values to a new array
@@ -15,6 +17,8 @@ void draw_from_constraints(struct constraints *c, struct topology *t, struct can
 	place_points(&drawing, params);
 	free(params);
 
+	reconstruct_drawing(c, assemblies, &assemblies_num);
+
 	begin_drawing(cv);
 	draw_topology(cv, t);
 	draw_constraints(cv, c);
@@ -50,7 +54,7 @@ int main(int argc, char *argv[]) {
 		// With another triangle sharing a point
 		PP_DISTANCE(&p4, &p5, 30),
 		PP_DISTANCE(&p3, &p4, 30),
-		PP_DISTANCE(&p3, &p5, 30),
+		PP_DISTANCE(&p5, &p3, 30),
 
 		// LL_ANGLE(&t2side, &t2base, DEG(120)),
 
@@ -61,7 +65,7 @@ int main(int argc, char *argv[]) {
 		POINT_ON_LINE(&p5, &t2base),
 
 		// The edge they don't share is constrained
-		LL_ANGLE(&t1base, &t2base, DEG(80)),
+		LL_ANGLE(&t1base, &t2base, DEG(70)),
 		CEND(),
 	});
 
diff --git a/inc/cad/solve.h b/inc/cad/solve.h
index 11afded..95c6b58 100644
--- a/inc/cad/solve.h
+++ b/inc/cad/solve.h
@@ -101,9 +101,22 @@ struct constraint {
 	uint8_t used;
 };
 
+struct subassembly {
+	struct solve_step *steps;
+	size_t steps_num;
+	size_t fix;
+
+	struct component **articulation;
+	struct element **articulation_position;
+	size_t articulation_num;
+
+	bool fixed;
+};
+
 void alias_point(struct constraints *c, struct component *alias, struct component *target);
 void add_constraint(struct constraints *c, struct constraint *new);
 void free_constraints(struct constraints *c);
-bool solve_constraints(struct constraints *constraints, struct drawing *drawing);
+bool solve_constraints(struct constraints *constraints, struct drawing *drawing, struct subassembly *assemblies, size_t *assemblies_num);
+void reconstruct_drawing(struct constraints *constraints, struct subassembly *assemblies, size_t *assemblies_num);
 
 #define DEG(x) ((x) * M_PI / 180.0)
diff --git a/src/main.c b/src/main.c
index 3781dcd..67bd511 100644
--- a/src/main.c
+++ b/src/main.c
@@ -116,7 +116,9 @@ struct mid a_midpoint() {
 
 void draw_from_constraints(struct constraints *c, struct topology *t, struct canvas *cv) {
 	struct drawing drawing = {};
-	solve_constraints(c, &drawing);
+	struct subassembly assemblies[16] = {};
+	size_t assembly_num;
+	solve_constraints(c, &drawing, assemblies, &assembly_num);
 
 	// Copy over all the parameter values to a new array
 	// @PERF: Maybe we should just store them in a separate array to start with
diff --git a/src/solve.c b/src/solve.c
index 789b977..1401f44 100644
--- a/src/solve.c
+++ b/src/solve.c
@@ -220,17 +220,6 @@ static bool fix_first(struct constraint *constraints, size_t constraints_num, si
 	return false;
 }
 
-struct subassembly {
-	struct solve_step *steps;
-	size_t steps_num;
-
-	struct component **articulation;
-	struct element *articulation_position;
-	size_t articulation_num;
-
-	bool fixed;
-};
-
 static size_t build_triangles(struct constraint *constraints, size_t constraints_num, size_t origin, uint8_t useid, struct subassembly *assembly) {
 	struct solve_step *steps = assembly->steps;
 	struct frontier frontier = {};
@@ -691,7 +680,8 @@ static void draw_solution(struct constraint *constraints, size_t fix, struct sol
 	}
 }
 
-bool solve_constraints(struct constraints *constraints, struct drawing *drawing) {
+bool solve_constraints(struct constraints *constraints, struct drawing *drawing, struct subassembly *assemblies, size_t *assemblies_num) {
+	*assemblies_num = 0;
 	// Replace all the aliased points with the point they point to
 	for(size_t i = 0; i < constraints->aliases_num; i++) {
 		struct alias *alias = &constraints->aliases[i];
@@ -706,52 +696,81 @@ bool solve_constraints(struct constraints *constraints, struct drawing *drawing)
 	// Pick some point point distance constraint as the base
 	size_t fix;
 
-	struct subassembly assemblies[16] = {0};
-	size_t assemblies_num = 0;
 	while(fix_first(constraints->elements, constraints->length, &fix)) {
 		// Build triangles on that root
-		assemblies[assemblies_num].steps = malloc(sizeof(struct solve_step) * constraints->length);
-		assemblies[assemblies_num].articulation = malloc(sizeof(struct component*) * constraints->length);
-		assemblies[assemblies_num].articulation_position = malloc(sizeof(struct element) * constraints->length);
-		assemblies[assemblies_num].steps_num = build_triangles(constraints->elements, constraints->length, fix, assemblies_num+1, &assemblies[assemblies_num]);
-
-		printf("Assembly %ld\n", assemblies_num);
-		for(size_t i = 0; i < assemblies[assemblies_num].articulation_num; i++) {
-			printf("  Articulation %p\n", assemblies[assemblies_num].articulation[i]);
-		}
+		assemblies[*assemblies_num].steps = malloc(sizeof(struct solve_step) * constraints->length);
+		assemblies[*assemblies_num].articulation = malloc(sizeof(struct component*) * constraints->length);
+		assemblies[*assemblies_num].articulation_position = malloc(sizeof(struct element*) * constraints->length);
+		assemblies[*assemblies_num].steps_num = build_triangles(constraints->elements, constraints->length, fix, *assemblies_num+1, &assemblies[*assemblies_num]);
+		assemblies[*assemblies_num].fix = fix;
+
+		// printf("Assembly %ld\n", *assemblies_num);
+		// for(size_t i = 0; i < assemblies[*assemblies_num].articulation_num; i++) {
+		// 	printf("  Articulation %p\n", assemblies[*assemblies_num].articulation[i]);
+		// }
 
 		// printf("Solved in %ld steps\n", steps_num);
 
-		draw_solution(constraints->elements, fix, assemblies[assemblies_num].steps, assemblies[assemblies_num].steps_num, drawing);
-		for(size_t i = 0; i < assemblies[assemblies_num].articulation_num; i++) {
-			memcpy(&assemblies[assemblies_num].articulation_position[i], assemblies[assemblies_num].articulation[i]->e, sizeof(struct component));
+		draw_solution(constraints->elements, fix, assemblies[*assemblies_num].steps, assemblies[*assemblies_num].steps_num, drawing);
+		for(size_t i = 0; i < assemblies[*assemblies_num].articulation_num; i++) {
+			assemblies[*assemblies_num].articulation_position[i] = assemblies[*assemblies_num].articulation[i]->e;
 		}
-		assemblies_num++;
-		assert(assemblies_num <= 16);
+		(*assemblies_num)++;
+		assert(*assemblies_num <= 16);
 	}
 
+	// Fill out the aliased points out with the values from their targets
+	for(size_t i = 0; i < constraints->aliases_num; i++) {
+		struct alias *alias = &constraints->aliases[i];
+
+		memcpy(alias->alias, alias->target, sizeof(struct component));
+	}
+
+	// Check for unsolved constraints
+	bool complete = true;
+	// for(size_t i = 0; i < constraints->length; i++) {
+	// 	if(!constraints->elements[i].used) {
+	// 		complete = false;
+	// 	}
+	// }
+
+	return complete;
+}
+
+static void affine_transform_vec2(mat3 m, vec2 in, vec2 out) {
+	vec3 h = {in[0], in[1], 1.0f};
+	vec3 result;
+	glm_mat3_mulv(m, h, result);
+	out[0] = result[0];
+	out[1] = result[1];
+}
+
+void reconstruct_drawing(struct constraints *constraints, struct subassembly *assemblies, size_t *assemblies_num) {
 	// We build everything from the first assembly
 	assemblies[0].fixed = true;
 	while(true) {
 		// Look for unfixed assembly we can connect to something that is fixed
-		for(size_t i = 0; i < assemblies_num; i++) {
+		for(size_t i = 0; i < *assemblies_num; i++) {
 			if(assemblies[i].fixed) continue;
 
 			// Find a fixed asssembly it connects to
-			for(size_t j = 0; j < assemblies_num; j++) {
+			for(size_t j = 0; j < *assemblies_num; j++) {
 				if(!assemblies[j].fixed) continue;
 
-				struct component *articulation1 = NULL;
+				size_t articulation_i;
+				size_t articulation_j;
 
 				// Find a shared articulation
-				for(size_t articuation_i = 0; articuation_i < assemblies[i].articulation_num; articuation_i++) {
-					for(size_t articuation_j = 0; articuation_j < assemblies[j].articulation_num; articuation_j++) {
-						if(assemblies[i].articulation[articuation_i] == assemblies[j].articulation[articuation_j]) {
-							articulation1 = assemblies[i].articulation[articuation_i];
-							break;
+				for(articulation_i = 0; articulation_i < assemblies[i].articulation_num; articulation_i++) {
+					for(articulation_j = 0; articulation_j < assemblies[j].articulation_num; articulation_j++) {
+						if(assemblies[i].articulation[articulation_i] == assemblies[j].articulation[articulation_j]) {
+							goto articulation_found;
 						}
 					}
 				}
+				continue;
+articulation_found:
+				;
 
 				struct constraint *constraint = NULL;
 				bool forward;
@@ -794,26 +813,61 @@ constraint_matches_j:
 				// share a constraint. We can hopefully place the rest of the
 				// assembly from that information
 
-				printf("We found a match %p, %p\n", articulation1, constraint);
-			}
-		}
-		break;
-	}
+				float theta = atan2(constraint->c1->e->line.norm[1], constraint->c1->e->line.norm[0]) - atan2(constraint->c2->e->line.norm[1], constraint->c2->e->line.norm[0]);
+				theta = forward ? theta : -theta;
 
-	// Fill out the aliased points out with the values from their targets
-	for(size_t i = 0; i < constraints->aliases_num; i++) {
-		struct alias *alias = &constraints->aliases[i];
+				constraint->used = true;
+				theta += constraint->forward ? constraint->v : -constraint->v;
 
-		memcpy(alias->alias, alias->target, sizeof(struct component));
-	}
+				assert(assemblies[i].articulation_position[articulation_i]->type == ETYPE_POINT);
+				assert(assemblies[j].articulation_position[articulation_j]->type == ETYPE_POINT);
 
-	// Check for unsolved constraints
-	bool complete = true;
-	// for(size_t i = 0; i < constraints->length; i++) {
-	// 	if(!constraints->elements[i].used) {
-	// 		complete = false;
-	// 	}
-	// }
+				mat3 transform;
+				glm_mat3_identity(transform);
 
-	return complete;
+				glm_translate2d(transform, assemblies[i].articulation_position[articulation_i]->point.pos);
+
+				glm_rotate2d(transform, theta);
+
+				{
+					vec2 negative_translate;
+					glm_vec2_negate_to(assemblies[j].articulation_position[articulation_j]->point.pos, negative_translate);
+					glm_translate2d(transform, negative_translate);
+				}
+
+				// Transform the baseline points
+				{
+					struct component *c = constraints->elements[assemblies[i].fix].c1;
+					assert(c->type == COM_POINT);
+
+					affine_transform_vec2(transform, c->e->point.pos, c->e->point.pos);
+				}
+				{
+					struct component *c = constraints->elements[assemblies[i].fix].c2;
+					assert(c->type == COM_POINT);
+
+					affine_transform_vec2(transform, c->e->point.pos, c->e->point.pos);
+				}
+
+				// Transform all other points in the body by iterating the
+				// steps. Each step places a single component.
+				for(size_t k = 0; k < assemblies[i].steps_num; k++) {
+					struct solve_step *step = &assemblies[i].steps[k];
+
+					struct component *c = step->i_forward ?
+						constraints->elements[step->i].c2 :
+						constraints->elements[step->i].c1;
+
+					if(c->type == COM_POINT) {
+						affine_transform_vec2(transform, c->e->point.pos, c->e->point.pos);
+					}
+					// @HACK We're not transforming lines, this relies on the
+					// user not requiring/caring about the lines AFTER solving.
+					// This is wrong, but useful enough for getting something
+					// on screen.
+				}
+			}
+		}
+		break;
+	}
 }
diff --git a/test/solve.c b/test/solve.c
index a331573..e1729ff 100644
--- a/test/solve.c
+++ b/test/solve.c
@@ -2,6 +2,8 @@
 #include <assert.h>
 
 int main(int argc, char *argv[]) {
+	struct subassembly assemblies[16] = {};
+	size_t assemblies_num;
 	{
 		printf("Triangle by 3 distances\n");
 		struct constraints constraints = {};
@@ -18,7 +20,7 @@ int main(int argc, char *argv[]) {
 		});
 
 		struct drawing drawing = {};
-		bool solved = solve_constraints(&constraints, &drawing);
+		bool solved = solve_constraints(&constraints, &drawing, assemblies, &assemblies_num);
 
 		assert(solved);
 
@@ -55,7 +57,7 @@ int main(int argc, char *argv[]) {
 		});
 
 		struct drawing drawing = {};
-		bool solved = solve_constraints(&constraints, &drawing);
+		bool solved = solve_constraints(&constraints, &drawing, assemblies, &assemblies_num);
 
 		assert(solved);
 
@@ -105,7 +107,7 @@ int main(int argc, char *argv[]) {
 		});
 
 		struct drawing drawing = {};
-		bool solved = solve_constraints(&constraints, &drawing);
+		bool solved = solve_constraints(&constraints, &drawing, assemblies, &assemblies_num);
 		assert(solved);
 
 		free_drawing(&drawing);
@@ -149,7 +151,18 @@ int main(int argc, char *argv[]) {
 		});
 
 		struct drawing drawing = {};
-		bool solved = solve_constraints(&constraints, &drawing);
+		bool solved = solve_constraints(&constraints, &drawing, assemblies, &assemblies_num);
+
+		// Copy over all the parameter values to a new array
+		// @PERF: Maybe we should just store them in a separate array to start with
+		double *params = malloc(sizeof(double) * (constraints.length));
+		for(size_t i = 0; i < constraints.length; i++) {
+			params[i] = constraints.elements[i].v;
+		}
+
+		place_points(&drawing, params);
+
+		reconstruct_drawing(&constraints, assemblies, &assemblies_num);
 
 		// @COMPL: We don't currently know how to derive an angle constraint
 		// from the rigid triangle. We'd probably need to do some sort of