Physically, I'm trying to solve the position of points in a 2D mechanical linkage.
This is how I've been trying to go about solving it:
The constraints are defined as:
$||{\bf p}_i - {\bf p}_j||_2 = d_{ij}$
Which says the distance between points $i$ and $j$ must be equal to $d_{ij}$.
The above equation can be written in quadratic form:
${\bf q}_{ij}^T A {\bf q}_{ij} = d_{ij}$
where
$ {\bf q}_{ij} = \begin{bmatrix} x_i \\ y_i \\ x_j \\ y_j \\ \end{bmatrix} $
$ A = \begin{bmatrix} 1 & 0 & -1 & 0 \\ 0 & 1 & 0 & -1 \\ -1 & 0 & 1 & 0 \\ 0 & -1 & 0 & 1 \end{bmatrix} $
For each constraint between any 2 points, there is an equation of the form above. For example, this is the system of equations for 3 points where points 1 and 2 are distance contrainted and points 2 and 3 are distance constrained:
$ \begin{align} {\bf q}_{12}^T A {\bf q}_{12} = d_{12} \\ {\bf q}_{23}^T A {\bf q}_{23} = d_{23} \end{align} $
Notice that for N points, there can be at most $N(N - 1)/2$ edges, however, since this is a mechanical linkage system, there are always at least 3 degrees of freedom in the system. For example, 6 points (hexagon) completely connected to each other can still, as a group, be translated in 2 dimension and rotated. These 6 points have 15 constraints, and each point contributes an x and y dof, so there's 12 variables. Since 15 > 12, it looks like an overdeteremined system, but I think it's not really because some of those contraints are linear combinations of the others:
So one question I have is how do I change my system of equations to remove the contraints which are linear cominations? Something like gaussian elimination comes to mind, but I don't know how to do it on a system of quadtratic equations.
Now, suppose one of the points in the system is perturbed. Some or all of the other points must change their position to keep the contraints satisfied.
So I'm looking for a solution to this system of equations:
$ \begin{align} ||({\bf p}_i + {\bf s}_i) - ({\bf p}_j + {\bf s}_j)||_2 = d_{ij} \end{align} $
Where ${\bf s}_i$ is the correction to the point ${\bf p}_i$. Once found, applying these corrections to the points will move them to positions which satisfy the contraints.
Above I pointed out that the system is at worst underdetermined with there being at least 3 extra dof. So there's many solutions for ${\bf s}_i$.
To reduce the solution space to a finite number of solutions, I want to find the solution which causes the error of each constraint to go to 0 as fast as possible.
Define the error of each constraint:
$ e_{ij} = ||{\bf p}_i - {\bf p}_j||_2 - d_{ij} $
Define the global error:
$ E = \sum_i \sum_j e_{ij} $
Define the gradient of the global error:
$ \nabla E $
So I'm looking for a set of corrections $S = \{{\bf s}_1, {\bf s}_2, ..., {\bf s}_i\}$ where each correction causes each $e_{ij} = 0$, and this set $S$ is some how "parallel" to $\nabla E$.
I've seen some solutions saying to use force-directed graphs, but I don't want to do that because it would allow points to be in invalid positions with small error. I don't want any solutions involving velocities, impulses, or forces. No time derivatives.
Ideally, I would like a direct method, but I'm worried that really small errors would be amplified in the solution. For example, 3 points, fully connected, but the area of the triangle between them is very small (so all three points are in a line). In this case, if the middle point moves perpendicularly away from the line, the change in error would be very small. Iterative methods probably wouldn't correctly move the points back into being in a line.
I made a simulation which solves the constraints one by one. While solving one constraint it ignores the other constraints: https://gyazo.com/00cbaac2dad54d929dc288cd116ba397
This simulation is springy and I don't like that. I want it to be very rigid. And I think I need to solve the system of constraints at the same time to get it to behave rigidly.
That simulation works by setting each:
$ {\bf s}_{ij} = - e_{ij} \nabla e_{ij} $
Where
$ {\bf s}_{ij} = \begin{bmatrix} x_i \\ y_i \\ x_j \\ y_j \\ \end{bmatrix} $
I'm stuck here and don't know how to proceed. I don't know how to combine all these quadratic equations into a single statement. Setting the ${\bf s}$ to $- E \nabla E$ doesn't work because the error it allows $E = 0$ if $e_{12} = -e_{23}$. I not sure how to incorporate the requirement that each $e_{ij}$ must also be 0. I'm also not aware of any methods of solving sparse quadratic systems.