77 #ifndef JIXIE_IMPLICIT_QR_SVD_H
78 #define JIXIE_IMPLICIT_QR_SVD_H
126 inline void transposeInPlace()
145 T t = JIXIE::MATH_TOOLS::rsqrt(d);
165 T t = JIXIE::MATH_TOOLS::rsqrt(d);
173 template < class MatrixType>
174 inline void fill( const MatrixType& R) const
176 MatrixType& A = const_cast<MatrixType&>(R);
177 A = MatrixType::Identity();
191 template < class MatrixType>
194 for ( int j = 0; j < MatrixType::ColsAtCompileTime; j++) {
197 A(rowi, j) = c * tau1 - s * tau2;
198 A(rowk, j) = s * tau1 + c * tau2;
209 template < class MatrixType>
212 for ( int j = 0; j < MatrixType::RowsAtCompileTime; j++) {
215 A(j, rowi) = c * tau1 - s * tau2;
216 A(j, rowk) = s * tau1 + c * tau2;
225 T new_c = c * A.c - s * A.s;
226 T new_s = s * A.c + c * A.s;
253 inline void zeroChase(Eigen::Matrix<T, 3, 3>& H, Eigen::Matrix<T, 3, 3>& U, Eigen::Matrix<T, 3, 3>& V)
273 r2. compute(H(0, 0) * H(0, 1) + H(1, 0) * H(1, 1), H(0, 0) * H(0, 2) + H(1, 0) * H(1, 2));
310 inline void makeUpperBidiag(Eigen::Matrix<T, 3, 3>& H, Eigen::Matrix<T, 3, 3>& U, Eigen::Matrix<T, 3, 3>& V)
312 U = Eigen::Matrix<T, 3, 3>::Identity();
313 V = Eigen::Matrix<T, 3, 3>::Identity();
341 inline void makeLambdaShape(Eigen::Matrix<T, 3, 3>& H, Eigen::Matrix<T, 3, 3>& U, Eigen::Matrix<T, 3, 3>& V)
343 U = Eigen::Matrix<T, 3, 3>::Identity();
344 V = Eigen::Matrix<T, 3, 3>::Identity();
401 template < class TA, class T, class TS>
402 inline std::enable_if_t<isSize<TA>(2, 2) && isSize<TS>(2, 2)>
405 const Eigen::MatrixBase<TS>& S_Sym)
407 Eigen::Matrix<T, 2, 1> x(A(0, 0) + A(1, 1), A(1, 0) - A(0, 1));
408 T denominator = x.norm();
411 if (denominator != 0) {
416 R.c = x(0) / denominator;
417 R.s = -x(1) / denominator;
419 Eigen::MatrixBase<TS>& S = const_cast<Eigen::MatrixBase<TS>& >(S_Sym);
435 template < class TA, class TR, class TS>
436 inline std::enable_if_t<isSize<TA>(2, 2) && isSize<TR>(2, 2) && isSize<TS>(2, 2)>
438 const Eigen::MatrixBase<TR>& R,
439 const Eigen::MatrixBase<TS>& S_Sym)
441 using T = ScalarType<TA>;
454 template < class TA, class T, class Ts>
455 inline std::enable_if_t<isSize<TA>(2, 2) && isSize<Ts>(2, 1)>
457 const Eigen::MatrixBase<TA>& A,
459 const Eigen::MatrixBase<Ts>& Sigma,
461 const ScalarType<TA> tol = 64 * std::numeric_limits<ScalarType<TA> >::epsilon())
464 Eigen::MatrixBase<Ts>& sigma = const_cast<Eigen::MatrixBase<Ts>& >(Sigma);
466 Eigen::Matrix<T, 2, 2> S_Sym;
480 T tau = 0.5 * (x - z);
481 T w = sqrt(tau * tau + y * y);
492 cosine = T(1) / sqrt(t * t + T(1));
499 T c2 = cosine * cosine;
500 T csy = 2 * cosine * sine * y;
502 sigma(0) = c2 * x - csy + s2 * z;
503 sigma(1) = s2 * x + csy + c2 * z;
508 if (sigma(0) < sigma(1)) {
509 std::swap(sigma(0), sigma(1));
526 template < class TA, class TU, class Ts, class TV>
527 inline std::enable_if_t<isSize<TA>(2, 2) && isSize<TU>(2, 2) && isSize<TV>(2, 2) && isSize<Ts>(2, 1)>
529 const Eigen::MatrixBase<TA>& A,
530 const Eigen::MatrixBase<TU>& U,
531 const Eigen::MatrixBase<Ts>& Sigma,
532 const Eigen::MatrixBase<TV>& V,
533 const ScalarType<TA> tol = 64 * std::numeric_limits<ScalarType<TA> >::epsilon())
535 using T = ScalarType<TA>;
559 T d = (T)0.5 * (a1 - a2);
562 T mu = a2 - copysign(bs / (fabs(d) + sqrt(d * d + bs)), d);
570 template < int t, class T>
571 inline void process(Eigen::Matrix<T, 3, 3>& B, Eigen::Matrix<T, 3, 3>& U, Eigen::Matrix<T, 3, 1>& sigma, Eigen::Matrix<T, 3, 3>& V)
573 int other = (t == 1) ? 0 : 2;
576 sigma(other) = B(other, other);
590 inline void flipSign( int i, Eigen::Matrix<T, 3, 3>& U, Eigen::Matrix<T, 3, 1>& sigma)
592 sigma(i) = -sigma(i);
593 U.col(i) = -U.col(i);
599 template < int t, class T>
600 std::enable_if_t<t == 0> sort(Eigen::Matrix<T, 3, 3>& U, Eigen::Matrix<T, 3, 1>& sigma, Eigen::Matrix<T, 3, 3>& V)
605 if (fabs(sigma(1)) >= fabs(sigma(2))) {
620 std::swap(sigma(1), sigma(2));
621 U.col(1).swap(U.col(2));
622 V.col(1).swap(V.col(2));
625 if (sigma(1) > sigma(0)) {
626 std::swap(sigma(0), sigma(1));
627 U.col(0).swap(U.col(1));
628 V.col(0).swap(V.col(1));
633 U.col(2) = -U.col(2);
634 V.col(2) = -V.col(2);
641 template < int t, class T>
642 std::enable_if_t<t == 1> sort(Eigen::Matrix<T, 3, 3>& U, Eigen::Matrix<T, 3, 1>& sigma, Eigen::Matrix<T, 3, 3>& V)
647 if (fabs(sigma(0)) >= sigma(1)) {
656 std::swap(sigma(0), sigma(1));
657 U.col(0).swap(U.col(1));
658 V.col(0).swap(V.col(1));
661 if (fabs(sigma(1)) < fabs(sigma(2))) {
662 std::swap(sigma(1), sigma(2));
663 U.col(1).swap(U.col(2));
664 V.col(1).swap(V.col(2));
669 U.col(1) = -U.col(1);
670 V.col(1) = -V.col(1);
689 Eigen::Matrix<T, 3, 3>& U,
690 Eigen::Matrix<T, 3, 1>& sigma,
691 Eigen::Matrix<T, 3, 3>& V,
692 T tol = 1024 * std::numeric_limits<T>::epsilon())
697 Eigen::Matrix<T, 3, 3> B = A;
698 U = Eigen::Matrix<T, 3, 3>::Identity();
699 V = Eigen::Matrix<T, 3, 3>::Identity();
712 T gamma_1 = alpha_1 * beta_1;
713 T gamma_2 = alpha_2 * beta_2;
714 tol *= max((T)0.5 * sqrt(alpha_1 * alpha_1 + alpha_2 * alpha_2 + alpha_3 * alpha_3 + beta_1 * beta_1 + beta_2 * beta_2), (T)1);
720 while (fabs(beta_2) > tol && fabs(beta_1) > tol
721 && fabs(alpha_1) > tol && fabs(alpha_2) > tol
722 && fabs(alpha_3) > tol) {
723 mu = wilkinsonShift(alpha_2 * alpha_2 + beta_1 * beta_1, gamma_2, alpha_3 * alpha_3 + beta_2 * beta_2);
725 r. compute(alpha_1 * alpha_1 - mu, gamma_1);
736 gamma_1 = alpha_1 * beta_1;
737 gamma_2 = alpha_2 * beta_2;
750 if (fabs(beta_2) <= tol) {
751 process<0>(B, U, sigma, V);
752 sort<0>(U, sigma, V);
760 else if (fabs(beta_1) <= tol) {
761 process<1>(B, U, sigma, V);
762 sort<1>(U, sigma, V);
770 else if (fabs(alpha_2) <= tol) {
782 process<0>(B, U, sigma, V);
783 sort<0>(U, sigma, V);
791 else if (fabs(alpha_3) <= tol) {
813 process<0>(B, U, sigma, V);
814 sort<0>(U, sigma, V);
822 else if (fabs(alpha_1) <= tol) {
845 process<1>(B, U, sigma, V);
846 sort<1>(U, sigma, V);
865 Eigen::Matrix<T, 3, 3>& R,
866 Eigen::Matrix<T, 3, 3>& S_Sym)
868 Eigen::Matrix<T, 3, 3> U;
869 Eigen::Matrix<T, 3, 1> sigma;
870 Eigen::Matrix<T, 3, 3> V;
873 R.noalias() = U * V.transpose();
874 S_Sym.noalias() = V * Eigen::DiagonalMatrix<T, 3, 3>(sigma) * V.transpose();
|
Follow us!