Matrix Solver / Optimizer

Hi guys,


I'm working on rigid folding in origami style.

In used this paper to approach the issue.

By extracting a single folding vertex with triangular faces attached. Starting from one face I go counter clockwise around the vertex, adding the next piece by multiplying its rotation matrix with the previous one. The rotation matrices depend on the angles p and the axis a.

The paper states, that the condition for the very last piece to meet the first one and thereby closing the yellow gap is:

F(p1, p2, ..... ,pn) = R1 * R2 * ..... * Rn = I

where R are the rotation matrices of each piece and
I being the identity matrix

I put that in a C# component, where I took random values of the angles p to rotate.

I can retrieve the end product of my rotation matrices, which (of course) is not the identity matrix I.

My question is, if you know of a library or a method how to optimize the rotation matrices depending on p only in C#. So that, when I change one p, the solver / optimizer can adjust the other angles.

I'd also like to add constraints, e.g. p should be as small as possible.


 
I've looked for matrix opimizers online as well, but since I'm very new to C#, I couldn't figure out, how to implement them, and if their even suitable to the issue.

I attached my current definition. Please also feel free to give hints on how to improve what I've done so far it. I guess it's not very clean, since I don't even know how to make my own public classes.

Any help would be super awesome.

Greets Benjamin