Begin solving other meta constraints

3 files changed, 189 insertions, 53 deletions

diff --git a/examples/mount.c b/examples/mount.c
index 7ffdc34..7f4ba15 100644
--- a/examples/mount.c
+++ b/examples/mount.c
@@ -1,5 +1,47 @@
 #include "cad.h"
 
+static const char *component_type_name[] = {
+	[COM_POINT] = "point",
+	[COM_LINE] = "line",
+};
+
+static void print_subassemblies(struct constraints *c, struct subassembly *assemblies, size_t assemblies_num) {
+	fprintf(stderr, "=== Subassemblies: %zu ===\n", assemblies_num);
+	for(size_t i = 0; i < assemblies_num; i++) {
+		struct subassembly *a = &assemblies[i];
+		fprintf(stderr, "Assembly %zu: %zu steps, fix=%zu, fixed=%s\n",
+			i, a->steps_num, a->fix, a->fixed ? "yes" : "no");
+
+		if(a->articulation_num > 0) {
+			fprintf(stderr, "  Articulations (%zu):\n", a->articulation_num);
+			for(size_t j = 0; j < a->articulation_num; j++) {
+				struct component *comp = a->articulation[j];
+				fprintf(stderr, "    [%zu] %s %p\n", j, component_type_name[comp->type], (void*)comp);
+			}
+		}
+
+		fprintf(stderr, "  Constraints used:\n");
+		for(size_t j = 0; j < c->length; j++) {
+			struct constraint *ct = &c->elements[j];
+			if(ct->used != i + 1) continue;
+			fprintf(stderr, "    [%zu] %s v=%.2f (%s %p, %s %p)\n",
+				j, constraint_type_name[ct->type], ct->v,
+				component_type_name[ct->c1->type], (void*)ct->c1,
+				component_type_name[ct->c2->type], (void*)ct->c2);
+		}
+	}
+	fprintf(stderr, "  Unused constraints:\n");
+	for(size_t j = 0; j < c->length; j++) {
+		struct constraint *ct = &c->elements[j];
+		if(ct->used != 0) continue;
+		fprintf(stderr, "    [%zu] %s v=%.2f (%s %p, %s %p)\n",
+			j, constraint_type_name[ct->type], ct->v,
+			component_type_name[ct->c1->type], (void*)ct->c1,
+			component_type_name[ct->c2->type], (void*)ct->c2);
+	}
+	fprintf(stderr, "=========================\n");
+}
+
 void draw_from_constraints(struct constraints *c, struct topology *t, struct canvas *cv) {
 	struct drawing drawing = {};
 	struct subassembly assemblies[16] = {};
@@ -18,6 +60,7 @@ void draw_from_constraints(struct constraints *c, struct topology *t, struct can
 	free(params);
 
 	reconstruct_drawing(c, assemblies, &assemblies_num);
+	print_subassemblies(c, assemblies, assemblies_num);
 
 	begin_drawing(cv);
 	draw_topology(cv, t);
@@ -28,80 +71,122 @@ void draw_from_constraints(struct constraints *c, struct topology *t, struct can
 }
 
 int main(int argc, char *argv[]) {
-	struct constraints constraints = {};
+	struct constraints constraints = {0};
 	struct topology topo = {};
 
 	struct component mid_bot = {.type = COM_POINT};
-	struct component pseudo = {.type = COM_POINT};
-	struct component bot = {.type = COM_LINE};
 
-	struct component symmetry = {.type = COM_LINE};
+	struct component right_corner = {.type = COM_POINT};
+	struct component below_right_hole_corner = {.type = COM_POINT};
+	struct component right_hole_bot = {.type = COM_POINT};
+	struct component right_hole_mid = {.type = COM_POINT};
+	struct component right_hole_top = {.type = COM_POINT};
 
-	struct component right_lower = {.type = COM_LINE};
-	struct component left_lower = {.type = COM_LINE};
+	struct component swoop_top = {.type = COM_POINT};
+	struct component swoop_mid = {.type = COM_POINT};
 
-	struct component right_under_hole_mid = {.type = COM_POINT};
+	struct component left_hole_top = {.type = COM_POINT};
+	struct component left_hole_mid = {.type = COM_POINT};
+	struct component left_hole_bot = {.type = COM_POINT};
+	struct component below_left_hole_corner = {.type = COM_POINT};
+	struct component left_corner = {.type = COM_POINT};
 
-	struct component right_hole_bot_tan = {.type = COM_LINE};
-	struct component left_hole_bot_tan = {.type = COM_LINE};
-	struct component right_hole_top_tan = {.type = COM_LINE};
-	struct component left_hole_top_tan = {.type = COM_LINE};
+	struct component symmetry = {.type = COM_LINE};
+	struct component bottom = {.type = COM_LINE};
 
-	struct component right_hole_mid = {.type = COM_POINT};
-	struct component left_hole_mid = {.type = COM_POINT};
+	struct component right_edge = {.type = COM_LINE};
+	struct component right_bottom_hole_tan = {.type = COM_LINE};
+	struct component left_edge = {.type = COM_LINE};
 
-	struct component swoop_mid = {.type = COM_POINT};
-	struct component swoop_top_tan = {.type = COM_POINT};
+	struct component up_tan_axis = {.type = COM_LINE};
+
+	struct component right_inner_hole_bisect = {.type = COM_LINE};
+	struct component right_inner_hole_top = {.type = COM_POINT};
+	struct component right_inner_hole_bot = {.type = COM_POINT};
+	struct component left_inner_hole_bisect = {.type = COM_LINE};
+	struct component left_inner_hole_top = {.type = COM_POINT};
+	struct component left_inner_hole_bot = {.type = COM_POINT};
 
-	struct component left_bend = {.type = COM_POINT};
-	struct component lb2 = {.type = COM_POINT};
 
-	struct component l1 = {.type = COM_LINE};
+	struct component right_hole_line = {.type = COM_LINE};
 
 	add_constraint(&constraints, (struct constraint[]){
-		PP_DISTANCE(&mid_bot, &pseudo, 10),
-		POINT_ON_LINE(&mid_bot, &bot),
-		POINT_ON_LINE(&pseudo, &bot),
+		PP_DISTANCE(&mid_bot, &right_corner, 35),
+		POINT_ON_LINE(&mid_bot, &bottom),
+		POINT_ON_LINE(&right_corner, &bottom),
 
-		LL_ANGLE(&bot, &symmetry, DEG(90)),
+		LL_ANGLE(&bottom, &symmetry, DEG(90)),
 		POINT_ON_LINE(&mid_bot, &symmetry),
 
-		// @HACK This is going in the wrong direction?
-		PL_DISTANCE(&mid_bot, &right_lower, 35),
-		LL_ANGLE(&bot, &right_lower, DEG(-90)),
-		PL_DISTANCE(&left_lower, &mid_bot, 35),
-		LL_ANGLE(&bot, &left_lower, DEG(90)),
-
-		PL_DISTANCE(&right_hole_mid, &bot, 80),
-		PL_DISTANCE(&left_hole_mid, &bot, 80),
+		PL_DISTANCE(&right_hole_mid, &bottom, 80),
 		PL_DISTANCE(&right_hole_mid, &symmetry, -60),
-		PL_DISTANCE(&left_hole_mid, &symmetry, 60),
 
-		PL_DISTANCE(&right_under_hole_mid, &right_lower, 20),
-		PL_DISTANCE(&right_under_hole_mid, &right_hole_bot_tan, 20),
+		LL_ANGLE(&bottom, &right_edge, DEG(90)),
+		// LL_ANGLE(&bottom, &left_edge, DEG(-90)),
 
-		LL_ANGLE(&right_hole_bot_tan, &symmetry, DEG(270)),
+		POINT_ON_LINE(&right_corner, &right_edge),
+		//POINT_ON_LINE(&left_corner, &left_edge),
 
-		PL_DISTANCE(&right_hole_mid, &right_hole_bot_tan, -28),
-		// PL_DISTANCE(&left_hole_mid, &left_hole_bot_tan, 56),
-		PL_DISTANCE(&right_hole_mid, &right_hole_top_tan, 28),
-		// PL_DISTANCE(&left_hole_mid, &left_hole_top_tan, 56),
+		LL_ANGLE(&symmetry, &right_bottom_hole_tan, DEG(-90)),
+		PL_DISTANCE(&right_hole_mid, &right_bottom_hole_tan, -28),
+
+		POINT_ON_LINE(&below_right_hole_corner, &right_edge),
+		POINT_ON_LINE(&below_right_hole_corner, &right_bottom_hole_tan),
+
+		POINT_ON_LINE(&right_hole_bot, &right_bottom_hole_tan),
+		PP_DISTANCE(&right_hole_mid, &right_hole_bot, 28),
 
 		POINT_ON_LINE(&swoop_mid, &symmetry),
-		PP_DISTANCE(&swoop_mid, &right_hole_mid, 120 - 28),
-		// PL_DISTANCE(&swoop_mid, &swoop_top_tan, 120),
-		PL_DISTANCE(&swoop_mid, &right_hole_top_tan, 120),
+
+		PP_DISTANCE(&swoop_mid, &right_hole_top, 120),
+		PP_DISTANCE(&right_hole_mid, &right_hole_top, 28),
+
+		POINT_ON_LINE(&right_hole_mid, &right_hole_line),
+		POINT_ON_LINE(&right_hole_top, &right_hole_line),
+		POINT_ON_LINE(&swoop_mid, &right_hole_line),
+
+		POINT_ON_LINE(&swoop_top, &symmetry),
+		PP_DISTANCE(&swoop_top, &swoop_mid, 120),
+
+		// LL_ANGLE(&right_inner_hole_bisect, &bottom, DEG(90)),
+		// LL_ANGLE(&left_inner_hole_bisect, &bottom, DEG(-90)),
+		// POINT_ON_LINE(&right_hole_mid, &right_inner_hole_bisect),
+		// POINT_ON_LINE(&left_hole_mid, &left_inner_hole_bisect),
+		// PP_DISTANCE(&right_hole_mid, &right_inner_hole_top, 14),
+		// PP_DISTANCE(&right_hole_mid, &right_inner_hole_bot, -14),
+		// PP_DISTANCE(&left_hole_mid, &left_inner_hole_top, 14),
+		// PP_DISTANCE(&left_hole_mid, &left_inner_hole_bot, -14),
+		// POINT_ON_LINE(&right_inner_hole_top, &right_inner_hole_bisect),
+		// POINT_ON_LINE(&right_inner_hole_bot, &right_inner_hole_bisect),
+		// POINT_ON_LINE(&left_inner_hole_top, &left_inner_hole_bisect),
+		// POINT_ON_LINE(&left_inner_hole_bot, &left_inner_hole_bisect),
 
 
-		// Make a triangle
 		CEND(),
 	});
 
 	add_fragment(&topo, (struct topology_elem[]){
 		TOPO_MOVETO(&mid_bot),
-		TOPO_LINETO(&right_hole_mid),
-
-		TOPO_MOVETO(&mid_bot),
+		TOPO_LINETO(&right_corner),
+		TOPO_LINETO(&below_right_hole_corner),
+		TOPO_LINETO(&right_hole_bot),
+		TOPO_ARCTO(&right_hole_mid, &right_hole_top),
+
+		TOPO_ARCTO(&swoop_mid, &swoop_top),
+		TOPO_ARCTO(&swoop_mid, &left_hole_top),
+		TOPO_ARCTO(&left_hole_mid, &left_hole_bot),
+
+		TOPO_LINETO(&below_left_hole_corner),
+		TOPO_LINETO(&left_corner),
+		TOPO_LINETO(&mid_bot),
+
+		TOPO_MOVETO(&right_inner_hole_bot),
+		TOPO_ARCTO(&right_hole_mid, &right_inner_hole_top),
+		TOPO_ARCTO(&right_hole_mid, &right_inner_hole_bot),
+
+		TOPO_MOVETO(&left_inner_hole_bot),
+		TOPO_ARCTO(&left_hole_mid, &left_inner_hole_top),
+		TOPO_ARCTO(&left_hole_mid, &left_inner_hole_bot),
 		// TOPO_LINETO(&p2),
 		// TOPO_LINETO(&p3),
 		TOPO_END(),
diff --git a/src/solve.c b/src/solve.c
index 66cbb7f..e3c456b 100644
--- a/src/solve.c
+++ b/src/solve.c
@@ -806,18 +806,68 @@ constraint_matches_j:
 					;
 
 					constraint = c;
+					goto constraint_found;
 				}
+				continue;
+constraint_found:
+				;
 
 				// Here we have two assemblies, one fixed and the other not,
 				// that share a single point and each one other point that
 				// share a constraint. Try place the rigid body based on that
 				// information
 
-				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;
-
-				constraint->used = true;
-				theta += constraint->forward ? constraint->v : -constraint->v;
+				float theta;
+				if(constraint->type == CT_LINE_LINE_ANGLE) {
+					assert(constraint->c1->e->type == ETYPE_LINE);
+					assert(constraint->c2->e->type == ETYPE_LINE);
+
+					// Align the two lines
+					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;
+
+					// Then rotate by whatever the constraint says
+					theta += constraint->forward ? constraint->v : -constraint->v;
+				} else if(constraint->type == CT_POINT_LINE_DISTANCE) {
+					assert(constraint->c1->e->type == ETYPE_POINT);
+					assert(constraint->c2->e->type == ETYPE_LINE);
+
+					struct line line = constraint->c2->e->line;
+					float norm_len = glm_vec2_norm(line.norm);
+					float beta = atan2(line.norm[1], line.norm[0]);
+					float v_signed = constraint->forward ? constraint->v : -constraint->v;
+
+					if(forward) {
+						// Point (c1) is in assembly i (unfixed), line (c2) is in assembly j (fixed)
+						vec2 v_src;
+						glm_vec2_sub(constraint->c1->e->point.pos, assemblies[i].articulation_position[articulation_i]->point.pos, v_src);
+						float r = glm_vec2_norm(v_src);
+						float alpha = atan2(v_src[1], v_src[0]);
+
+						float d_pivot = (glm_vec2_dot(line.norm, assemblies[j].articulation_position[articulation_j]->point.pos) + line.C) / norm_len;
+						float cos_val = (v_signed - d_pivot) / r;
+						if(cos_val > 1.0f) cos_val = 1.0f;
+						if(cos_val < -1.0f) cos_val = -1.0f;
+
+						theta = beta - alpha + acos(cos_val);
+					} else {
+						// Line (c2) is in assembly i (unfixed), point (c1) is in assembly j (fixed)
+						vec2 v_ext;
+						glm_vec2_sub(constraint->c1->e->point.pos, assemblies[j].articulation_position[articulation_j]->point.pos, v_ext);
+						float r = glm_vec2_norm(v_ext);
+						float alpha_ext = atan2(v_ext[1], v_ext[0]);
+
+						float d_pivot_i = (glm_vec2_dot(line.norm, assemblies[i].articulation_position[articulation_i]->point.pos) + line.C) / norm_len;
+						float cos_val = (v_signed - d_pivot_i) / r;
+						if(cos_val > 1.0f) cos_val = 1.0f;
+						if(cos_val < -1.0f) cos_val = -1.0f;
+
+						theta = alpha_ext - beta + acos(cos_val);
+					}
+				} else {
+					abort();
+				}
+				constraint->used = i+1;
 
 				assert(assemblies[i].articulation_position[articulation_i]->type == ETYPE_POINT);
 				assert(assemblies[j].articulation_position[articulation_j]->type == ETYPE_POINT);
@@ -825,13 +875,13 @@ constraint_matches_j:
 				mat3 transform;
 				glm_mat3_identity(transform);
 
-				glm_translate2d(transform, assemblies[i].articulation_position[articulation_i]->point.pos);
+				glm_translate2d(transform, assemblies[j].articulation_position[articulation_j]->point.pos);
 
 				glm_rotate2d(transform, theta);
 
 				{
 					vec2 negative_translate;
-					glm_vec2_negate_to(assemblies[j].articulation_position[articulation_j]->point.pos, negative_translate);
+					glm_vec2_negate_to(assemblies[i].articulation_position[articulation_i]->point.pos, negative_translate);
 					glm_translate2d(transform, negative_translate);
 				}
 
@@ -888,6 +938,7 @@ constraint_matches_j:
 						abort();
 					}
 				}
+				assemblies[i].fixed = true;
 			}
 		}
 		break;
