qpatchmath.cpp

来自「算断裂的」· C++ 代码 · 共 2,107 行 · 第 1/5 页

    PatchMath::Workspace::RealVector& select_col, 
    PatchMath::Workspace::RealMatrix& hhmat,
    PatchMath::Workspace::RealVector& betavec) {

    int m = A.numrow();
    int n = A.numcol();
    int nstep = (m < n)? m : n;

    for (int k = 0; k < nstep; ++k) {
      int select_col1;
      {
        Real mx1 = 0.0;
        for (int j = 0; j < n; ++j) {
          bool j_already_selected = false;
          {
            for (int i = 0; i < k; ++i) {
              if (static_cast<int>(select_col[i]) == j) {
                j_already_selected = true;
                break;
              }
            }
          }
          if (j_already_selected) continue;
          
          // Find the norm of column j residual entries.
          Real nr1 = 0.0;
          {
            for (int i = k; i < m; ++i) {
              nr1 += A(i,j) * A(i,j);
            }
          }
          if (nr1 >= mx1) {
            mx1 = nr1;
            select_col1 = j;
          }
        }
      }
      select_col[k] = static_cast<Real>(select_col1);
      if (k == m - 1)
        break;
      Real* hhmat_col = &hhmat(0,k);
      Real beta = 
        Matrix::compute_hh_transform(&hhmat_col[k], &A(k, select_col1), 1, m - k);
      betavec[k] = beta;
      {
        for (int j = 0; j < n; ++j) {
          Real ip1 = 0.0;
          {
            for (int i = k; i < m; ++i)
              ip1 += A(i,j) * hhmat_col[i];
          }
          ip1 *= beta;
          {
            for (int i = k; i < m; ++i)
              A(i,j) -= ip1 * hhmat_col[i];
          }
        }
      }
    }
  }

  // Multiply HH transforms together to get an explicit
  // orthogonal matrix Q.

  void formQ(PatchMath::Workspace::RealMatrix& hhmat,
    PatchMath::Workspace::RealVector& betavec,
    PatchMath::Workspace::RealMatrix& Q) {

    int m = hhmat.numrow();
    int n = hhmat.numcol();

    // Initialize Q to be the identity.

    {
      for (int i = 0; i < m; ++i)
        for (int j = 0; j < m; ++j)
          Q(i,j) = static_cast<Real>(static_cast<int>(i == j));
    }

    // Loop over HH transforms in reverse order 

    for (int k = n - 1; k >= 0; --k) {
      
      // Apply the kth HH transform to Q.
      // j loops over columns of Q.
      for (int j = k; j < m; ++j) {
        Real ip1 = 0.0;
        {
          for (int i = k; i < m; ++i)
            ip1 += hhmat(i,k) * Q(i,j);
        }
        ip1 *= betavec[k];
        {
          for (int i = k; i < m; ++i)
            Q(i,j) -= ip1 * hhmat(i,k);
        }
      }
    }
  }

        
  // This is a helper class for math on 
  // a one-variable Bezier.

  class Solve_OneVar_Bezier {
  public:
    virtual Real get_bez_coeff(int j) const = 0;
    Solve_OneVar_Bezier() { }
    virtual ~Solve_OneVar_Bezier() { }
    void solve(int degree,
      vector<PatchMath::IntersectRecord>& output_stack,
      PatchMath::Workspace& workspace, 
      double scfac, 
      double tol) const;
    Real eval(Real param,
      int degree) const;
    Real eval_deriv(Real param,
      int degree) const;
  };

  // Derived class for math on a one-variable Bezier that arises
  // from intersecting a line with a curve in 2D.

  class Solve_OneVar_Bezier1 : public Solve_OneVar_Bezier {
  private: 
    const Matrix& lineeq_;
    const Point& linerhs_;
    const PatchMath& pm_;
  public:
    Solve_OneVar_Bezier1(const Matrix& lineeq,
      const Point& linerhs,
      const PatchMath& pm) : lineeq_(lineeq), linerhs_(linerhs), pm_(pm) { }
    Real get_bez_coeff(int cpnum) const {
      return lineeq_(0,0) * PatchMath::Bezier_Helper::control_point_coord(pm_, cpnum, 0) +
        lineeq_(0,1) * PatchMath::Bezier_Helper::control_point_coord(pm_, cpnum, 1) -
        linerhs_[0];
    }
  };

  // Derived class for math on a one-variable Bezier that
  // arises from intersecting a curve with a plane in 3D.

  class Solve_OneVar_Bezier2 : public Solve_OneVar_Bezier {
  private:
    const Point& plane_normal_;
    Real plane_rhs_;
    const PatchMath& pm_;
  public:
    Solve_OneVar_Bezier2(const Point& plane_normal,
      Real plane_rhs,
      const PatchMath& pm) : plane_normal_(plane_normal), plane_rhs_(plane_rhs), 
      pm_(pm) { }
    Real get_bez_coeff(int cpnum) const {
      return plane_normal_[0] * PatchMath::Bezier_Helper::control_point_coord(pm_, cpnum, 0) +
        plane_normal_[1] * PatchMath::Bezier_Helper::control_point_coord(pm_, cpnum,1) +
        plane_normal_[2] * PatchMath::Bezier_Helper::control_point_coord(pm_, cpnum,2) -
        plane_rhs_;
    }
  };

  // Derived class for evaluating one dimension of a Bezier curve.

  class Solve_OneVar_Bezier3 : public Solve_OneVar_Bezier {
  private:
    int select_dim_;
    const PatchMath& pm_;
  public:
    Solve_OneVar_Bezier3(int select_dim, 
      const PatchMath& pm) : select_dim_(select_dim), pm_(pm) 
    { }
    Real get_bez_coeff(int cpnum) const {
      return PatchMath::Bezier_Helper::control_point_coord(pm_, cpnum, select_dim_);
    }
  };


  // Derived class for evaluating the derivative of a Bezier curve.


  class Solve_OneVar_Bezier4 : public Solve_OneVar_Bezier {
  private:
    const Point& direc_;
    int di_;
    const PatchMath& pm_;
  public:
    Solve_OneVar_Bezier4(const Point& direc, int di,
      const PatchMath& pm) : direc_(direc), di_(di), pm_(pm) 
    { }
    Real get_bez_coeff(int cpnum) const {
      Real t = 0.0;
      for (int j = 0; j < di_; ++j) {
        t += (PatchMath::Bezier_Helper::control_point_coord(pm_, cpnum + 1, j) -
          PatchMath::Bezier_Helper::control_point_coord(pm_, cpnum, j)) *
          direc_[j];
      }
      return t;
    }
  };


  // Evaluate a one-variable Bezier curve using Horner algorithm.
  

  Real Solve_OneVar_Bezier::eval(Real param,
    int degree) const {
    Real u = 1.0 - param;
    Real pwrv = 1.0; 
    Real result = 0.0;
    int binom = 1;
    for (int j = 0; j < degree + 1; ++j) {
      result *= u;
      result += binom * pwrv * get_bez_coeff(j);      
      pwrv *= param;
      binom *= (degree - j);
      binom /= (j + 1);
    }
    return result;
  }
  
  // Evaluate derivative a one-variable Bezier curve using Horner algorithm.
  

  Real Solve_OneVar_Bezier::eval_deriv(Real param,
    int degree) const {
    Real u = 1.0 - param;
    Real pwrv = 1.0; 
    Real result = 0.0;
    int binom = 1;
    Real prevbez = get_bez_coeff(0);
    for (int j = 0; j < degree; ++j) {
      result *= u;
      Real nextbez = get_bez_coeff(j + 1);
      result += binom * pwrv * (nextbez - prevbez);
      prevbez = nextbez;
      pwrv *= param;
      binom *= (degree - j - 1);
      binom /= (j + 1);
    }
    return result * degree;
  }


  // solve: find zeros of a one-variable Bezier.
  // Transforms to an eigenvalue problem and invokes LAPACK.
  // Doesn't set output_stack real coord fields -- caller must do this.
 
  void Solve_OneVar_Bezier::solve(int degree,
    vector<PatchMath::IntersectRecord>& output_stack,
    PatchMath::Workspace& workspace, 
    double scfac, 
    double tol) const {


#ifdef DEBUGGING
    using QMG::Local_to_PatchMath_CPP::debug_str;
    using QMG::Local_to_PatchMath_CPP::dump_everything;

    if (dump_everything) {
      *debug_str << " In solve bezier degree = " << degree 
        << " scfac = " << scfac << " tol = " << tol << "\n bezcoeff = [";
      for (int j = 0; j < degree + 1; ++j) 
        *debug_str << std::setprecision(17) << get_bez_coeff(j) << " ";
      *debug_str << "]\n";
    }
#endif

    // If the bezier control points miss 0, then early return

    {
      bool pos_seen = false;
      bool neg_seen = false;
      for (int i = 0; i <= degree; ++i) {
        Real bz = get_bez_coeff(i);
        if (bz <= scfac * tol)
          neg_seen = true;
        if (bz >= -scfac * tol)
          pos_seen = true;
      }
      if (!pos_seen || !neg_seen) {
#ifdef DEBUGGING
        if (dump_everything)
          *debug_str << "origin missed; returning early";
#endif
        return;
      }
    }


    if (degree == 1) {

      // In the linear case just solve the one-variable equation.

      Real denom = get_bez_coeff(0) - get_bez_coeff(1);
      if (denom == 0) {
        return;
      }
      Real sol = get_bez_coeff(0) / denom;
      Real uncer = tol * scfac / fabs(denom);
      if (sol < -uncer || sol > 1.0 + uncer)
        return;
      PatchMath::IntersectRecord result;
      result.paramcoord[0] = sol;
      result.param_uncertainty = uncer;
      result.degen = fabs(sol) <= uncer || fabs(sol - 1.0) <= uncer;
      output_stack.push_back(result);
      return;
    }
    else {
      using std::abs;
      using std::setprecision;
      PatchMath::Workspace::StartPosMarker startposmarker_(workspace);
      
      int degree1 = degree + 1;

      // Multiply by binomial coefs.

      PatchMath::Workspace::RealVector bvec(workspace, degree1);
     
      Real bezmax = 0;
      {
        int binom = 1;
        for (int i = 0; i < degree + 1; ++i) {
          bvec[i] = get_bez_coeff(i) * binom;
          binom *= (degree - i);
          binom /= (i + 1);
          if (fabs(bvec[i]) > bezmax)
            bezmax = fabs(bvec[i]);
        }
      }

      if (bezmax == 0) return;
      
      // grab space in the workspace.
      
      Lapack_int lapack_cworkspace_size = degree * 8 + 
        ((degree < 64)? 64 : degree) * degree + 256;
      
      PatchMath::Workspace::ComplexVector powerv(workspace, degree1);
      PatchMath::Workspace::ComplexVector test_shiftpoly(workspace, degree1);
      PatchMath::Workspace::ComplexMatrix A(workspace, degree, degree);
      PatchMath::Workspace::ComplexMatrix A0(workspace, degree, degree);
      PatchMath::Workspace::ComplexVector test_eigs(workspace, degree);
      PatchMath::Workspace::ComplexVector best_eigs(workspace, degree);
      PatchMath::Workspace::RealVector 
        lapack_rworkspace(workspace, degree * 2);
      PatchMath::Workspace::ComplexVector lapack_cworkspace(workspace, 
        lapack_cworkspace_size);

      // Make a substitution [u;v] = [1, zs ; -conj(zs), 1] * [u'; v']
      // where a is a random complex number.  This is to make sure
      // the leading coef does not vanish.

      Complex best_zs;
      Complex best_conjzs;
      Real best_lead = -1.0;

      for (int trycount = 0; trycount < 5; ++trycount) {

        // Choose the shift.

        Complex zs = Complex(random_real() + 0.5, random_real() + 0.5);
        Complex conjzs = Complex(zs.real(), -zs.imag());

#ifdef DEBUGGING
        if (dump_everything) {
          *debug_str << "trycount = " << trycount 
                     << "\n zs = " << Format_Complex(zs)
                     << "\n conjzs = " << Format_Complex(conjzs)
                     << '\n';
        }
#endif

        // To make the substitution, use a variant of Horner's rule
        //   powerv is a polynomial that holds ( -conj(zs)*u'+ v') ^k
        // for increasing powers of k.
        //   shifpoly is the result of the substitution.

        {
          for (int j = 0; j < degree1; ++j)
            powerv[j] = Complex(0.0, 0.0);
          powerv[0] = Complex(1.0, 0.0);
        }

        {
          for (int j = 0; j < degree1; ++j)
            test_shiftpoly[j] = Complex(0.0, 0.0);
          test_shiftpoly[0] = Complex(bvec[degree], 0.0);
        }
        // Now repeatedly multiply shiftpoly by u, and then add v^k, for
        // increasing values of k.

        {
          for (int k = 0; k < degree; ++k) {
            
            // Multiply shiftpoly by u, which is u' + zs*v'
            {
              for (int l = k + 1; l > 0; --l) {
                test_shiftpoly[l] = test_shiftpoly[l - 1] + 
                  test_shiftpoly[l] * zs;
              }
              test_shiftpoly[0] = test_shiftpoly[0] * zs;
            }
          

            // Multiply powerv by v, which is  -conj(zs)  * u' + v'

            {
              for (int l = k + 1; l > 0; --l) {
                powerv[l] -= powerv[l - 1] * conjzs;
              }
            }
            // Add bez[k+1] * powerv to shiftpoly.

            {
              for (int l = 0; l <= k + 1; ++l) {
                test_shiftpoly[l] += bvec[degree - k - 1] * powerv[l];
              }
            }
          }
        }

        // If the leading coef of shiftpoly is too small, must try again.