29 #include <Eigen/Dense>
36 #define PP4x2_VECSIZE 7
38 #define DIXON_RESULTANT_SIZE 3
44 namespace kinematic_closure {
47 Eigen::IOFormat
scipy(8, 0,
", ",
",\n",
"[",
"]",
"[",
"]");
50 typedef Eigen::Matrix<Real, 8, 8>
Matrix8;
52 typedef Eigen::Matrix<Real, Eigen::Dynamic, 1, 0, 16, 1>
Vector16;
70 for (
unsigned i=1; i<=t.size(); i++ ) {
71 C[i] = A[1]+t[i] * (A[2]+t[i]*A[3]);
79 for (
unsigned i=1; i<=t.size(); i++ ) {
80 C[i] = A[1]+t[i] * (A[2] + t[i] * (A[3] + t[i] * (A[4] + t[i]*A[5])));
88 for (
unsigned i=1; i<=t.size(); i++ ) {
89 C[i] = B[1]+t[i]*(B[2]+t[i]*(B[3]+t[i]*(B[4]+t[i]*(B[5]+t[i]*(B[6]+t[i]*B[7])))));
97 for (
unsigned i=1; i<=t.size(); i++ ) {
98 C[i] = A[1]+t[i]*(A[2]+t[i]*(A[3]+t[i]*(A[4]+t[i]*(A[5]+t[i]*(A[6]+t[i]*(A[7]+t[i]*(A[8]+t[i]*A[9])))))));
106 for (
unsigned i=1; i<=t.size(); i++ ) {
107 C[i] = A[1]+t[i]*(A[2]+t[i]*(A[3]+t[i]*(A[4]+t[i]*(A[5]+t[i]*(A[6]+t[i]*(A[7]+t[i]*(A[8]+t[i]*(A[9]+t[i]*(A[10]+t[i]*(A[11]+t[i]*(A[12]+t[i]*(A[13]+t[i]*(A[14]+t[i]*(A[15]+t[i]*(A[16]+t[i]*A[17])))))))))))))));
117 C[4] = A[3]*B[2]+A[2]*B[3];
118 C[3] = A[3]*B[1]+A[2]*B[2]+A[1]*B[3];
119 C[2] = A[2]*B[1]+A[1]*B[2];
129 C[6] = A[5]*B[2]+A[4]*B[3];
130 C[5] = A[5]*B[1]+A[4]*B[2]+A[3]*B[3];
131 C[4] = A[4]*B[1]+A[3]*B[2]+A[2]*B[3];
132 C[3] = A[3]*B[1]+A[2]*B[2]+A[1]*B[3];
133 C[2] = A[2]*B[1]+A[1]*B[2];
143 C[8] = A[5]*B[4]+A[4]*B[5];
144 C[7] = A[5]*B[3]+A[4]*B[4]+A[3]*B[5];
145 C[6] = A[5]*B[2]+A[4]*B[3]+A[3]*B[4]+A[2]*B[5];
146 C[5] = A[5]*B[1]+A[4]*B[2]+A[3]*B[3]+A[2]*B[4]+A[1]*B[5];
147 C[4] = A[4]*B[1]+A[3]*B[2]+A[2]*B[3]+A[1]*B[4];
148 C[3] = A[3]*B[1]+A[2]*B[2]+A[1]*B[3];
149 C[2] = A[2]*B[1]+A[1]*B[2];
160 C[7] = A[4]*A[4] + 2* A[5]*A[3];
161 C[6] = 2*(A[5]*A[2]+A[4]*A[3]);
162 C[5] = A[3]*A[3] + 2*(A[5]*A[1]+A[4]*A[2]);
163 C[4] = 2*(A[4]*A[1]+A[3]*A[2]);
164 C[3] = A[2]*A[2] + 2* A[3]*A[1];
175 C[12] = A[7]*B[6]+A[6]*B[7];
176 C[11] = A[7]*B[5]+A[6]*B[6]+A[5]*B[7];
177 C[10] = A[7]*B[4]+A[6]*B[5]+A[5]*B[6]+A[4]*B[7];
178 C[9] = A[7]*B[3]+A[6]*B[4]+A[5]*B[5]+A[4]*B[6]+A[3]*B[7];
179 C[8] = A[7]*B[2]+A[6]*B[3]+A[5]*B[4]+A[4]*B[5]+A[3]*B[6]+A[2]*B[7];
180 C[7] = A[7]*B[1]+A[6]*B[2]+A[5]*B[3]+A[4]*B[4]+A[3]*B[5]+A[2]*B[6]+A[1]*B[7];
181 C[6] = A[6]*B[1]+A[5]*B[2]+A[4]*B[3]+A[3]*B[4]+A[2]*B[5]+A[1]*B[6];
182 C[5] = A[5]*B[1]+A[4]*B[2]+A[3]*B[3]+A[2]*B[4]+A[1]*B[5];
183 C[4] = A[4]*B[1]+A[3]*B[2]+A[2]*B[3]+A[1]*B[4];
184 C[3] = A[3]*B[1]+A[2]*B[2]+A[1]*B[3];
185 C[2] = A[2]*B[1]+A[1]*B[2];
195 C[16] = A[13]*B[4]+A[12]*B[5];
196 C[15] = A[13]*B[3]+A[12]*B[4]+A[11]*B[5];
197 C[14] = A[13]*B[2]+A[12]*B[3]+A[11]*B[4]+A[10]*B[5];
198 C[13] = A[13]*B[1]+A[12]*B[2]+A[11]*B[3]+A[10]*B[4]+A[9]*B[5];
199 C[12] = A[12]*B[1]+A[11]*B[2]+A[10]*B[3]+A[ 9]*B[4]+A[8]*B[5];
200 C[11] = A[11]*B[1]+A[10]*B[2]+A[ 9]*B[3]+A[ 8]*B[4]+A[7]*B[5];
201 C[10] = A[10]*B[1]+A[ 9]*B[2]+A[ 8]*B[3]+A[ 7]*B[4]+A[6]*B[5];
202 C[ 9] = A[ 9]*B[1]+A[ 8]*B[2]+A[ 7]*B[3]+A[ 6]*B[4]+A[5]*B[5];
203 C[ 8] = A[ 8]*B[1]+A[ 7]*B[2]+A[ 6]*B[3]+A[ 5]*B[4]+A[4]*B[5];
204 C[ 7] = A[ 7]*B[1]+A[ 6]*B[2]+A[ 5]*B[3]+A[ 4]*B[4]+A[3]*B[5];
205 C[ 6] = A[ 6]*B[1]+A[ 5]*B[2]+A[ 4]*B[3]+A[ 3]*B[4]+A[2]*B[5];
206 C[ 5] = A[ 5]*B[1]+A[ 4]*B[2]+A[ 3]*B[3]+A[ 2]*B[4]+A[1]*B[5];
207 C[ 4] = A[ 4]*B[1]+A[ 3]*B[2]+A[ 2]*B[3]+A[ 1]*B[4];
208 C[ 3] = A[ 3]*B[1]+A[ 2]*B[2]+A[ 1]*B[3];
209 C[ 2] = A[ 2]*B[1]+A[ 1]*B[2];
219 C[16] = A[9]*B[8]+A[8]*B[9];
220 C[15] = A[9]*B[7]+A[8]*B[8]+A[7]*B[9];
221 C[14] = A[9]*B[6]+A[8]*B[7]+A[7]*B[8]+A[6]*B[9];
222 C[13] = A[9]*B[5]+A[8]*B[6]+A[7]*B[7]+A[6]*B[8]+A[5]*B[9];
223 C[12] = A[9]*B[4]+A[8]*B[5]+A[7]*B[6]+A[6]*B[7]+A[5]*B[8]+A[4]*B[9];
224 C[11] = A[9]*B[3]+A[8]*B[4]+A[7]*B[5]+A[6]*B[6]+A[5]*B[7]+A[4]*B[8]+A[3]*B[9];
225 C[10] = A[9]*B[2]+A[8]*B[3]+A[7]*B[4]+A[6]*B[5]+A[5]*B[6]+A[4]*B[7]+A[3]*B[8]+A[2]*B[9];
226 C[ 9] = A[9]*B[1]+A[8]*B[2]+A[7]*B[3]+A[6]*B[4]+A[5]*B[5]+A[4]*B[6]+A[3]*B[7]+A[2]*B[8]+A[1]*B[9];
227 C[ 8] = A[8]*B[1]+A[7]*B[2]+A[6]*B[3]+A[5]*B[4]+A[4]*B[5]+A[3]*B[6]+A[2]*B[7]+A[1]*B[8];
228 C[ 7] = A[7]*B[1]+A[6]*B[2]+A[5]*B[3]+A[4]*B[4]+A[3]*B[5]+A[2]*B[6]+A[1]*B[7];
229 C[ 6] = A[6]*B[1]+A[5]*B[2]+A[4]*B[3]+A[3]*B[4]+A[2]*B[5]+A[1]*B[6];
230 C[ 5] = A[5]*B[1]+A[4]*B[2]+A[3]*B[3]+A[2]*B[4]+A[1]*B[5];
231 C[ 4] = A[4]*B[1]+A[3]*B[2]+A[2]*B[3]+A[ 1]*B[4];
232 C[ 3] = A[3]*B[1]+A[2]*B[2]+A[1]*B[3];
233 C[ 2] = A[2]*B[1]+A[1]*B[2];
243 C[16] = 2* A[9]*A[8];
244 C[15] = A[8]*A[8] + 2* A[9]*A[7];
245 C[14] = 2*(A[9]*A[6]+A[8]*A[7]);
246 C[13] = A[7]*A[7] + 2*(A[9]*A[5]+A[8]*A[6]);
247 C[12] = 2*(A[9]*A[4]+A[8]*A[5]+A[7]*A[6]);
248 C[11] = A[6]*A[6] + 2*(A[9]*A[3]+A[8]*A[4]+A[7]*A[5]);
249 C[10] = 2*(A[9]*A[2]+A[8]*A[3]+A[7]*A[4]+A[6]*A[5]);
250 C[ 9] = A[5]*A[5] + 2*(A[9]*A[1]+A[8]*A[2]+A[7]*A[3]+A[6]*A[4]);
251 C[ 8] = 2*(A[8]*A[1]+A[7]*A[2]+A[6]*A[3]+A[5]*A[4]);
252 C[ 7] = A[4]*A[4] + 2*(A[7]*A[1]+A[6]*A[2]+A[5]*A[3]);
253 C[ 6] = 2*(A[6]*A[1]+A[5]*A[2]+A[4]*A[3]);
254 C[ 5] = A[3]*A[3] + 2*(A[5]*A[1]+A[4]*A[2]);
255 C[ 4] = 2*(A[4]*A[1]+A[3]*A[2]);
256 C[ 3] = A[2]*A[2] + 2* A[3]*A[1];
257 C[ 2] = 2* A[2]*A[1];
268 for (
unsigned i=1; i<=A.size(); i++ ) {
285 for (
int i=1; i<=3; i++ ) {
380 Matrix8 * R[3] = {&R0, &R1, &R2};
382 for (
int i = 1; i <= 3; i++ ) {
386 Ri(0, 1) = A[1][i]; Ri(0, 2) = A[2][i]; Ri(0, 3) = A[3][i];
387 Ri(1, 0) = A[1][i]; Ri(1, 1) = A[2][i]; Ri(1, 2) = A[3][i];
389 Ri(0, 5) = B[1][i]; Ri(0, 6) = B[2][i]; Ri(0, 7) = B[3][i];
390 Ri(1, 4) = B[1][i]; Ri(1, 5) = B[2][i]; Ri(1, 6) = B[3][i];
391 Ri(2, 1) = B[1][i]; Ri(2, 2) = B[2][i]; Ri(2, 3) = B[3][i];
392 Ri(3, 0) = B[1][i]; Ri(3, 1) = B[2][i]; Ri(3, 2) = B[3][i];
394 Ri(2, 5) = C[1][i]; Ri(2, 6) = C[2][i]; Ri(2, 7) = C[3][i];
395 Ri(3, 4) = C[1][i]; Ri(3, 5) = C[2][i]; Ri(3, 6) = C[3][i];
397 Ri(4, 5) = D[1][i]; Ri(4, 6) = D[2][i]; Ri(4, 7) = D[3][i];
398 Ri(5, 4) = D[1][i]; Ri(5, 5) = D[2][i]; Ri(5, 6) = D[3][i];
399 Ri(6, 1) = D[1][i]; Ri(6, 2) = D[2][i]; Ri(6, 3) = D[3][i];
400 Ri(7, 0) = D[1][i]; Ri(7, 1) = D[2][i]; Ri(7, 2) = D[3][i];
411 int num_solutions = u.size();
412 double u_squared, u_squared_plus_1;
414 cos.resize(num_solutions);
415 sin.resize(num_solutions);
417 for (
int i = 1; i <= num_solutions; i++ ) {
421 for (
int j = 1; j <= 3; j++ ) {
422 u_squared = u[i][j] * u[i][j];
423 u_squared_plus_1 = u_squared + 1;
425 cos[i][j] = (1 - u_squared) / u_squared_plus_1;
426 sin[i][j] = 2 * u[i][j] / u_squared_plus_1;
440 int & num_solutions) {
459 U.topRightCorner(8, 8) =
I;
460 U.bottomLeftCorner(8, 8) = -R0;
461 U.bottomRightCorner(8, 8) = -R1;
463 V.topLeftCorner(8, 8) =
I;
464 V.bottomRightCorner(8, 8) = R2;
473 u.resize(num_solutions);
475 for (
int i = 0; i < num_solutions; i++ ) {
478 if (
std::abs(eigenvectors(0, i)) > 1e-8 ) {
479 u[i+1][1] = eigenvectors(1, i) / eigenvectors(0, i);
480 u[i+1][2] = eigenvectors(4, i) / eigenvectors(0, i);
482 u[i+1][1] = eigenvectors(3, i) / eigenvectors(2, i);
483 u[i+1][2] = eigenvectors(6, i) / eigenvectors(2, i);
486 u[i+1][3] = eigenvalues(i);
503 using namespace numeric::kinematic_closure;
506 utility::vector1<Real> A0D1, A1D0, A1D2, A2D1, A0D2, A2D0, B0D1, B0D2, B1D0, B1D2, B2D0, B2D1, C0D1, C0D2, C1D0, C1D2, C2D0, C2D1, D0D2;
520 utility::vector1<Real> cram22, cram32, cram42, cram52, cram122, cram132, cram222, cram232, cram242, cram252;
530 Real tol_secant=1.0e-15;
531 int max_iter_sturm=100, max_iter_secant=20;
642 Det678[i]=(-C0Det23[i] + C1Det22[i] - C2Det21[i]);
643 Det123[i]=( A0Det23[i] - A1Det22[i] + A2Det21[i]);
684 for (
unsigned i=1; i<=ddet.size(); i++ ) {
685 ddet[i] = (-2*D1235_4678[i] + D1256_3478[i] - D1257_3468[i] - D1356_2478[i] + D1357_2468[i]);
693 for (
unsigned i=1; i<=ddet.size(); i++ ) {
694 Q[i]=ddet[i] / ddet[17];
700 if ( nsol==0 )
return;
713 for (
int i=1; i<=nsol; i++ ) {
715 tau[i][order[3]]=roots[i];
729 for (
int i=1; i<=nsol; i++ ) {
730 cram11[i] = pdet23[i]*pd0d2[i];
736 for (
int i=1; i<=nsol; i++ ) {
737 cram21[i] = -pdet31[i]*pd2[i];
742 for (
int i=1; i<=nsol; i++ ) {
743 cram31[i] = -pdet32[i]*pd2[i];
748 for (
int i=1; i<=nsol; i++ ) {
749 cram41[i] = -pdet31[i]*pd3[i];
753 for (
int i=1; i<=nsol; i++ ) {
754 cram51[i] = -pdet32[i]*pd3[i];
758 for (
int i=1; i<=nsol; i++ ) {
759 crden[i] = -cram11[i]*cram12[i] + cram21[i]*cram22[i] - cram31[i]*cram32[i] - cram41[i]*cram42[i] + cram51[i]*cram52[i];
768 for (
int i=1; i<=nsol; i++ ) {
769 crn111[i] = pdet22[i]*pd0d2[i];
774 for (
int i=1; i<=nsol; i++ ) {
775 crn121[i] = -pdet31[i]*pd1[i];
779 for (
int i=1; i<=nsol; i++ ) {
780 crn131[i] = -pdet32[i]*pd1[i];
783 cram_num1.resize(nsol);
784 for (
int i=1; i<=nsol; i++ ) {
785 cram_num1[i] = -crn111[i]*crn112[i]+crn121[i]*crn122[i]-crn131[i]*crn132[i];
786 tau[i][order[1]] = -cram_num1[i] / crden[i];
795 for (
int i=1; i<=nsol; i++ ) {
796 crn221[i] = -pdet21[i]*pd2[i];
800 for (
int i=1; i<=nsol; i++ ) {
801 crn231[i] = -pdet21[i]*pd3[i];
805 for (
int i=1; i<=nsol; i++ ) {
806 crn241[i] = -pdet22[i]*pd2[i];
810 for (
int i=1; i<=nsol; i++ ) {
811 crn251[i] = -pdet22[i]*pd3[i];
816 for (
int i=1; i<=nsol; i++ ) {
817 crn261[i] = -pdet33[i]*pd0d2[i];
820 cram_num2.resize(nsol);
821 for (
int i=1; i<=nsol; i++ ) {
822 cram_num2[i] = crn221[i]*crn222[i] - crn231[i]*crn232[i] - crn241[i]*crn242[i] + crn251[i]*crn252[i] - crn261[i]*crn262[i];
823 tau[i][order[2]] = -cram_num2[i] / crden[i];
833 for (
int i=1; i<=nsol; i++ ) {
838 for (
int j=1; j<=3; j++ ) {
839 tsq[i][j]=(tau[i][j])*(tau[i][j]);
840 tsq1[i][j]=tsq[i][j]+1;
841 cos[i][j]=(1-tsq[i][j]) / tsq1[i][j];
842 sin[i][j]=2 * tau[i][j] / tsq1[i][j];
953 A[1]=15.999410431075338;
955 A[3]=-8.999904923464781;
956 B[1]=72.015965384650187;
958 B[3]=7.000641402062060;
967 Real Avals[]={-0.061472096577788, -2.424098126176366, -0.016332287075674, 2.421954432062721, -0.090242315482933, 0.643479843225995, 0.061472096577788, -0.646358402699405, 0.016332287075674};
968 Real Bvals[]={-0.000625561301665, 0.0, 1.070810990722253, 0.054344479784912, 0.0, 0.054308028868740, -1.071191403371629, 0.0, 0.000963218011333};
969 Real Cvals[]={0.063394336809916, 2.498922707477288, -0.025585174231195, -2.497689253369826, 0.177963037883907, 1.008036647097698, -0.063394336809916, -1.006882302775246, 0.025585174231195};
970 Real Dvals[]={0.673457026339820, -0.015821144672541, 1.005285612756159, -0.078123031130501, 3.277064399971811, 0.078123031130501, 1.004894243686905, 0.075272539656448, -0.573852487168651};
983 for (
int i=1; i<=3; i++ ) {
992 for (
int i=1; i<=3; i++ ) {
993 for (
int j=1; j<=3; j++ ) {
1010 cout <<
"number of solutions" << nsol << std::endl;
void initialize_sturm(double *tol_secant, int *max_iter_sturm, int *max_iter_secant)
vector1< PseudoVector > PseudoMatrix
void test_polyProduct4x2()
void point_value8(const utility::vector1< Real > &A, const utility::vector1< Real > &t, utility::vector1< Real > &C)
void polyProduct4sq(const utility::vector1< Real > &A, utility::vector1< Real > &C)
void polyProduct2x2(const utility::vector1< Real > &A, const utility::vector1< Real > &B, utility::vector1< Real > &C)
Eigen::Matrix< Real, 8, 8 > Matrix8
void solve_sturm(const int &p_order, int &n_root, const utility::vector1< double > &poly_coeffs, utility::vector1< double > &roots)
void polyProduct6x6(const utility::vector1< Real > &A, const utility::vector1< Real > &B, utility::vector1< Real > &C)
Header file for GeneralizedEigenSolver.
Eigen::Matrix< Real, Eigen::Dynamic, 1, 0, 16, 1 > Vector16
int num_real_solutions() const
Return the number of real solutions that were calculated.
implements sturm chain solver
void polyProduct8x8(const utility::vector1< Real > &A, const utility::vector1< Real > &B, utility::vector1< Real > &C)
void test_polyProduct4x4()
vector1< Real > PseudoVector
void point_value4(const utility::vector1< Real > &A, const utility::vector1< Real > &t, utility::vector1< Real > &C)
Eigen::Matrix< RealScalar, Eigen::Dynamic, 1, Options, MaxColsAtCompileTime, 1 > RealEigenvalueType
Vector type used to represent the real subset of calculated eigenvalues.
void polyProduct4x2(const utility::vector1< Real > &A, const utility::vector1< Real > &B, utility::vector1< Real > &C)
void dixonResultant(const utility::vector1< utility::vector1< Real > > &A, const utility::vector1< utility::vector1< Real > > &B, const utility::vector1< utility::vector1< Real > > &C, const utility::vector1< utility::vector1< Real > > &D, utility::vector1< utility::vector1< utility::vector1< Real > > > &R)
void point_value6(const utility::vector1< Real > &B, const utility::vector1< Real > &t, utility::vector1< Real > &C)
RealEigenvalueType real_eigenvalues() const
Return any real eigenvalues that were calculated.
void vectorDiff(const utility::vector1< Real > &A, const utility::vector1< Real > &B, utility::vector1< Real > &C)
void printMatrix(const utility::vector1< utility::vector1< Real > > &M)
prints the matrix
T abs(T const &x)
std::abs( x ) == | x |
std::vector with 1-based indexing
Header file for dixon code for ALC.
#define DIXON_RESULTANT_SIZE
Solves generalized eigenvalue problems.
Eigen::Matrix< Real, 16, 16 > Matrix16
rosetta project type declarations. Should be kept updated with core/types.hh. This exists because num...
void dixon_eig(PseudoMatrix const &A, PseudoMatrix const &B, PseudoMatrix const &C, PseudoMatrix const &D, vector1< int > const &, PseudoMatrix &cos, PseudoMatrix &sin, PseudoMatrix &u, int &num_solutions)
void polyProduct4x4(const utility::vector1< Real > &A, const utility::vector1< Real > &B, utility::vector1< Real > &C)
void point_value2(const utility::vector1< Real > &A, const utility::vector1< Real > &t, utility::vector1< Real > &C)
dixon functions ///
void test_polyProduct6x6()
ocstream cout(std::cout)
Wrapper around std::cout.
void point_value16(const utility::vector1< Real > &A, const utility::vector1< Real > &t, utility::vector1< Real > &C)
void printVector(const utility::vector1< Real > &V)
helper functions ///
Eigen::IOFormat scipy(8, 0,", ",",\n","[","]","[","]")
RealEigenvectorType real_eigenvectors() const
Return any real eigenvectors that were calculated.
void polyProduct8sq(const utility::vector1< Real > &A, utility::vector1< Real > &C)
void build_dixon_matrices(PseudoMatrix const &A, PseudoMatrix const &B, PseudoMatrix const &C, PseudoMatrix const &D, Matrix8 &R0, Matrix8 &R1, Matrix8 &R2)
void test_polyProduct2x2()
linear_algebra::GeneralizedEigenSolver< Matrix16 > SolverType
Eigen::Matrix< RealScalar, RowsAtCompileTime, Eigen::Dynamic, Options, MaxRowsAtCompileTime, MaxColsAtCompileTime > RealEigenvectorType
Matrix type used to represent the real subset of calculated eigenvectors.
Header file for kinematic_closure_helpers.cc.
void polyProduct12x4(const utility::vector1< Real > &A, const utility::vector1< Real > &B, utility::vector1< Real > &C)
void test_polyProduct4sq()
void build_sin_and_cos(PseudoMatrix const &u, PseudoMatrix &sin, PseudoMatrix &cos)
void dixon_sturm(const utility::vector1< utility::vector1< Real > > &A, const utility::vector1< utility::vector1< Real > > &B, const utility::vector1< utility::vector1< Real > > &C, const utility::vector1< utility::vector1< Real > > &D, const utility::vector1< int > &order, utility::vector1< utility::vector1< Real > > &cos, utility::vector1< utility::vector1< Real > > &sin, utility::vector1< utility::vector1< Real > > &tau, int &nsol)