diff --git a/src/svg.c b/src/svg.c
index a59f57b..6c7533e 100644
--- a/src/svg.c
+++ b/src/svg.c
@@ -369,7 +369,7 @@ void begin_drawing(struct canvas *canvas) {
 	assert(canvas->f != NULL);
 	assert(canvas->state == CANVAS_INIT);
 
-	fprintf(canvas->f, "<svg version=\"1.1\" viewBox=\"-100 -100 200 200\" width=\"1200\" height=\"1200\" xmlns=\"http://www.w3.org/2000/svg\">\n");
+	fprintf(canvas->f, "<svg version=\"1.1\" viewBox=\"-100 -190 200 200\" width=\"1200\" height=\"1200\" xmlns=\"http://www.w3.org/2000/svg\">\n");
 	fprintf(canvas->f, "<defs>\n");
 	fprintf(canvas->f, "\t<marker id=\"triangle\" viewBox=\"0 0 10 10\" refX=\"10\" refY=\"5\" markerUnits=\"strokeWidth\" markerWidth=\"6\" markerHeight=\"6\" orient=\"auto-start-reverse\">\n");
 	fprintf(canvas->f, "\t\t<path d=\"M 0 0 L 10 5 L 0 10 z\" fill=\"blue\" opacity=\"0.3\" />\n");