Example #1
0
Vector3	Matrix3::Column(int j) const
{
	return Vector3(mat(1,j),mat(2,j),mat(3,j));
}
Example #2
0
void PT::asqtad_fat(AsqDArg *asq_arg, matrix *fatlink){
  char *fname = "Smear()";

//--------------------------------------------------------------------
// c1 -> one link; c2 -> 3-link; c3 -> 3-link staple; c5 -> 5-link staple;
// c7 -> 7-link staple; c6 -> 5-link "straight" staple
//--------------------------------------------------------------------

  IFloat c1 = asq_arg->c1;
  IFloat c3 = asq_arg->c3;
  IFloat c5 = asq_arg->c5;
  IFloat c7 = asq_arg->c7;
  IFloat c6 = asq_arg->c6;
#if 0
  IFloat c1 = GJP.KS_coeff();
  IFloat c2 = GJP.Naik_coeff();
  IFloat c3 = -GJP.staple3_coeff();
  IFloat c5 = GJP.staple5_coeff();
  IFloat c7 = -GJP.staple7_coeff();
  IFloat c6 = GJP.Lepage_coeff();
#endif

  int i, j; 
  const int N = 4;
//  int vol = GJP.VolNodeSites();
//  ParTransAsqtad pt(*this);
  matrix *result[NUM_DIR];
  matrix *Unit;
  matrix *P3[NUM_DIR];
  matrix *P3mu[NUM_DIR];
  matrix *P5[NUM_DIR];
  matrix *P5mu[NUM_DIR];
  matrix *P7[NUM_DIR];
  matrix *P7mu[NUM_DIR];
  matrix *P6[NUM_DIR];
  matrix *P6mu[NUM_DIR];
  matrix *Pmumu[NUM_DIR];
  matrix *Pmumumu[NUM_DIR];
//VRB.Flow(cname,fname,"vol=%d\n",vol);
  Unit = (matrix *)Alloc(cname,fname,"Unit",sizeof(matrix)*vol);
  for(i = 0;i<N;i++)
    Pmumumu[i] = P7mu[i] = (matrix *)Alloc(cname,fname,"Pmumumu[i]",sizeof(matrix)*vol);
  for(i = 0;i<N;i++)
    Pmumu[i] = P7[i] = (matrix *)Alloc(cname,fname,"Pmumu[i]",sizeof(matrix)*vol);
  for(i = 0;i<N;i++)
    P6mu[i] = P5mu[i] = (matrix *)Alloc(cname,fname,"P6mu[i]",sizeof(matrix)*vol);
  for(i = 0;i<N;i++)
    P6[i] = P5[i] = (matrix *)Alloc(cname,fname,"P6[i]",sizeof(matrix)*vol);
  for(i = 0;i<N;i++)
    P3mu[i] = (matrix *)Alloc(cname,fname,"P3mu[i]",sizeof(matrix)*vol);
  for(i = 0;i<N;i++)
    P3[i] = (matrix *)Alloc(cname,fname,"P3[i]",sizeof(matrix)*vol);
  for(j = 0;j<POS_DIR;j++){
//     result[j] = fields[0] + vol*j;
     result[j] = fatlink + vol*j;
//    result[j+POS_DIR] = fields[1] + vol*j;
//     VRB.Flow(cname,fname,"result[%d]=%p\n",j,result[j]);
  }
//     result[j] = (matrix *)fmalloc(sizeof(matrix)*vol);

  for(j = 0;j<vol;j++) Unit[j] = c1;
  for(j = 0;j<POS_DIR;j++)
#if 1
    bzero((char *)result[j],vol*18*sizeof(Float));
#else
     for(int k = 0;k<vol;k++) (result[j]+k)->Zeromatrix();
#endif
//  Float dtime = -dclock();
  int nflops = 0;
//  ParTrans::PTflops=0;
  int dir[] = {6,0,2,4,7,1,3,5},dirs[8]; //mapping between ParTrans and DiracOpAsqtad
  matrix *min[NUM_DIR],*mout[NUM_DIR];
  if (NUM_DIR%N !=0) fprintf(stderr,"%s:NUM_DIR(%d)is not divisible by N(%d)\n",fname,NUM_DIR,N);
  for(int mu = 0;mu<POS_DIR;mu += N){
	int mu_p,nu_p,rho_p,sigma_p;
    for(i  = 0;i<N;i++){
      mu_p = Rotate(mu,i);
      min[i] = Unit;
      mout[i] = result[mu_p];
      dirs[i] = dir[mu_p];
    }
    mat(N,mout,min,dirs);
    for(int nu = 0;nu<NUM_DIR;nu++)
    if(NotParallel(mu,nu)){
      for(i  = 0;i<N;i++){
	nu_p = Rotate(nu,i);
        min[i] = Unit;
        mout[i] = P3[i];
        dirs[i] = dir[nu_p];
      }
      mat(N,mout,min,dirs);
      for(i  = 0;i<N;i++){
		mu_p = Rotate(mu,i);
        min[i] = P3[i];
        mout[i] = P3mu[i];
        dirs[i] = dir[mu_p];
      }
      mat(N,mout,min,dirs);
      for(int rho = 0;rho<NUM_DIR;rho++)
      if(NotParallel(mu,rho) && NotParallel(nu,rho)){
        for(i  = 0;i<N;i++){
  	   	  rho_p = Rotate(rho,i);
          min[i] = P3[i];
          mout[i] = P5[i];
          dirs[i] = dir[rho_p];
        }
        mat(N,mout,min,dirs);
        for(i  = 0;i<N;i++){
	  	mu_p = Rotate(mu,i);
          min[i] = P5[i];
          mout[i] = P5mu[i];
          dirs[i] = dir[mu_p];
        }
        mat(N,mout,min,dirs);
        for(int sigma = 0;sigma<NUM_DIR;sigma++)
        if(NotParallel(mu,sigma) && NotParallel(nu,sigma)&&NotParallel(rho,sigma)){
          for(i  = 0;i<N;i++){
    		sigma_p = Rotate(sigma,i);
            min[i] = P5[i];
            mout[i] = P7[i];
            dirs[i] = dir[sigma_p];
          }
          mat(N,mout,min,dirs);
          for(i  = 0;i<N;i++){
    		mu_p = Rotate(mu,i);
            min[i] = P7[i];
            mout[i] = P7mu[i];
            dirs[i] = dir[mu_p];
          }
          mat(N,mout,min,dirs);
          for(i  = 0;i<N;i++){
    		sigma_p = Rotate(sigma,i);
            int sig_n = (sigma_p+4)%8;
            min[i] = P7mu[i];
            mout[i] = P7[i];
            dirs[i] = dir[sig_n];
          }
          mat(N,mout,min,dirs);
		  Float c75 = c7/c5;
          for(i  = 0;i<N;i++)
            vaxpy3_m(P5mu[i],&c75,P7[i],P5mu[i],vol*3);
	  nflops +=vol*18*2*N;
        }
        for(i  = 0;i<N;i++){
            rho_p = Rotate(rho,i);
            int rho_n = (rho_p+4)%8;
            min[i] = P5mu[i];
            mout[i] = P5[i];
            dirs[i] = dir[rho_n];
        }
        mat(N,mout,min,dirs);
		Float c53 = c5/c3;
        for(i  = 0;i<N;i++)
          vaxpy3_m(P3mu[i],&c53,P5[i],P3mu[i],vol*3);
		nflops +=vol*18*2*N;
      }
      for(i  = 0;i<N;i++){
        nu_p = Rotate(nu,i);
        min[i] = P3[i];
        mout[i] = P6[i];
        dirs[i] = dir[nu_p];
      }
      mat(N,mout,min,dirs);
      for(i  = 0;i<N;i++){
        mu_p = Rotate(mu,i);
        min[i] = P6[i];
        mout[i] = P6mu[i];
        dirs[i] = dir[mu_p];
      }
      mat(N,mout,min,dirs);
      for(i  = 0;i<N;i++){
        nu_p = Rotate(nu,i);
        int nu_n = (nu_p+4)%8;
        min[i] = P6mu[i];
        mout[i] = P6[i];
        dirs[i] = dir[nu_n];
      }
      mat(N,mout,min,dirs);
	  Float c63 = c6/c3;
      for(i  = 0;i<N;i++)
        vaxpy3_m(P3mu[i],&c63,P6[i],P3mu[i],vol*3);
	  nflops +=vol*18*2*N;
      for(i  = 0;i<N;i++){
        nu_p = Rotate(nu,i);
        int nu_n = (nu_p+4)%8;
        min[i] = P3mu[i];
        mout[i] = P3[i];
        dirs[i] = dir[nu_n];
      }
      mat(N,mout,min,dirs);

	  Float c31 = c3/c1;
	  for(i  = 0;i<N;i++){
	    mu_p =Rotate(mu,i);
        vaxpy3_m(result[mu_p],&c31,P3[i],result[mu_p],vol*3);
      }
	  nflops +=vol*18*2*N;
    }
  }

  Free( Unit);
  for(j = 0;j<N;j++){
    Free( P3[j]);
    Free( P3mu[j]);
    Free( P5[j]);
    Free( P5mu[j]);
    Free( P7[j]);
    Free( P7mu[j]);
  }
}
Example #3
0
void read()
{
    Abc::IArchive archive(Alembic::AbcCoreHDF5::ReadArchive(),
            "HasAMaterial.abc");
    
    Abc::IObject an_object(archive.getTop(), "an_object");
    
    
    if (const Abc::PropertyHeader * header =
            an_object.getProperties().getPropertyHeader(".byanyothername"))
    {
        TESTING_ASSERT(Mat::OMaterialSchema::matches(*header));
        if (Mat::OMaterialSchema::matches(*header))
        {
            std::cout << ".byanyothername yes.\n";
            
            Mat::IMaterialSchema mat(an_object.getProperties(), ".byanyothername");
            
            printMaterialSchema(mat);
            
        }
        else
        {
            std::cout << ".byanyothername no.\n";
        }
        
    }
    
    if (const Abc::PropertyHeader * header =
            an_object.getProperties().getPropertyHeader("butnotbythisone"))
    {
        TESTING_ASSERT( !Mat::OMaterialSchema::matches(*header) );
        if (Mat::OMaterialSchema::matches(*header))
        {
            std::cout << "butnotbythisone yes.\n";
        }
        else
        {
            std::cout << "butnotbythisone no.\n";
        }
    }
    
    
    if (const Abc::PropertyHeader * header =
            an_object.getProperties().getPropertyHeader(".material"))
    {
        TESTING_ASSERT(!Mat::OMaterialSchema::matches(*header));
        if (Mat::OMaterialSchema::matches(*header))
        {
            std::cout << "manually built .material yes.\n";
        }
        else
        {
            std::cout << "manually built .material no.\n";
        }
    }
    
    std::cout << "-----------\n";
    
    Abc::IObject anotherObj(archive.getTop(), "another_object");
    
    
    std::string assignmentPath;
    if (Mat::getMaterialAssignmentPath(anotherObj, assignmentPath))
    {
        std::cout << "another_object assignment path: " << assignmentPath;
        std::cout << std::endl;
    }
    TESTING_ASSERT(assignmentPath == "/some/material");

    Mat::IMaterialSchema hasMat;
    
    TESTING_ASSERT(Mat::hasMaterial(anotherObj, hasMat));
    if (Mat::hasMaterial(anotherObj, hasMat))
    {
        std::cout << "another_object has local material: " << std::endl;
        
        printMaterialSchema(hasMat);
    }
    
}
Example #4
0
void init_eigen(py::module &m) {
    typedef Eigen::Matrix<float, 5, 6, Eigen::RowMajor> FixedMatrixR;
    typedef Eigen::Matrix<float, 5, 6> FixedMatrixC;
    typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> DenseMatrixR;
    typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> DenseMatrixC;
    typedef Eigen::SparseMatrix<float, Eigen::RowMajor> SparseMatrixR;
    typedef Eigen::SparseMatrix<float> SparseMatrixC;

    // Non-symmetric matrix with zero elements
    Eigen::MatrixXf mat(5, 6);
    mat << 0, 3, 0, 0, 0, 11, 22, 0, 0, 0, 17, 11, 7, 5, 0, 1, 0, 11, 0,
        0, 0, 0, 0, 11, 0, 0, 14, 0, 8, 11;

    m.def("fixed_r", [mat]() -> FixedMatrixR { 
        return FixedMatrixR(mat);
    });

    m.def("fixed_c", [mat]() -> FixedMatrixC { 
        return FixedMatrixC(mat);
    });

    m.def("fixed_passthrough_r", [](const FixedMatrixR &m) -> FixedMatrixR { 
        return m;
    });

    m.def("fixed_passthrough_c", [](const FixedMatrixC &m) -> FixedMatrixC { 
        return m;
    });

    m.def("dense_r", [mat]() -> DenseMatrixR { 
        return DenseMatrixR(mat);
    });

    m.def("dense_c", [mat]() -> DenseMatrixC { 
        return DenseMatrixC(mat);
    });

    m.def("dense_passthrough_r", [](const DenseMatrixR &m) -> DenseMatrixR { 
        return m;
    });

    m.def("dense_passthrough_c", [](const DenseMatrixC &m) -> DenseMatrixC { 
        return m;
    });

    m.def("sparse_r", [mat]() -> SparseMatrixR { 
        return Eigen::SparseView<Eigen::MatrixXf>(mat);
    });

    m.def("sparse_c", [mat]() -> SparseMatrixC { 
        return Eigen::SparseView<Eigen::MatrixXf>(mat);
    });

    m.def("sparse_passthrough_r", [](const SparseMatrixR &m) -> SparseMatrixR { 
        return m;
    });

    m.def("sparse_passthrough_c", [](const SparseMatrixC &m) -> SparseMatrixC { 
        return m;
    });
}
Example #5
0
    void deflat(T& M, const Interface& i, double coef) 
    {
        // deflate the Matrix
        for (Interface::const_iterator omit=i.begin();omit!=i.end();++omit) {
            for (Mesh::const_vertex_iterator vit1=omit->mesh().vertex_begin();vit1!=omit->mesh().vertex_end();++vit1) {
                #pragma omp parallel for
                #ifndef OPENMP_3_0
                for (int i2=vit1-omit->mesh().vertex_begin();i2<omit->mesh().vertex_size();++i2) {
                    const Mesh::const_vertex_iterator vit2 = omit->mesh().vertex_begin()+i2;
                #else
                for (Mesh::const_vertex_iterator vit2=vit1;vit2<omit->mesh().vertex_end();++vit2) {
                #endif
                    M((*vit1)->index(),(*vit2)->index()) += coef;
                }
            }
        }
    }

    void assemble_HM(const Geometry& geo, SymMatrix& mat, const unsigned gauss_order) 
    {
        mat = SymMatrix((geo.size()-geo.outermost_interface().nb_triangles()));
        mat.set(0.0);
        double K = 1.0 / (4.0 * M_PI);

        // We iterate over the meshes (or pair of domains) to fill the lower half of the HeadMat (since its symmetry)
        for ( Geometry::const_iterator mit1 = geo.begin(); mit1 != geo.end(); ++mit1) {

            for ( Geometry::const_iterator mit2 = geo.begin(); (mit2 != (mit1+1)); ++mit2) {

                // if mit1 and mit2 communicate, i.e they are used for the definition of a common domain
                const int orientation = geo.oriented(*mit1, *mit2); // equals  0, if they don't have any domains in common
                                                                       // equals  1, if they are both oriented toward the same domain
                                                                       // equals -1, if they are not

                if ( orientation != 0 ) {

                    double Scoeff =   orientation * geo.sigma_inv(*mit1, *mit2) * K;
                    double Dcoeff = - orientation * geo.indicator(*mit1, *mit2) * K;
                    double Ncoeff;

                    if ( !(mit1->outermost() || mit2->outermost()) ) {
                        // Computing S block first because it's needed for the corresponding N block
                        operatorS(*mit1, *mit2, mat, Scoeff, gauss_order);
                        Ncoeff = geo.sigma(*mit1, *mit2)/geo.sigma_inv(*mit1, *mit2);
                    } else {
                        Ncoeff = orientation * geo.sigma(*mit1, *mit2) * K;
                    }

                    if ( !mit1->outermost() ) {
                        // Computing D block
                        operatorD(*mit1, *mit2, mat, Dcoeff, gauss_order,false);
                    }
                    if ( ( *mit1 != *mit2 ) && ( !mit2->outermost() ) ) {
                        // Computing D* block
                        operatorD(*mit1, *mit2, mat, Dcoeff, gauss_order, true);
                    }

                    // Computing N block
                    operatorN(*mit1, *mit2, mat, Ncoeff, gauss_order);
                }
            }
        }

        // Deflate the diagonal block (N33) of 'mat' : (in order to have a zero-mean potential for the outermost interface)
        const Interface i = geo.outermost_interface();
        unsigned i_first = (*i.begin()->mesh().vertex_begin())->index();
        deflat(mat, i, mat(i_first, i_first) / (geo.outermost_interface().nb_vertices()));
    }
Example #6
0
TEST_SUBMODULE(eigen, m) {
    using FixedMatrixR = Eigen::Matrix<float, 5, 6, Eigen::RowMajor>;
    using FixedMatrixC = Eigen::Matrix<float, 5, 6>;
    using DenseMatrixR = Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
    using DenseMatrixC = Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic>;
    using FourRowMatrixC = Eigen::Matrix<float, 4, Eigen::Dynamic>;
    using FourColMatrixC = Eigen::Matrix<float, Eigen::Dynamic, 4>;
    using FourRowMatrixR = Eigen::Matrix<float, 4, Eigen::Dynamic>;
    using FourColMatrixR = Eigen::Matrix<float, Eigen::Dynamic, 4>;
    using SparseMatrixR = Eigen::SparseMatrix<float, Eigen::RowMajor>;
    using SparseMatrixC = Eigen::SparseMatrix<float>;

    m.attr("have_eigen") = true;

    // various tests
    m.def("double_col", [](const Eigen::VectorXf &x) -> Eigen::VectorXf { return 2.0f * x; });
    m.def("double_row", [](const Eigen::RowVectorXf &x) -> Eigen::RowVectorXf { return 2.0f * x; });
    m.def("double_complex", [](const Eigen::VectorXcf &x) -> Eigen::VectorXcf { return 2.0f * x; });
    m.def("double_threec", [](py::EigenDRef<Eigen::Vector3f> x) { x *= 2; });
    m.def("double_threer", [](py::EigenDRef<Eigen::RowVector3f> x) { x *= 2; });
    m.def("double_mat_cm", [](Eigen::MatrixXf x) -> Eigen::MatrixXf { return 2.0f * x; });
    m.def("double_mat_rm", [](DenseMatrixR x) -> DenseMatrixR { return 2.0f * x; });

    // test_eigen_ref_to_python
    // Different ways of passing via Eigen::Ref; the first and second are the Eigen-recommended
    m.def("cholesky1", [](Eigen::Ref<MatrixXdR> x) -> Eigen::MatrixXd { return x.llt().matrixL(); });
    m.def("cholesky2", [](const Eigen::Ref<const MatrixXdR> &x) -> Eigen::MatrixXd { return x.llt().matrixL(); });
    m.def("cholesky3", [](const Eigen::Ref<MatrixXdR> &x) -> Eigen::MatrixXd { return x.llt().matrixL(); });
    m.def("cholesky4", [](Eigen::Ref<const MatrixXdR> x) -> Eigen::MatrixXd { return x.llt().matrixL(); });

    // test_eigen_ref_mutators
    // Mutators: these add some value to the given element using Eigen, but Eigen should be mapping into
    // the numpy array data and so the result should show up there.  There are three versions: one that
    // works on a contiguous-row matrix (numpy's default), one for a contiguous-column matrix, and one
    // for any matrix.
    auto add_rm = [](Eigen::Ref<MatrixXdR> x, int r, int c, double v) { x(r,c) += v; };
    auto add_cm = [](Eigen::Ref<Eigen::MatrixXd> x, int r, int c, double v) { x(r,c) += v; };

    // Mutators (Eigen maps into numpy variables):
    m.def("add_rm", add_rm); // Only takes row-contiguous
    m.def("add_cm", add_cm); // Only takes column-contiguous
    // Overloaded versions that will accept either row or column contiguous:
    m.def("add1", add_rm);
    m.def("add1", add_cm);
    m.def("add2", add_cm);
    m.def("add2", add_rm);
    // This one accepts a matrix of any stride:
    m.def("add_any", [](py::EigenDRef<Eigen::MatrixXd> x, int r, int c, double v) { x(r,c) += v; });

    // Return mutable references (numpy maps into eigen varibles)
    m.def("get_cm_ref", []() { return Eigen::Ref<Eigen::MatrixXd>(get_cm()); });
    m.def("get_rm_ref", []() { return Eigen::Ref<MatrixXdR>(get_rm()); });
    // The same references, but non-mutable (numpy maps into eigen variables, but is !writeable)
    m.def("get_cm_const_ref", []() { return Eigen::Ref<const Eigen::MatrixXd>(get_cm()); });
    m.def("get_rm_const_ref", []() { return Eigen::Ref<const MatrixXdR>(get_rm()); });

    m.def("reset_refs", reset_refs); // Restores get_{cm,rm}_ref to original values

    // Increments and returns ref to (same) matrix
    m.def("incr_matrix", [](Eigen::Ref<Eigen::MatrixXd> m, double v) {
        m += Eigen::MatrixXd::Constant(m.rows(), m.cols(), v);
        return m;
    }, py::return_value_policy::reference);

    // Same, but accepts a matrix of any strides
    m.def("incr_matrix_any", [](py::EigenDRef<Eigen::MatrixXd> m, double v) {
        m += Eigen::MatrixXd::Constant(m.rows(), m.cols(), v);
        return m;
    }, py::return_value_policy::reference);

    // Returns an eigen slice of even rows
    m.def("even_rows", [](py::EigenDRef<Eigen::MatrixXd> m) {
        return py::EigenDMap<Eigen::MatrixXd>(
                m.data(), (m.rows() + 1) / 2, m.cols(),
                py::EigenDStride(m.outerStride(), 2 * m.innerStride()));
    }, py::return_value_policy::reference);

    // Returns an eigen slice of even columns
    m.def("even_cols", [](py::EigenDRef<Eigen::MatrixXd> m) {
        return py::EigenDMap<Eigen::MatrixXd>(
                m.data(), m.rows(), (m.cols() + 1) / 2,
                py::EigenDStride(2 * m.outerStride(), m.innerStride()));
    }, py::return_value_policy::reference);

    // Returns diagonals: a vector-like object with an inner stride != 1
    m.def("diagonal", [](const Eigen::Ref<const Eigen::MatrixXd> &x) { return x.diagonal(); });
    m.def("diagonal_1", [](const Eigen::Ref<const Eigen::MatrixXd> &x) { return x.diagonal<1>(); });
    m.def("diagonal_n", [](const Eigen::Ref<const Eigen::MatrixXd> &x, int index) { return x.diagonal(index); });

    // Return a block of a matrix (gives non-standard strides)
    m.def("block", [](const Eigen::Ref<const Eigen::MatrixXd> &x, int start_row, int start_col, int block_rows, int block_cols) {
        return x.block(start_row, start_col, block_rows, block_cols);
    });

    // test_eigen_return_references, test_eigen_keepalive
    // return value referencing/copying tests:
    class ReturnTester {
        Eigen::MatrixXd mat = create();
    public:
        ReturnTester() { print_created(this); }
        ~ReturnTester() { print_destroyed(this); }
        static Eigen::MatrixXd create() { return Eigen::MatrixXd::Ones(10, 10); }
        static const Eigen::MatrixXd createConst() { return Eigen::MatrixXd::Ones(10, 10); }
        Eigen::MatrixXd &get() { return mat; }
        Eigen::MatrixXd *getPtr() { return &mat; }
        const Eigen::MatrixXd &view() { return mat; }
        const Eigen::MatrixXd *viewPtr() { return &mat; }
        Eigen::Ref<Eigen::MatrixXd> ref() { return mat; }
        Eigen::Ref<const Eigen::MatrixXd> refConst() { return mat; }
        Eigen::Block<Eigen::MatrixXd> block(int r, int c, int nrow, int ncol) { return mat.block(r, c, nrow, ncol); }
        Eigen::Block<const Eigen::MatrixXd> blockConst(int r, int c, int nrow, int ncol) const { return mat.block(r, c, nrow, ncol); }
        py::EigenDMap<Eigen::Matrix2d> corners() { return py::EigenDMap<Eigen::Matrix2d>(mat.data(),
                    py::EigenDStride(mat.outerStride() * (mat.outerSize()-1), mat.innerStride() * (mat.innerSize()-1))); }
        py::EigenDMap<const Eigen::Matrix2d> cornersConst() const { return py::EigenDMap<const Eigen::Matrix2d>(mat.data(),
                    py::EigenDStride(mat.outerStride() * (mat.outerSize()-1), mat.innerStride() * (mat.innerSize()-1))); }
    };
    using rvp = py::return_value_policy;
    py::class_<ReturnTester>(m, "ReturnTester")
        .def(py::init<>())
        .def_static("create", &ReturnTester::create)
        .def_static("create_const", &ReturnTester::createConst)
        .def("get", &ReturnTester::get, rvp::reference_internal)
        .def("get_ptr", &ReturnTester::getPtr, rvp::reference_internal)
        .def("view", &ReturnTester::view, rvp::reference_internal)
        .def("view_ptr", &ReturnTester::view, rvp::reference_internal)
        .def("copy_get", &ReturnTester::get)   // Default rvp: copy
        .def("copy_view", &ReturnTester::view) //         "
        .def("ref", &ReturnTester::ref) // Default for Ref is to reference
        .def("ref_const", &ReturnTester::refConst) // Likewise, but const
        .def("ref_safe", &ReturnTester::ref, rvp::reference_internal)
        .def("ref_const_safe", &ReturnTester::refConst, rvp::reference_internal)
        .def("copy_ref", &ReturnTester::ref, rvp::copy)
        .def("copy_ref_const", &ReturnTester::refConst, rvp::copy)
        .def("block", &ReturnTester::block)
        .def("block_safe", &ReturnTester::block, rvp::reference_internal)
        .def("block_const", &ReturnTester::blockConst, rvp::reference_internal)
        .def("copy_block", &ReturnTester::block, rvp::copy)
        .def("corners", &ReturnTester::corners, rvp::reference_internal)
        .def("corners_const", &ReturnTester::cornersConst, rvp::reference_internal)
        ;

    // test_special_matrix_objects
    // Returns a DiagonalMatrix with diagonal (1,2,3,...)
    m.def("incr_diag", [](int k) {
        Eigen::DiagonalMatrix<int, Eigen::Dynamic> m(k);
        for (int i = 0; i < k; i++) m.diagonal()[i] = i+1;
        return m;
    });

    // Returns a SelfAdjointView referencing the lower triangle of m
    m.def("symmetric_lower", [](const Eigen::MatrixXi &m) {
            return m.selfadjointView<Eigen::Lower>();
    });
    // Returns a SelfAdjointView referencing the lower triangle of m
    m.def("symmetric_upper", [](const Eigen::MatrixXi &m) {
            return m.selfadjointView<Eigen::Upper>();
    });

    // Test matrix for various functions below.
    Eigen::MatrixXf mat(5, 6);
    mat << 0,  3,  0,  0,  0, 11,
           22, 0,  0,  0, 17, 11,
           7,  5,  0,  1,  0, 11,
           0,  0,  0,  0,  0, 11,
           0,  0, 14,  0,  8, 11;

    // test_fixed, and various other tests
    m.def("fixed_r", [mat]() -> FixedMatrixR { return FixedMatrixR(mat); });
    m.def("fixed_r_const", [mat]() -> const FixedMatrixR { return FixedMatrixR(mat); });
    m.def("fixed_c", [mat]() -> FixedMatrixC { return FixedMatrixC(mat); });
    m.def("fixed_copy_r", [](const FixedMatrixR &m) -> FixedMatrixR { return m; });
    m.def("fixed_copy_c", [](const FixedMatrixC &m) -> FixedMatrixC { return m; });
    // test_mutator_descriptors
    m.def("fixed_mutator_r", [](Eigen::Ref<FixedMatrixR>) {});
    m.def("fixed_mutator_c", [](Eigen::Ref<FixedMatrixC>) {});
    m.def("fixed_mutator_a", [](py::EigenDRef<FixedMatrixC>) {});
    // test_dense
    m.def("dense_r", [mat]() -> DenseMatrixR { return DenseMatrixR(mat); });
    m.def("dense_c", [mat]() -> DenseMatrixC { return DenseMatrixC(mat); });
    m.def("dense_copy_r", [](const DenseMatrixR &m) -> DenseMatrixR { return m; });
    m.def("dense_copy_c", [](const DenseMatrixC &m) -> DenseMatrixC { return m; });
    // test_sparse, test_sparse_signature
    m.def("sparse_r", [mat]() -> SparseMatrixR { return Eigen::SparseView<Eigen::MatrixXf>(mat); });
    m.def("sparse_c", [mat]() -> SparseMatrixC { return Eigen::SparseView<Eigen::MatrixXf>(mat); });
    m.def("sparse_copy_r", [](const SparseMatrixR &m) -> SparseMatrixR { return m; });
    m.def("sparse_copy_c", [](const SparseMatrixC &m) -> SparseMatrixC { return m; });
    // test_partially_fixed
    m.def("partial_copy_four_rm_r", [](const FourRowMatrixR &m) -> FourRowMatrixR { return m; });
    m.def("partial_copy_four_rm_c", [](const FourColMatrixR &m) -> FourColMatrixR { return m; });
    m.def("partial_copy_four_cm_r", [](const FourRowMatrixC &m) -> FourRowMatrixC { return m; });
    m.def("partial_copy_four_cm_c", [](const FourColMatrixC &m) -> FourColMatrixC { return m; });

    // test_cpp_casting
    // Test that we can cast a numpy object to a Eigen::MatrixXd explicitly
    m.def("cpp_copy", [](py::handle m) { return m.cast<Eigen::MatrixXd>()(1, 0); });
    m.def("cpp_ref_c", [](py::handle m) { return m.cast<Eigen::Ref<Eigen::MatrixXd>>()(1, 0); });
    m.def("cpp_ref_r", [](py::handle m) { return m.cast<Eigen::Ref<MatrixXdR>>()(1, 0); });
    m.def("cpp_ref_any", [](py::handle m) { return m.cast<py::EigenDRef<Eigen::MatrixXd>>()(1, 0); });


    // test_nocopy_wrapper
    // Test that we can prevent copying into an argument that would normally copy: First a version
    // that would allow copying (if types or strides don't match) for comparison:
    m.def("get_elem", &get_elem);
    // Now this alternative that calls the tells pybind to fail rather than copy:
    m.def("get_elem_nocopy", [](Eigen::Ref<const Eigen::MatrixXd> m) -> double { return get_elem(m); },
            py::arg().noconvert());
    // Also test a row-major-only no-copy const ref:
    m.def("get_elem_rm_nocopy", [](Eigen::Ref<const Eigen::Matrix<long, -1, -1, Eigen::RowMajor>> &m) -> long { return m(2, 1); },
            py::arg().noconvert());

    // test_issue738
    // Issue #738: 1xN or Nx1 2D matrices were neither accepted nor properly copied with an
    // incompatible stride value on the length-1 dimension--but that should be allowed (without
    // requiring a copy!) because the stride value can be safely ignored on a size-1 dimension.
    m.def("iss738_f1", &adjust_matrix<const Eigen::Ref<const Eigen::MatrixXd> &>, py::arg().noconvert());
    m.def("iss738_f2", &adjust_matrix<const Eigen::Ref<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>> &>, py::arg().noconvert());

    // test_issue1105
    // Issue #1105: when converting from a numpy two-dimensional (Nx1) or (1xN) value into a dense
    // eigen Vector or RowVector, the argument would fail to load because the numpy copy would fail:
    // numpy won't broadcast a Nx1 into a 1-dimensional vector.
    m.def("iss1105_col", [](Eigen::VectorXd) { return true; });
    m.def("iss1105_row", [](Eigen::RowVectorXd) { return true; });

    // test_named_arguments
    // Make sure named arguments are working properly:
    m.def("matrix_multiply", [](const py::EigenDRef<const Eigen::MatrixXd> A, const py::EigenDRef<const Eigen::MatrixXd> B)
            -> Eigen::MatrixXd {
        if (A.cols() != B.rows()) throw std::domain_error("Nonconformable matrices!");
        return A * B;
    }, py::arg("A"), py::arg("B"));

    // test_custom_operator_new
    py::class_<CustomOperatorNew>(m, "CustomOperatorNew")
        .def(py::init<>())
        .def_readonly("a", &CustomOperatorNew::a)
        .def_readonly("b", &CustomOperatorNew::b);

    // test_eigen_ref_life_support
    // In case of a failure (the caster's temp array does not live long enough), creating
    // a new array (np.ones(10)) increases the chances that the temp array will be garbage
    // collected and/or that its memory will be overridden with different values.
    m.def("get_elem_direct", [](Eigen::Ref<const Eigen::VectorXd> v) {
        py::module::import("numpy").attr("ones")(10);
        return v(5);
    });
    m.def("get_elem_indirect", [](std::vector<Eigen::Ref<const Eigen::VectorXd>> v) {
        py::module::import("numpy").attr("ones")(10);
        return v[0](5);
    });
}
void NaturalSpline1<Real>::CreatePeriodicSpline ()
{
	mB = new1<Real>(mNumSegments);
	mC = new1<Real>(mNumSegments);
	mD = new1<Real>(mNumSegments);

#if 1
	// Solving the system using a standard linear solver appears to be
	// numerically stable.
	const int size = 4*mNumSegments;
	GMatrix<Real> mat(size,size);
	GVector<Real> rhs(size);
	int i, j, k;
	Real delta, delta2, delta3;
	for (i = 0, j = 0; i < mNumSegments-1; ++i, j += 4)
	{
		delta = mTimes[i+1] - mTimes[i];
		delta2 = delta*delta;
		delta3 = delta*delta2;

		mat[j+0][j+0] = (Real)1;
		mat[j+0][j+1] = (Real)0;
		mat[j+0][j+2] = (Real)0;
		mat[j+0][j+3] = (Real)0;
		mat[j+1][j+0] = (Real)1;
		mat[j+1][j+1] = delta;
		mat[j+1][j+2] = delta2;
		mat[j+1][j+3] = delta3;
		mat[j+2][j+0] = (Real)0;
		mat[j+2][j+1] = (Real)1;
		mat[j+2][j+2] = ((Real)2)*delta;
		mat[j+2][j+3] = ((Real)3)*delta2;
		mat[j+3][j+0] = (Real)0;
		mat[j+3][j+1] = (Real)0;
		mat[j+3][j+2] = (Real)1;
		mat[j+3][j+3] = ((Real)3)*delta;

		k = j + 4;
		mat[j+0][k+0] = (Real)0;
		mat[j+0][k+1] = (Real)0;
		mat[j+0][k+2] = (Real)0;
		mat[j+0][k+3] = (Real)0;
		mat[j+1][k+0] = (Real)-1;
		mat[j+1][k+1] = (Real)0;
		mat[j+1][k+2] = (Real)0;
		mat[j+1][k+3] = (Real)0;
		mat[j+2][k+0] = (Real)0;
		mat[j+2][k+1] = (Real)-1;
		mat[j+2][k+2] = (Real)0;
		mat[j+2][k+3] = (Real)0;
		mat[j+3][k+0] = (Real)0;
		mat[j+3][k+1] = (Real)0;
		mat[j+3][k+2] = (Real)-1;
		mat[j+3][k+3] = (Real)0;
	}

	delta = mTimes[i+1] - mTimes[i];
	delta2 = delta*delta;
	delta3 = delta*delta2;

	mat[j+0][j+0] = (Real)1;
	mat[j+0][j+1] = (Real)0;
	mat[j+0][j+2] = (Real)0;
	mat[j+0][j+3] = (Real)0;
	mat[j+1][j+0] = (Real)1;
	mat[j+1][j+1] = delta;
	mat[j+1][j+2] = delta2;
	mat[j+1][j+3] = delta3;
	mat[j+2][j+0] = (Real)0;
	mat[j+2][j+1] = (Real)1;
	mat[j+2][j+2] = ((Real)2)*delta;
	mat[j+2][j+3] = ((Real)3)*delta2;
	mat[j+3][j+0] = (Real)0;
	mat[j+3][j+1] = (Real)0;
	mat[j+3][j+2] = (Real)1;
	mat[j+3][j+3] = ((Real)3)*delta;

	k = 0;
	mat[j+0][k+0] = (Real)0;
	mat[j+0][k+1] = (Real)0;
	mat[j+0][k+2] = (Real)0;
	mat[j+0][k+3] = (Real)0;
	mat[j+1][k+0] = (Real)-1;
	mat[j+1][k+1] = (Real)0;
	mat[j+1][k+2] = (Real)0;
	mat[j+1][k+3] = (Real)0;
	mat[j+2][k+0] = (Real)0;
	mat[j+2][k+1] = (Real)-1;
	mat[j+2][k+2] = (Real)0;
	mat[j+2][k+3] = (Real)0;
	mat[j+3][k+0] = (Real)0;
	mat[j+3][k+1] = (Real)0;
	mat[j+3][k+2] = (Real)-1;
	mat[j+3][k+3] = (Real)0;

	for (i = 0, j = 0; i < mNumSegments; ++i, j += 4)
	{
		rhs[j+0] = mA[i];
		rhs[j+1] = (Real)0;
		rhs[j+2] = (Real)0;
		rhs[j+3] = (Real)0;
	}

	GVector<Real> coeff(size);
	bool solved = LinearSystem<Real>().Solve(mat, rhs, coeff);
	assertion(solved, "Failed to solve linear system\n");
	WM5_UNUSED(solved);

	for (i = 0, j = 0; i < mNumSegments; ++i)
	{
		j++;
		mB[i] = coeff[j++];
		mC[i] = coeff[j++];
		mD[i] = coeff[j++];
	}
#endif

#if 0
	// Solving the system using the equations derived in the PDF
	// "Fitting a Natural Spline to Samples of the Form (t,f(t))"
	// is ill-conditioned.  TODO: Find a way to row-reduce the matrix of the
	// PDF in a numerically stable manner yet retaining the O(n) asymptotic
	// behavior.

	// Compute the inverses M[i]^{-1}.
	const int numSegmentsM1 = mNumSegments - 1;
	Matrix4<Real>* invM = new1<Matrix4<Real> >(numSegmentsM1);

	Real delta;
	int i;
	for (i = 0; i < numSegmentsM1; i++)
	{
		delta = mTimes[i+1] - mTimes[i];
		Real invDelta1 = ((Real)1)/delta;
		Real invDelta2 = invDelta1/delta;
		Real invDelta3 = invDelta2/delta;

		Matrix4<Real>& invMi = invM[i];
		invMi[0][0] = (Real)1;
		invMi[0][1] = (Real)0;
		invMi[0][2] = (Real)0;
		invMi[0][3] = (Real)0;
		invMi[1][0] = ((Real)(-3))*invDelta1;
		invMi[1][1] = ((Real)3)*invDelta1;
		invMi[1][2] = (Real)(-2);
		invMi[1][3] = delta;
		invMi[2][0] = ((Real)3)*invDelta2;
		invMi[2][1] = ((Real)(-3))*invDelta2;
		invMi[2][2] = ((Real)3)*invDelta1;
		invMi[2][3] = (Real)(-2);
		invMi[3][0] = -invDelta3;
		invMi[3][1] = invDelta3;
		invMi[3][2] = -invDelta2;
		invMi[3][3] = invDelta1;
	}

	// Matrix M[n-1].
	delta = mTimes[i+1] - mTimes[i];
	Real delta2 = delta*delta;
	Real delta3 = delta2*delta;
	Matrix4<Real> lastM
		(
		(Real)1, (Real)0, (Real)0, (Real)0,
		(Real)1, delta, delta2, delta3,
		(Real)0, (Real)1, ((Real)2)*delta, ((Real)3)*delta2,
		(Real)0, (Real)0, (Real)1, ((Real)3)*delta
		);

	// Matrix L.
	Matrix4<Real> LMat
		(
		(Real)0, (Real)0, (Real)0, (Real)0,
		(Real)1, (Real)0, (Real)0, (Real)0,
		(Real)0, (Real)1, (Real)0, (Real)0,
		(Real)0, (Real)0, (Real)1, (Real)0
		);

	// Vector U.
	Vector<4,Real> U((Real)1,(Real)0,(Real)0,(Real)0);

	// Initialize P = L and Q = f[n-2]*U.
	Matrix4<Real> P = LMat;

	const int numSegmentsM2 = mNumSegments - 2;
	Vector<4,Real> Q = mA[numSegmentsM2]*U;

	// Compute P and Q.
	for (i = numSegmentsM2; i >= 0; --i)
	{
		// Matrix L*M[i]^{-1}.
		Matrix4<Real> LMInv = LMat*invM[i];

		// Update P.
		P = LMInv*P;

		// Update Q.
		if (i > 0)
		{
			Q = mA[i-1]*U + LMInv*Q;
		}
		else
		{
			Q = mA[numSegmentsM1]*U + LMInv*Q;
		}
	}

	// Final update of P.
	P = lastM - P;

	// Compute P^{-1}.
	Matrix4<Real> invP = P.Inverse();

	// Compute K[n-1].
	Vector<4,Real> coeff = invP*Q;
	mB[numSegmentsM1] = coeff[1];
	mC[numSegmentsM1] = coeff[2];
	mD[numSegmentsM1] = coeff[3];

	// Back substitution for the other K[i].
	for (i = numSegmentsM2; i >= 0; i--)
	{
		coeff = invM[i]*(mA[i]*U + LMat*coeff);
		mB[i] = coeff[1];
		mC[i] = coeff[2];
		mD[i] = coeff[3];
	}

	delete1(invM);
#endif
}
void SkSVGPaint::setSave(SkSVGParser& parser) {
    SkTDArray<SkString*> clips;
    SkSVGPaint* walking = parser.fHead;
    int index;
    SkMatrix sum;
    sum.reset();
    while (walking != NULL) {
        for (index = kInitial + 1; index < kTerminal; index++) {
            SkString* lastAttr = (*walking)[index];
            if (lastAttr->size() == 0)
                continue;
            if (index == kTransform) {
                const char* str = lastAttr->c_str();
                SkASSERT(strncmp(str, "matrix(", 7) == 0);
                str += 6;
                const char* strEnd = strrchr(str, ')');
                SkASSERT(strEnd != NULL);
                SkString mat(str, strEnd - str);
                SkSVGParser::ConvertToArray(mat);
                SkScalar values[6];
                SkParse::FindScalars(mat.c_str() + 1, values, 6);
                SkMatrix matrix;
                matrix.reset();
                matrix.setScaleX(values[0]);
                matrix.setSkewY(values[1]);
                matrix.setSkewX(values[2]);
                matrix.setScaleY(values[3]);
                matrix.setTranslateX(values[4]);
                matrix.setTranslateY(values[5]);
                sum.setConcat(matrix, sum);
                continue;
            }
            if ( index == kClipPath)
                *clips.insert(0) = lastAttr;
        }
        walking = walking->fNext;
    }
    if ((sum == parser.fLastTransform) == false) {
        SkMatrix inverse;
        bool success = parser.fLastTransform.invert(&inverse);
        SkASSERT(success == true);
        SkMatrix output;
        output.setConcat(inverse, sum);
        parser.fLastTransform = sum;
        SkString outputStr;
        outputStr.appendUnichar('[');
        outputStr.appendScalar(output.getScaleX());
        outputStr.appendUnichar(',');
        outputStr.appendScalar(output.getSkewX());
        outputStr.appendUnichar(',');
        outputStr.appendScalar(output.getTranslateX());
        outputStr.appendUnichar(',');
        outputStr.appendScalar(output.getSkewY());
        outputStr.appendUnichar(',');
        outputStr.appendScalar(output.getScaleY());
        outputStr.appendUnichar(',');
        outputStr.appendScalar(output.getTranslateY());
        outputStr.appendUnichar(',');
        outputStr.appendScalar(output.getPerspX());
        outputStr.appendUnichar(',');
        outputStr.appendScalar(output.getPerspY());
        outputStr.append(",1]");
        parser._startElement("matrix");
        parser._addAttributeLen("matrix", outputStr.c_str(), outputStr.size());
        parser._endElement();
    }
#if 0   // incomplete
    if (parser.fTransformClips.size() > 0) {
        // need to reset the clip when the 'g' scope is ended
        parser._startElement("add");
        const char* start = strchr(current->f_clipPath.c_str(), '#') + 1;
        SkASSERT(start);
        parser._addAttributeLen("use", start, strlen(start) - 1);
        parser._endElement();   // clip
    }
#endif
}
Example #9
0
int getStaticTransform(string calib_filename, geometry_msgs::TransformStamped *ros_msgTf, std_msgs::Header *header, string camera_name)
{
    ifstream calib_file(calib_filename.c_str());
    if (!calib_file.is_open())
        return false;

    ROS_DEBUG_STREAM("Reading transformation" << (camera_name.empty()? string(""): string(" for camera ")+camera_name) << " from " << calib_filename);

    typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
    boost::char_separator<char> sep{" "};

    string line="";
    char index=0;
    tokenizer::iterator token_iterator;

    tf2::Transform t;

    while (getline(calib_file,line))
    {
        // Parse string phase 1, tokenize it using Boost.
        tokenizer tok(line,sep);

        // Move the iterator at the beginning of the tokenize vector and check for T/R matrices.
        token_iterator=tok.begin();
        if (strcmp((*token_iterator).c_str(),((string)(string("T")+(camera_name.empty()?string(""):string("_")+camera_name)+string(":"))).c_str())==0) //Calibration Matrix
        {
            index=0; //should be 3 at the end
            ROS_DEBUG_STREAM("T" << (camera_name.empty()?string(""):string("_")+camera_name));
            double T[3];
            for (token_iterator++; token_iterator != tok.end(); token_iterator++)
            {
                // std::cout << *token_iterator << '\n';
                T[index++]=boost::lexical_cast<double>(*token_iterator);
            }
            t.setOrigin(tf2::Vector3(T[0],T[1],T[2]));
        }

        // EXPERIMENTAL: use with unrectified images

        //        token_iterator=tok.begin();
        //        if (strcmp((*token_iterator).c_str(),((string)(string("D_")+camera_name+string(":"))).c_str())==0) //Distortion Coefficients
        //        {
        //            index=0; //should be 5 at the end
        //            ROS_DEBUG_STREAM("D_" << camera_name);
        //            for (token_iterator++; token_iterator != tok.end(); token_iterator++)
        //            {
        ////                std::cout << *token_iterator << '\n';
        //                D[index++]=boost::lexical_cast<double>(*token_iterator);
        //            }
        //        }

        token_iterator=tok.begin();
        if (strcmp((*token_iterator).c_str(),((string)(string("R")+(camera_name.empty()?string(""):string("_")+camera_name)+string(":"))).c_str())==0) //Rectification Matrix
        {
            index=0; //should be 9 at the end
            ROS_DEBUG_STREAM("R" << (camera_name.empty()?string(""):string("_")+camera_name));
            double R[9];
            for (token_iterator++; token_iterator != tok.end(); token_iterator++)
            {
                // std::cout << *token_iterator << '\n';
                R[index++]=boost::lexical_cast<double>(*token_iterator);
            }
            tf2::Matrix3x3 mat(R[0],R[1],R[2],
                              R[3],R[4],R[5],
                              R[6],R[7],R[8]);
            tf2::Quaternion quat;  mat.getRotation(quat);
            t.setRotation(quat);
        }

        // token_iterator=tok.begin();
        // if (strcmp((*token_iterator).c_str(),((string)(string("P_rect_")+camera_name+string(":"))).c_str())==0) //Projection Matrix Rectified
        // {
        //     index=0; //should be 12 at the end
        //     ROS_DEBUG_STREAM("P_rect_" << camera_name);
        //     for (token_iterator++; token_iterator != tok.end(); token_iterator++)
        //     {
        //         //std::cout << *token_iterator << '\n';
        //         P[index++]=boost::lexical_cast<double>(*token_iterator);
        //     }
        // }

    }

    // fill up tf message
    ros_msgTf->header.frame_id = header->frame_id;
    ros_msgTf->header.stamp = header->stamp;

    // Invert transform to match the tf tree structure: oxts -> velodyne -> cam00 -> cam{01,02,03}
    tf2::convert(t.inverse(),ros_msgTf->transform);

    ROS_DEBUG_STREAM("... ok");
    return true;
}
////////////////////////////
// Unpack plugins to 0.csx, 1.csx etc.
bool CRuntime::UnpackPlugins(int startresource)
{
	// Store the old current directory, but look for DLLs for plugin loading etc in the temp directory
	char oldCurrentDirectory[MAX_PATH];
	GetCurrentDirectory(MAX_PATH, oldCurrentDirectory);
	SetCurrentDirectory(tempDir);

	// Get number of plugins
	char strbuf[256];

	if (LoadString(GetModuleHandle(NULL), IDS_numPlugins, strbuf, 256) == 0)
		throw runtime_error("Error unpacking resources (1)");

	numPlugins = atoi(strbuf);

	// If over 1000 plugins or negative, probably undefined string - show error
	if (numPlugins < 0 || numPlugins > 1000)
		throw runtime_error("Error unpacking resources (2)");

	// Loop all plugin resources
	for (int i = 0; i < numPlugins; i++) 
	{
		CString curFile;
		curFile.Format("%s%d.csx", tempDir, i);

		// Create the file for writing
		FILE* f = fopen(CSTR(curFile), "wb");

		if (f == NULL) {
			CString msg;
			msg.Format("Error unpacking resources (3): could not create file %s (%d)", curFile, errno);
			throw runtime_error((const char*)msg);
		}

		// Get the resource data
		HRSRC resLoad = FindResource(NULL, MAKEINTRESOURCE(startresource + i), "DLLBLOCK");

		int e = GetLastError();
		CString s; s.Format("Error unpacking resources (4): %d", e);

		if (resLoad == NULL) throw runtime_error((const char*)s);

		HGLOBAL resData = LoadResource(NULL, resLoad);
		LPCSTR dllData = (LPCSTR)LockResource(resData);

		// Write the resource data to disk
		fwrite(dllData, 1, SizeofResource(NULL, resLoad), f);
		fclose(f);
		FreeResource(resData);

		// Load up this module
		PluginModule plugin;
		plugin.module = LoadLibrary(CSTR(curFile));

		if (plugin.module == NULL) {
			CString msg;
			msg.Format("Failed to load plugin %s (%d)", curFile, GetLastError());
			throw runtime_error((const char*)msg);
		}

		//DWORD e = GetLastError();

		// Get proc addresses
		plugin.RTCreateObject = (LPRTCREATEOBJECT)GetProcAddress(plugin.module, "RTCreateObject");
		plugin.RTDestroyObject = (LPRTDESTROYOBJECT)GetProcAddress(plugin.module, "RTDestroyObject");
		plugin.RTWindowProc = (PLUGINWNDPROC)GetProcAddress(plugin.module, "RTWindowProc");

		// Initialise
		LPRTDLLLOAD RTDllLoad = (LPRTDLLLOAD)GetProcAddress(plugin.module, "RTDllLoad");

		// Handle vectors
		PluginModuleVectors emv;
		pluginvecs.push_back(emv);
		PluginModuleVectors* pv = &(pluginvecs.back());

		MicroAceTool mat(&(pv->CndRoutines), &(pv->ActRoutines), &(pv->ExpRoutines), &(pv->ExpNames));
	
		// Load vectors
		plugin.ideFlags = RTDllLoad(&mat);

		// Get the plugin routine tables
		plugin.pvCndRoutines = &(pv->CndRoutines);
		plugin.pvActRoutines = &(pv->ActRoutines);
		plugin.pvExpRoutines = &(pv->ExpRoutines);
		plugin.pvExpNames = &(pv->ExpNames);

		// Make all expression names lowercase
		vector<CString>::iterator j;
		for (j = plugin.pvExpNames->begin(); j != plugin.pvExpNames->end(); j++)
			j->MakeLower();
		
		// Add to plugins list
		plugins.push_back(plugin);
	}

	// Restore current directory
	SetCurrentDirectory(oldCurrentDirectory);

	return true;
}
Example #11
0
File: irdwt.c Project: EQ4/rwt
void irdwt(double *x, size_t nrows, size_t ncols, double *h, int ncoeff, int levels, double *y_low, double *y_high) {
  double  *coeff_low, *coeff_high, *y_dummy_low_low, *y_dummy_low_high, *y_dummy_high_low;
  double *y_dummy_high_high, *x_dummy_low , *x_dummy_high, *x_high;
  long i;
  int current_level, three_n_L, ncoeff_minus_one, sample_f;
  size_t current_rows, current_cols, column_cursor, column_blocks_per_row;
  size_t idx_rows, idx_cols, n_r, n_c;
  size_t row_blocks_per_column, column_cursor_plus_n, column_cursor_plus_double_n;

  irdwt_allocate(nrows, ncols, ncoeff, &x_high, &x_dummy_low, &x_dummy_high, &y_dummy_low_low, 
    &y_dummy_low_high, &y_dummy_high_low, &y_dummy_high_high, &coeff_low, &coeff_high);
  irdwt_coefficients(ncoeff, h, &coeff_low, &coeff_high);
 
  if (ncols==1) {
    ncols = nrows;
    nrows = 1;
  }
  /* analysis lowpass and highpass */
  
  three_n_L = 3*ncols*levels;
  ncoeff_minus_one = ncoeff - 1;
  /*! we calculate sample_f = 2^(levels - 1) with a loop since that is actually the recommended method for whole numbers */
  sample_f = 1;
  for (i=1; i<levels; i++)
    sample_f = sample_f*2;

  current_rows = nrows/sample_f;
  current_cols = ncols/sample_f;
  /* restore y_low in x */
  for (i=0; i<nrows*ncols; i++)
    x[i] = y_low[i];
  
  /* main loop */
  for (current_level=levels; current_level >= 1; current_level--) {
    /* actual (level dependent) column offset */
    if (nrows==1)
      column_cursor = ncols*(current_level-1);
    else
      column_cursor = 3*ncols*(current_level-1);
    column_cursor_plus_n = column_cursor + ncols;
    column_cursor_plus_double_n = column_cursor_plus_n + ncols;
    
    /* go by columns in case of a 2D signal*/
    if (nrows>1) {
      row_blocks_per_column = nrows/current_rows;   /* # of row blocks per column */
      for (idx_cols=0; idx_cols<ncols; idx_cols++) {          /* loop over column */
	for (n_r=0; n_r<row_blocks_per_column; n_r++) { /* loop within one column */
	  /* store in dummy variables */
	  idx_rows = -sample_f + n_r;
	  for (i=0; i<current_rows; i++) {    
	    idx_rows = idx_rows + sample_f;
	    y_dummy_low_low[i+ncoeff_minus_one]   = mat(x,      idx_rows, idx_cols,                               nrows, ncols);
	    y_dummy_low_high[i+ncoeff_minus_one]  = mat(y_high, idx_rows, idx_cols + column_cursor,               nrows, three_n_L);
	    y_dummy_high_low[i+ncoeff_minus_one]  = mat(y_high, idx_rows, idx_cols + column_cursor_plus_n,        nrows, three_n_L);
	    y_dummy_high_high[i+ncoeff_minus_one] = mat(y_high, idx_rows, idx_cols + column_cursor_plus_double_n, nrows, three_n_L);
	  }
	  /* perform filtering and adding: first LL/LH, then HL/HH */
	  irdwt_convolution(x_dummy_low,  current_rows, coeff_low, coeff_high, ncoeff, y_dummy_low_low,  y_dummy_low_high); 
	  irdwt_convolution(x_dummy_high, current_rows, coeff_low, coeff_high, ncoeff, y_dummy_high_low, y_dummy_high_high); 
	  /* store dummy variables in matrices */
	  idx_rows = -sample_f + n_r;
	  for (i=0; i<current_rows; i++) {
	    idx_rows = idx_rows + sample_f;
	    mat(x,      idx_rows, idx_cols, nrows, ncols) = x_dummy_low[i];
	    mat(x_high, idx_rows, idx_cols, nrows, ncols) = x_dummy_high[i];
	  }
	}
      }
    }
    
    /* go by rows */
    column_blocks_per_row = ncols/current_cols; /* # of column blocks per row */
    for (idx_rows=0; idx_rows<nrows; idx_rows++) {          /* loop over rows */
      for (n_c=0; n_c<column_blocks_per_row; n_c++) {  /* loop within one row */      
	/* store in dummy variable */
	idx_cols = -sample_f + n_c;
	for  (i=0; i<current_cols; i++) {    
	  idx_cols = idx_cols + sample_f;
	  y_dummy_low_low[i+ncoeff_minus_one] = mat(x, idx_rows, idx_cols, nrows, ncols);  
	  if (nrows>1)
	    y_dummy_high_high[i+ncoeff_minus_one] = mat(x_high, idx_rows, idx_cols,                 nrows, ncols);
	  else
            y_dummy_high_high[i+ncoeff_minus_one] = mat(y_high, idx_rows, idx_cols + column_cursor, nrows, three_n_L);
	} 
	/* perform filtering lowpass/highpass */
	irdwt_convolution(x_dummy_low, current_cols, coeff_low, coeff_high, ncoeff, y_dummy_low_low, y_dummy_high_high); 
	/* restore dummy variables in matrices */
	idx_cols = -sample_f + n_c;
	for (i=0; i<current_cols; i++) {    
	  idx_cols = idx_cols + sample_f;
	  mat(x, idx_rows, idx_cols, nrows, ncols) = x_dummy_low[i];  
	}
      }
    }
    sample_f = sample_f/2;
    current_rows = current_rows*2;
    current_cols = current_cols*2;
  }
  irdwt_free(&x_dummy_low, &x_dummy_high, &y_dummy_low_low, &y_dummy_low_high, &y_dummy_high_low, &y_dummy_high_high, &coeff_low, &coeff_high);
}
Example #12
0
//----------------------------------------------------------------------------
PX2::Transform SceneBuilder::GetLocalTransform (INode *node, TimeValue time)
{
	// 计算节点的本地变换。Max节点的变换方法提供的节点的世界变换,所以我们
	// 必须做一些操纵去获得节点的本地变换。

	Matrix3 maxLocal = node->GetObjTMAfterWSM(time) *
		Inverse(node->GetParentNode()->GetObjTMAfterWSM(time));

	// 分解变换
	AffineParts affParts;
	decomp_affine(maxLocal, &affParts);

	// Position
	bool isTranslationZero = 
		fabsf(affParts.t.x) < MIN_DIFFERENCE &&
		fabsf(affParts.t.y) < MIN_DIFFERENCE &&
		fabsf(affParts.t.z) < MIN_DIFFERENCE;

	// Rotation
	float qSign = (affParts.q.w >= 0.0f ? 1.0f : -1.0f);
	bool isRotationIndentity = 
		fabsf(qSign*affParts.q.w - 1.0f) < MIN_DIFFERENCE &&
		fabsf(affParts.q.x) < MIN_DIFFERENCE &&
		fabsf(affParts.q.y) < MIN_DIFFERENCE &&
		fabsf(affParts.q.z) < MIN_DIFFERENCE;

	// Reflect
	bool hasReflection = (affParts.f < 0.0f);

	// Uniform scale
	bool isScaleUniform = (fabsf(affParts.k.x - affParts.k.y)<MIN_DIFFERENCE &&
		fabsf(affParts.k.y - affParts.k.z)<MIN_DIFFERENCE);

	// Unity scale
	bool isScaleUnity = isScaleUniform &&
		fabsf(affParts.k.x - 1.0f) < MIN_DIFFERENCE;

	// Scale orientation is identity?
	float uSign = (affParts.u.w >= 0.0f ? 1.0f : -1.0f);
	bool isOrientIndentity = isScaleUniform || (
		fabsf(uSign*affParts.u.w - 1.0f) < MIN_DIFFERENCE &&
		fabsf(affParts.u.x) < MIN_DIFFERENCE &&
		fabsf(affParts.u.y) < MIN_DIFFERENCE &&
		fabsf(affParts.u.z) < MIN_DIFFERENCE);

	// 计算Phoenix2等价变换
	PX2::Transform local;

	if (!isTranslationZero)
	{
		local.SetTranslate(PX2::APoint(affParts.t.x, affParts.t.y,
			affParts.t.z));
	}

	if (hasReflection)
	{
		affParts.k *= -1.0f;
	}
	
	if (isScaleUniform)
	{
		// 矩阵的形式为R*(s*I),s是统一缩放矩阵。
		if (!isRotationIndentity)
		{
			PX2::HMatrix rot;
			PX2::HQuaternion(affParts.q.w, -affParts.q.x, -affParts.q.y,
				-affParts.q.z).ToRotationMatrix(rot);
			local.SetRotate(rot);
		}

		if (!isScaleUnity)
		{
			local.SetUniformScale(affParts.k.x);
		}
	}
	else if (isOrientIndentity)
	{
		if (!isRotationIndentity)
		{
			PX2::HMatrix rot;
			PX2::HQuaternion(affParts.q.w, -affParts.q.x, -affParts.q.y,
				-affParts.q.z).ToRotationMatrix(rot);
			local.SetRotate(rot);
		}

		local.SetScale(PX2::APoint(affParts.k.x, affParts.k.y, affParts.k.z));
	}
	else
	{
		PX2::Matrix3f mat(
			maxLocal.GetAddr()[0][0],
			maxLocal.GetAddr()[1][0],
			maxLocal.GetAddr()[2][0],
			maxLocal.GetAddr()[0][1],
			maxLocal.GetAddr()[1][1],
			maxLocal.GetAddr()[2][1],
			maxLocal.GetAddr()[0][2],
			maxLocal.GetAddr()[1][2],
			maxLocal.GetAddr()[2][2]);

		local.SetMatrix(PX2::HMatrix(mat));
	}

	return local;
}
Example #13
0
float& Matrix3::operator()(int i, int j)
{
	return mat(i,j);
}
Example #14
0
//access operation
float  Matrix3::operator()(int i, int j) const
{
	return mat(i,j);
}
Example #15
0
Mat4 Mat4::getTransposed() const
{
    Mat4 mat(*this);
    mat.transpose();
    return mat;
}
Example #16
0
void CG::operator()(cudaColorSpinorField &x, cudaColorSpinorField &b) 
{
  int k=0;
  int rUpdate = 0;
    
  cudaColorSpinorField r(b);

  ColorSpinorParam param(x);
  param.create = QUDA_ZERO_FIELD_CREATE;
  cudaColorSpinorField y(b, param); 
  
  mat(r, x, y);
  zeroCuda(y);

  double r2 = xmyNormCuda(b, r);
  rUpdate ++;
  
  param.precision = invParam.cuda_prec_sloppy;
  cudaColorSpinorField Ap(x, param);
  cudaColorSpinorField tmp(x, param);
  cudaColorSpinorField tmp2(x, param); // only needed for clover and twisted mass

  cudaColorSpinorField *x_sloppy, *r_sloppy;
  if (invParam.cuda_prec_sloppy == x.Precision()) {
    param.create = QUDA_REFERENCE_FIELD_CREATE;
    x_sloppy = &x;
    r_sloppy = &r;
  } else {
    param.create = QUDA_COPY_FIELD_CREATE;
    x_sloppy = new cudaColorSpinorField(x, param);
    r_sloppy = new cudaColorSpinorField(r, param);
  }

  cudaColorSpinorField &xSloppy = *x_sloppy;
  cudaColorSpinorField &rSloppy = *r_sloppy;

  cudaColorSpinorField p(rSloppy);

  double r2_old;
  double src_norm = norm2(b);
  double stop = src_norm*invParam.tol*invParam.tol; // stopping condition of solver

  double alpha=0.0, beta=0.0;
  double pAp;

  double rNorm = sqrt(r2);
  double r0Norm = rNorm;
  double maxrx = rNorm;
  double maxrr = rNorm;
  double delta = invParam.reliable_delta;

  if (invParam.verbosity == QUDA_DEBUG_VERBOSE) {
    double x2 = norm2(x);
    double p2 = norm2(p);
    printf("CG: %d iterations, r2 = %e, x2 = %e, p2 = %e, alpha = %e, beta = %e\n", 
	   k, r2, x2, p2, alpha, beta);
  } else if (invParam.verbosity >= QUDA_VERBOSE) {
    printfQuda("CG: %d iterations, r2 = %e\n", k, r2);
  }

  quda::blas_flops = 0;

  stopwatchStart();
  while (r2 > stop && k<invParam.maxiter) {

    matSloppy(Ap, p, tmp, tmp2); // tmp as tmp
    
    pAp = reDotProductCuda(p, Ap);
    alpha = r2 / pAp;        
    r2_old = r2;
    r2 = axpyNormCuda(-alpha, Ap, rSloppy);

    // reliable update conditions
    rNorm = sqrt(r2);
    if (rNorm > maxrx) maxrx = rNorm;
    if (rNorm > maxrr) maxrr = rNorm;
    int updateX = (rNorm < delta*r0Norm && r0Norm <= maxrx) ? 1 : 0;
    int updateR = ((rNorm < delta*maxrr && r0Norm <= maxrr) || updateX) ? 1 : 0;
    
    if ( !(updateR || updateX)) {
      beta = r2 / r2_old;
      axpyZpbxCuda(alpha, p, xSloppy, rSloppy, beta);
    } else {
      axpyCuda(alpha, p, xSloppy);
      if (x.Precision() != xSloppy.Precision()) copyCuda(x, xSloppy);
      
      xpyCuda(x, y); // swap these around?
      mat(r, y, x); // here we can use x as tmp
      r2 = xmyNormCuda(b, r);
      if (x.Precision() != rSloppy.Precision()) copyCuda(rSloppy, r);            
      zeroCuda(xSloppy);

      rNorm = sqrt(r2);
      maxrr = rNorm;
      maxrx = rNorm;
      r0Norm = rNorm;      
      rUpdate++;

      beta = r2 / r2_old; 
      xpayCuda(rSloppy, beta, p);
    }

    k++;
    if (invParam.verbosity == QUDA_DEBUG_VERBOSE) {
      double x2 = norm2(x);
      double p2 = norm2(p);
      printf("CG: %d iterations, r2 = %e, x2 = %e, p2 = %e, alpha = %e, beta = %e\n", 
	     k, r2, x2, p2, alpha, beta);
    } else if (invParam.verbosity >= QUDA_VERBOSE) {
      printfQuda("CG: %d iterations, r2 = %e\n", k, r2);
    }
  }

  if (x.Precision() != xSloppy.Precision()) copyCuda(x, xSloppy);
  xpyCuda(y, x);

  invParam.secs = stopwatchReadSeconds();

  
  if (k==invParam.maxiter) 
    warningQuda("Exceeded maximum iterations %d", invParam.maxiter);

  if (invParam.verbosity >= QUDA_SUMMARIZE)
    printfQuda("CG: Reliable updates = %d\n", rUpdate);

  double gflops = (quda::blas_flops + mat.flops() + matSloppy.flops())*1e-9;
  reduceDouble(gflops);

  //  printfQuda("%f gflops\n", gflops / stopwatchReadSeconds());
  invParam.gflops = gflops;
  invParam.iter = k;

  quda::blas_flops = 0;

  if (invParam.verbosity >= QUDA_SUMMARIZE){
    mat(r, x, y);
    double true_res = xmyNormCuda(b, r);
    printfQuda("CG: Converged after %d iterations, relative residua: iterated = %e, true = %e\n", 
	       k, sqrt(r2/src_norm), sqrt(true_res / src_norm));    
  }

  if (invParam.cuda_prec_sloppy != x.Precision()) {
    delete r_sloppy;
    delete x_sloppy;
  }

  return;
}
void Quaternion::to_matrix( Matrix3* matp ) const
{
    Matrix3& mat = *matp;
    rotate_axes( *this, &mat( 0, 0 ), &mat( 1, 0 ), &mat( 2, 0 ) );
}
Example #18
0
int triang(int m, int n, int (*mat)(void *info, int k, int ind[],
      double val[]), void *info, double tol, int rn[], int cn[])
{     int head, i, j, jj, k, kk, ks, len, len2, next_j, ns, size;
      int *cind, *rind, *cnt, *ptr, *list, *prev, *next;
      double *cval, *rval, *big;
      char *flag;
      /* allocate working arrays */
      cind = talloc(1+m, int);
      cval = talloc(1+m, double);
      rind = talloc(1+n, int);
      rval = talloc(1+n, double);
      cnt = ptr = talloc(1+m, int);
      list = talloc(1+n, int);
      prev = talloc(1+n, int);
      next = talloc(1+n, int);
      big = talloc(1+n, double);
      flag = talloc(1+n, char);
      /*--------------------------------------------------------------*/
      /* build linked lists of columns having equal lengths           */
      /*--------------------------------------------------------------*/
      /* ptr[len], 0 <= len <= m, is number of first column of length
       * len;
       * next[j], 1 <= j <= n, is number of next column having the same
       * length as column j;
       * big[j], 1 <= j <= n, is maximal magnitude of elements in j-th
       * column */
      for (len = 0; len <= m; len++)
         ptr[len] = 0;
      for (j = 1; j <= n; j++)
      {  /* get j-th column */
         len = mat(info, -j, cind, cval);
         xassert(0 <= len && len <= m);
         /* add this column to beginning of list ptr[len] */
         next[j] = ptr[len];
         ptr[len] = j;
         /* determine maximal magnitude of elements in this column */
         big[j] = 0.0;
         for (k = 1; k <= len; k++)
         {  if (big[j] < fabs(cval[k]))
               big[j] = fabs(cval[k]);
         }
      }
      /*--------------------------------------------------------------*/
      /* build doubly linked list of columns ordered by decreasing    */
      /* column lengths                                               */
      /*--------------------------------------------------------------*/
      /* head is number of first column in the list;
       * prev[j], 1 <= j <= n, is number of column that precedes j-th
       * column in the list;
       * next[j], 1 <= j <= n, is number of column that follows j-th
       * column in the list */
      head = 0;
      for (len = 0; len <= m; len++)
      {  /* walk thru list of columns of length len */
         for (j = ptr[len]; j != 0; j = next_j)
         {  next_j = next[j];
            /* add j-th column to beginning of the column list */
            prev[j] = 0;
            next[j] = head;
            if (head != 0)
               prev[head] = j;
            head = j;
         }
      }
      /*--------------------------------------------------------------*/
      /* build initial singleton list                                 */
      /*--------------------------------------------------------------*/
      /* there are used two list of columns:
       * 1) doubly linked list of active columns, in which all columns
       *    are ordered by decreasing column lengths;
       * 2) singleton list; an active column is included in this list
       *    if it has at least one row singleton in active submatrix */
      /* flag[j], 1 <= j <= n, is a flag of j-th column:
       * 0  j-th column is inactive;
       * 1  j-th column is active;
       * 2  j-th column is active and has row singleton(s) */
      /* initially all columns are active */
      for (j = 1; j <= n; j++)
         flag[j] = 1;
      /* initialize row counts and build initial singleton list */
      /* cnt[i], 1 <= i <= m, is number of non-zeros, which i-th row
       * has in active submatrix;
       * ns is size of singleton list;
       * list[1], ..., list[ns] are numbers of active columns included
       * in the singleton list */
      ns = 0;
      for (i = 1; i <= m; i++)
      {  /* get i-th row */
         len = cnt[i] = mat(info, +i, rind, rval);
         xassert(0 <= len && len <= n);
         if (len == 1)
         {  /* a[i,j] is row singleton */
            j = rind[1];
            xassert(1 <= j && j <= n);
            if (flag[j] != 2)
            {  /* include j-th column in singleton list */
               flag[j] = 2;
               list[++ns] = j;
            }
         }
      }
      /*--------------------------------------------------------------*/
      /* main loop                                                    */
      /*--------------------------------------------------------------*/
      size = 0; /* size of triangular part */
      /* loop until active column list is non-empty, i.e. until the
       * active submatrix has at least one column */
      while (head != 0)
      {  if (ns == 0)
         {  /* singleton list is empty */
            /* remove from the active submatrix a column of maximal
             * length in the hope that some row singletons appear */
            j = head;
            len = mat(info, -j, cind, cval);
            xassert(0 <= len && len <= m);
            goto drop;
         }
         /* take column j from the singleton list */
         j = list[ns--];
         xassert(flag[j] == 2);
         /* j-th column has at least one row singleton in the active
          * submatrix; choose one having maximal magnitude */
         len = mat(info, -j, cind, cval);
         xassert(0 <= len && len <= m);
         kk = 0;
         for (k = 1; k <= len; k++)
         {  i = cind[k];
            xassert(1 <= i && i <= m);
            if (cnt[i] == 1)
            {  /* a[i,j] is row singleton */
               if (kk == 0 || fabs(cval[kk]) < fabs(cval[k]))
                  kk = k;
            }
         }
         xassert(kk > 0);
         /* check magnitude of the row singleton chosen */
         if (fabs(cval[kk]) < tol * big[j])
         {  /* all row singletons are too small in magnitude; drop j-th
             * column */
            goto drop;
         }
         /* row singleton a[i,j] is ok; add i-th row and j-th column to
          * the triangular part */
         size++;
         rn[size] = cind[kk];
         cn[size] = j;
drop:    /* remove j-th column from the active submatrix */
         xassert(flag[j]);
         flag[j] = 0;
         if (prev[j] == 0)
            head = next[j];
         else
            next[prev[j]] = next[j];
         if (next[j] == 0)
            ;
         else
            prev[next[j]] = prev[j];
         /* decrease row counts */
         for (k = 1; k <= len; k++)
         {  i = cind[k];
            xassert(1 <= i && i <= m);
            xassert(cnt[i] > 0);
            cnt[i]--;
            if (cnt[i] == 1)
            {  /* new singleton appeared in i-th row; determine number
                * of corresponding column (it is the only active column
                * in this row) */
               len2 = mat(info, +i, rind, rval);
               xassert(0 <= len2 && len2 <= n);
               ks = 0;
               for (kk = 1; kk <= len2; kk++)
               {  jj = rind[kk];
                  xassert(1 <= jj && jj <= n);
                  if (flag[jj])
                  {  xassert(ks == 0);
                     ks = kk;
                  }
               }
               xassert(ks > 0);
               /* a[i,jj] is new row singleton */
               jj = rind[ks];
               if (flag[jj] != 2)
               {  /* include jj-th column in the singleton list */
                  flag[jj] = 2;
                  list[++ns] = jj;
               }
            }
         }
      }
      /* now all row counts should be zero */
      for (i = 1; i <= m; i++)
         xassert(cnt[i] == 0);
      /* deallocate working arrays */
      tfree(cind);
      tfree(cval);
      tfree(rind);
      tfree(rval);
      tfree(ptr);
      tfree(list);
      tfree(prev);
      tfree(next);
      tfree(big);
      tfree(flag);
      return size;
}
Example #19
0
DEF_TEST(Matrix44, reporter) {
    SkMatrix44 mat(SkMatrix44::kUninitialized_Constructor);
    SkMatrix44 inverse(SkMatrix44::kUninitialized_Constructor);
    SkMatrix44 iden1(SkMatrix44::kUninitialized_Constructor);
    SkMatrix44 iden2(SkMatrix44::kUninitialized_Constructor);
    SkMatrix44 rot(SkMatrix44::kUninitialized_Constructor);

    mat.setTranslate(1, 1, 1);
    mat.invert(&inverse);
    iden1.setConcat(mat, inverse);
    REPORTER_ASSERT(reporter, is_identity(iden1));

    mat.setScale(2, 2, 2);
    mat.invert(&inverse);
    iden1.setConcat(mat, inverse);
    REPORTER_ASSERT(reporter, is_identity(iden1));

    mat.setScale(SK_MScalar1/2, SK_MScalar1/2, SK_MScalar1/2);
    mat.invert(&inverse);
    iden1.setConcat(mat, inverse);
    REPORTER_ASSERT(reporter, is_identity(iden1));

    mat.setScale(3, 3, 3);
    rot.setRotateDegreesAbout(0, 0, -1, 90);
    mat.postConcat(rot);
    REPORTER_ASSERT(reporter, mat.invert(nullptr));
    mat.invert(&inverse);
    iden1.setConcat(mat, inverse);
    REPORTER_ASSERT(reporter, is_identity(iden1));
    iden2.setConcat(inverse, mat);
    REPORTER_ASSERT(reporter, is_identity(iden2));

    // test tiny-valued matrix inverse
    mat.reset();
    auto v = SkDoubleToMScalar(1.0e-12);
    mat.setScale(v,v,v);
    rot.setRotateDegreesAbout(0, 0, -1, 90);
    mat.postConcat(rot);
    mat.postTranslate(v,v,v);
    REPORTER_ASSERT(reporter, mat.invert(nullptr));
    mat.invert(&inverse);
    iden1.setConcat(mat, inverse);
    REPORTER_ASSERT(reporter, is_identity(iden1));

    // test mixed-valued matrix inverse
    mat.reset();
    mat.setScale(SkDoubleToMScalar(1.0e-2),
                 SkDoubleToMScalar(3.0),
                 SkDoubleToMScalar(1.0e+2));
    rot.setRotateDegreesAbout(0, 0, -1, 90);
    mat.postConcat(rot);
    mat.postTranslate(SkDoubleToMScalar(1.0e+2),
                      SkDoubleToMScalar(3.0),
                      SkDoubleToMScalar(1.0e-2));
    REPORTER_ASSERT(reporter, mat.invert(nullptr));
    mat.invert(&inverse);
    iden1.setConcat(mat, inverse);
    REPORTER_ASSERT(reporter, is_identity(iden1));

    // test degenerate matrix
    mat.reset();
    mat.set3x3(1.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0);
    REPORTER_ASSERT(reporter, !mat.invert(nullptr));

    // test rol/col Major getters
    {
        mat.setTranslate(2, 3, 4);
        float dataf[16];
        double datad[16];

        mat.asColMajorf(dataf);
        assert16<float>(reporter, dataf,
                        1, 0, 0, 0,
                        0, 1, 0, 0,
                        0, 0, 1, 0,
                        2, 3, 4, 1);
        mat.asColMajord(datad);
        assert16<double>(reporter, datad, 1, 0, 0, 0,
                         0, 1, 0, 0,
                         0, 0, 1, 0,
                         2, 3, 4, 1);
        mat.asRowMajorf(dataf);
        assert16<float>(reporter, dataf, 1, 0, 0, 2,
                        0, 1, 0, 3,
                        0, 0, 1, 4,
                        0, 0, 0, 1);
        mat.asRowMajord(datad);
        assert16<double>(reporter, datad, 1, 0, 0, 2,
                         0, 1, 0, 3,
                         0, 0, 1, 4,
                         0, 0, 0, 1);
    }

    test_concat(reporter);

    if (false) { // avoid bit rot, suppress warning (working on making this pass)
        test_common_angles(reporter);
    }

    test_constructor(reporter);
    test_gettype(reporter);
    test_determinant(reporter);
    test_invert(reporter);
    test_transpose(reporter);
    test_get_set_double(reporter);
    test_set_row_col_major(reporter);
    test_set_3x3(reporter);
    test_translate(reporter);
    test_scale(reporter);
    test_map2(reporter);
    test_3x3_conversion(reporter);
    test_has_perspective(reporter);
    test_preserves_2d_axis_alignment(reporter);
    test_toint(reporter);
}
Example #20
0
RES Camera::_get_gizmo_geometry() const {


	Ref<SurfaceTool> surface_tool( memnew( SurfaceTool ));

	Ref<FixedMaterial> mat( memnew( FixedMaterial ));

	mat->set_parameter( FixedMaterial::PARAM_DIFFUSE,Color(1.0,0.5,1.0,0.5) );
	mat->set_line_width(4);
	mat->set_flag(Material::FLAG_DOUBLE_SIDED,true);
	mat->set_flag(Material::FLAG_UNSHADED,true);
	//mat->set_hint(Material::HINT_NO_DEPTH_DRAW,true);

	surface_tool->begin(Mesh::PRIMITIVE_LINES);
	surface_tool->set_material(mat);

	switch(mode) {

		case PROJECTION_PERSPECTIVE: {



			Vector3 side=Vector3( Math::sin(Math::deg2rad(fov)), 0, -Math::cos(Math::deg2rad(fov)) );
			Vector3 nside=side;
			nside.x=-nside.x;
			Vector3 up=Vector3(0,side.x,0);


#define ADD_TRIANGLE( m_a, m_b, m_c)\
{\
	surface_tool->add_vertex(m_a);\
	surface_tool->add_vertex(m_b);\
	surface_tool->add_vertex(m_b);\
	surface_tool->add_vertex(m_c);\
	surface_tool->add_vertex(m_c);\
	surface_tool->add_vertex(m_a);\
}

			ADD_TRIANGLE( Vector3(), side+up, side-up );
			ADD_TRIANGLE( Vector3(), nside+up, nside-up );
			ADD_TRIANGLE( Vector3(), side+up, nside+up );
			ADD_TRIANGLE( Vector3(), side-up, nside-up );

			side.x*=0.25;
			nside.x*=0.25;
			Vector3 tup( 0, up.y*3/2,side.z);
			ADD_TRIANGLE( tup, side+up, nside+up );

		} break;
		case PROJECTION_ORTHOGONAL: {

#define ADD_QUAD( m_a, m_b, m_c, m_d)\
{\
	surface_tool->add_vertex(m_a);\
	surface_tool->add_vertex(m_b);\
	surface_tool->add_vertex(m_b);\
	surface_tool->add_vertex(m_c);\
	surface_tool->add_vertex(m_c);\
	surface_tool->add_vertex(m_d);\
	surface_tool->add_vertex(m_d);\
	surface_tool->add_vertex(m_a);\
}

			float hsize=size*0.5;
			Vector3 right(hsize,0,0);
			Vector3 up(0,hsize,0);
			Vector3 back(0,0,-1.0);
			Vector3 front(0,0,0);

			ADD_QUAD( -up-right,-up+right,up+right,up-right);
			ADD_QUAD( -up-right+back,-up+right+back,up+right+back,up-right+back);
			ADD_QUAD( up+right,up+right+back,up-right+back,up-right);
			ADD_QUAD( -up+right,-up+right+back,-up-right+back,-up-right);

			right.x*=0.25;
			Vector3 tup( 0, up.y*3/2,back.z );
			ADD_TRIANGLE( tup, right+up+back, -right+up+back );

		} break;

	}

	return surface_tool->commit();

}
    //-----------------------------------------------------------------------
    void RevolutionAffector::_affectParticles(ParticleSystem* pSystem, Real timeElapsed)
    {
        // 因为timeElapsed有可能有0,加上判断,避免除0错
        if (false == Ogre::Math::RealEqual(timeElapsed, 0.0f))
        {
            ParticleIterator pi = pSystem->_getIterator();
            Particle *p;

            Quaternion q( Radian(Degree(mRotationSpeed*timeElapsed).valueRadians()), mRotateAxis);
            Matrix3 mat(Matrix3::IDENTITY);
            q.ToRotationMatrix(mat);

            Ogre::Vector3 randomPoint;
            randomPoint.x = Math::RangeRandom(mCenterOffsetMin.x, mCenterOffsetMax.x);
            randomPoint.y = Math::RangeRandom(mCenterOffsetMin.y, mCenterOffsetMax.y);
            randomPoint.z = Math::RangeRandom(mCenterOffsetMin.z, mCenterOffsetMax.z);

            Vector3 particleSystemPos(Vector3::ZERO);

            bool localSpace = pSystem->getKeepParticlesInLocalSpace();

            particleSystemPos = pSystem->getParentSceneNode()->_getDerivedPosition();

            Ogre::Vector3 destPoint = particleSystemPos + randomPoint;

            Vector3 particlePos(Vector3::ZERO);

            Vector3 RadiusIncrementDir(Vector3::ZERO);

            bool needFmod = mRepeatTimes != 1.0f;

            while (!pi.end())
            {
                p = pi.getNext();

                particlePos = p->position;

                /** 如果是localSpace,那么p->position得到的是粒子的相对位置,所以要加上
                粒子系统的位置particleSystemPos,得到绝对位置,再减去destPoint才能得到
                正确的向量
                particlePos + particleSystemPos - destPoint
                =   particlePos + particleSystemPos - particleSystemPos - randomPoint
                =   particlePos - randomPoint
                */
                if (localSpace)
                {
                    RadiusIncrementDir = particlePos - randomPoint;
                    RadiusIncrementDir.normalise();

                    particlePos = mat *( particlePos - randomPoint ) + randomPoint - particlePos;
                }
                else
                {
                    RadiusIncrementDir = particlePos - destPoint;
                    RadiusIncrementDir.normalise();

                    particlePos = mat *( particlePos - destPoint ) + destPoint - particlePos;
                }

                p->direction = particlePos / timeElapsed;

                if (mUseRadiusIncrementScale)
                {
                    const Real		life_time		= p->totalTimeToLive;
                    Real			particle_time	= 1.0f - (p->timeToLive / life_time); 

                    // wrap the particle time
                    Real            repeatedParticleTime = 
                        needFmod ? fmod( particle_time * mRepeatTimes, 1.0f ) : particle_time;

                    if (repeatedParticleTime <= mTimeAdj[0])
                    {
                        p->direction += RadiusIncrementDir * mRadiusIncrementAdj[0];
                    } 
                    else if (repeatedParticleTime >= mTimeAdj[MAX_STAGES - 1])
                    {
                        p->direction += RadiusIncrementDir * mRadiusIncrementAdj[MAX_STAGES - 1];
                    } 
                    else
                    {
                        for (int i=0;i<MAX_STAGES-1;i++)
                        {
                            if (repeatedParticleTime >= mTimeAdj[i] && repeatedParticleTime < mTimeAdj[i + 1])
                            {
                                repeatedParticleTime -= mTimeAdj[i];
                                repeatedParticleTime /= (mTimeAdj[i+1]-mTimeAdj[i]);
                                p->direction += RadiusIncrementDir * 
                                    ( (mRadiusIncrementAdj[i+1] * repeatedParticleTime) + (mRadiusIncrementAdj[i] * (1.0f - repeatedParticleTime)) );
                                break;
                            }
                        }
                    }
                }
                else
                {
                    p->direction += RadiusIncrementDir*mRadiusIncrement;
                }

                //	case RotationType::OUTER_NORMAL:
                ////		p->direction.x += pos.x;
                //       //       p->direction.z += pos.z;
                //		p->direction = pos;

                //		break;

                //	case RotationType::OUTER_FAST:
                ////		p->direction.x += (p->direction.x + pos.x) /2;
                //     //         p->direction.z += (p->direction.z + pos.z) /2;
                //		p->direction += (p->direction + pos) /2;
                //		break;
            }
        }
    }
Example #22
0
/* TODO */
Dmatrix Dmatrix::operator * (const Dmatrix & matrix) {
  Dmatrix mat(matrix) ; 
  return mat ; 
}
Example #23
0
	Vec3b convertColor(Vec3b color, int code) {
		Mat_<Vec3b> mat(1, 1, CV_8UC3);
		mat(0, 0) = color;
		cvtColor(mat, mat, code);
		return mat(0, 0);
	}
Example #24
0
/** solve one of the eight Appollonius Equations
| Cx - Ci|^2=(Rx+Ri)^2
with Cx the center of the common tangent circle, Rx the radius. Ci and Ri are the Center and radius of the i-th existing circle
**/
std::vector<RS_Circle> RS_Circle::solveAppolloniusSingle(const std::vector<RS_Circle>& circles)
{
//          std::cout<<__FILE__<<" : "<<__func__<<" : line "<<__LINE__<<std::endl;
//          for(int i=0;i<circles.size();i++){
//std::cout<<"i="<<i<<"\t center="<<circles[i].getCenter()<<"\tr="<<circles[i].getRadius()<<std::endl;
//          }
	std::vector<RS_Circle> ret;

	std::vector<RS_Vector> centers;
	std::vector<double> radii;

	for(auto c: circles){
		if(c.getCenter().valid==false) return ret;
		centers.push_back(c.getCenter());
		radii.push_back(c.getRadius());
	}
//              for(int i=0;i<circles.size();i++){
//    std::cout<<"i="<<i<<"\t center="<<circles[i].getCenter()<<"\tr="<<radii.at(i)<<std::endl;
//              }
/** form the linear equation to solve center in radius **/
	std::vector<std::vector<double> > mat(2,std::vector<double>(3,0.));
    mat[0][0]=centers[2].x - centers[0].x;
    mat[0][1]=centers[2].y - centers[0].y;
    mat[1][0]=centers[2].x - centers[1].x;
    mat[1][1]=centers[2].y - centers[1].y;
    if(fabs(mat[0][0]*mat[1][1] - mat[0][1]*mat[1][0])<RS_TOLERANCE2){
//        DEBUG_HEADER
//        std::cout<<"The provided circles are in a line, not common tangent circle"<<std::endl;
        size_t i0=0;
        if( centers[0].distanceTo(centers[1]) <= RS_TOLERANCE ||  centers[0].distanceTo(centers[2]) <= RS_TOLERANCE) i0 = 1;
        LC_Quadratic lc0(& (circles[i0]), & (circles[(i0+1)%3]));
        LC_Quadratic lc1(& (circles[i0]), & (circles[(i0+2)%3]));
		auto c0 = LC_Quadratic::getIntersection(lc0, lc1);
//        qDebug()<<"c0.size()="<<c0.size();
        for(size_t i=0; i<c0.size(); i++){
            const double dc =  c0[i].distanceTo(centers[i0]);
			ret.push_back(RS_Circle(nullptr, {c0[i], fabs(dc - radii[i0])}));
            if( dc > radii[i0]) {
				ret.push_back(RS_Circle(nullptr, {c0[i], dc + radii[i0]}));
            }
        }
        return ret;
    }
    // r^0 term
    mat[0][2]=0.5*(centers[2].squared()-centers[0].squared()+radii[0]*radii[0]-radii[2]*radii[2]);
    mat[1][2]=0.5*(centers[2].squared()-centers[1].squared()+radii[1]*radii[1]-radii[2]*radii[2]);
//    std::cout<<__FILE__<<" : "<<__func__<<" : line "<<__LINE__<<std::endl;
//    for(unsigned short i=0;i<=1;i++){
//        std::cout<<"eqs P:"<<i<<" : "<<mat[i][0]<<"*x + "<<mat[i][1]<<"*y = "<<mat[i][2]<<std::endl;
//    }
//    std::vector<std::vector<double> > sm(2,std::vector<double>(2,0.));
	std::vector<double> sm(2,0.);
    if(RS_Math::linearSolver(mat,sm)==false){
        return ret;
    }

    RS_Vector vp(sm[0],sm[1]);
//      std::cout<<__FILE__<<" : "<<__func__<<" : line "<<__LINE__<<std::endl;
//      std::cout<<"vp="<<vp<<std::endl;

    // r term
    mat[0][2]= radii[0]-radii[2];
    mat[1][2]= radii[1]-radii[2];
//    for(unsigned short i=0;i<=1;i++){
//        std::cout<<"eqs Q:"<<i<<" : "<<mat[i][0]<<"*x + "<<mat[i][1]<<"*y = "<<mat[i][2]<<std::endl;
//    }
    if(RS_Math::linearSolver(mat,sm)==false){
        return ret;
    }
    RS_Vector vq(sm[0],sm[1]);
//      std::cout<<"vq="<<vq<<std::endl;
    //form quadratic equation for r
    RS_Vector dcp=vp-centers[0];
    double a=vq.squared()-1.;
    if(fabs(a)<RS_TOLERANCE*1e-4) {
        return ret;
    }
    std::vector<double> ce(0,0.);
    ce.push_back(2.*(dcp.dotP(vq)-radii[0])/a);
    ce.push_back((dcp.squared()-radii[0]*radii[0])/a);
    std::vector<double>&& vr=RS_Math::quadraticSolver(ce);
    for(size_t i=0; i < vr.size();i++){
        if(vr.at(i)<RS_TOLERANCE) continue;
		ret.emplace_back(RS_Circle(nullptr, {vp+vq*vr.at(i),fabs(vr.at(i))}));
    }
//    std::cout<<__FILE__<<" : "<<__func__<<" : line "<<__LINE__<<std::endl;
//    std::cout<<"Found "<<ret.size()<<" solutions"<<std::endl;

    return ret;
}
Example #25
0
void PT::asqtad_long(AsqDArg *asq_arg, matrix *longlink, matrix *longlink_m){

  IFloat c2 = asq_arg->c2;
  int i, j; 
  const int N = 4;

  matrix *result[NUM_DIR];
  matrix *Unit;
  matrix *Pmumu[NUM_DIR];
  int dir[] = {6,0,2,4,7,1,3,5},dirs[8]; //mapping between ParTrans and DiracOpAsqtad
  matrix *min[NUM_DIR],*mout[NUM_DIR];

  for(j = 0;j<POS_DIR;j++){
     result[j] = longlink + vol*j;
//     VRB.Flow(cname,fname,"result[%d]=%p\n",j,result[j]);
  }
  if (longlink_m != NULL)
  for(j = 0;j<POS_DIR;j++){
     result[j+POS_DIR] = longlink_m + vol*j;
  }
  for(j = 0;j<vol;j++) Unit[j] = c2;
  int n_dirs = POS_DIR;
  if (longlink_m != NULL) n_dirs = NUM_DIR;
  for(int mu = 0;mu<n_dirs;mu += N){
    if (mu == 4) for(j = 0;j<vol;j++) Unit[j] = -c2;
    int mu_p;
    for(i  = 0;i<N;i++){
      mu_p = Rotate(mu,i);
      min[i] =  Unit;
      mout[i] = result[mu_p];
      dirs[i] = dir[mu_p];
    }
    mat(N,mout,min,dirs);
    for(i  = 0;i<N;i++){
      mu_p = Rotate(mu,i);
      min[i] =  result[mu_p];
      mout[i] = Pmumu[i];
      dirs[i] = dir[mu_p];
    }
    mat(N,mout,min,dirs);
    for(i  = 0;i<N;i++){
      mu_p = Rotate(mu,i);
      min[i] = Pmumu[i];
      mout[i] =  result[mu_p];
      dirs[i] = dir[mu_p];
    }
    mat(N,mout,min,dirs);
  }

  Free( Unit);
  for(j = 0;j<N;j++){
    Free( Pmumu[j]);
  }

//  dtime +=dclock();
//  nflops += ParTrans::PTflops;
//  printf("%s:%s:",cname,fname);
//  print_flops(nflops,dtime);

#if 0
  printf("%s:%s:",cname,fname);
  for(i = 0;i<3;i++){
  for(j = 0;j<POS_DIR;j++){
	IFloat *tmp = (IFloat *)(fields[i]+vol*j);
  printf("%e ",*tmp);
  }
  printf("\n");
  }
#endif

}
Example #26
0
Mat4 Mat4::getInversed() const
{
    Mat4 mat(*this);
    mat.inverse();
    return mat;
}
void TSShapeInstance::handleBlendSequence(TSThread * thread, S32 a, S32 b)
{
   S32 jrot=0;
   S32 jtrans=0;
   S32 jscale=0;
   TSIntegerSet nodeMatters = thread->getSequence()->translationMatters;
   nodeMatters.overlap(thread->getSequence()->rotationMatters);
   nodeMatters.overlap(thread->getSequence()->scaleMatters);
   nodeMatters.takeAway(mHandsOffNodes);
   S32 start = nodeMatters.start();
   S32 end   = b;
   for (S32 nodeIndex=start; nodeIndex<end; nodeMatters.next(nodeIndex))
   {
      // skip nodes outside of this detail
      if (start<a || mDisableBlendNodes.test(nodeIndex))
      {
         if (thread->getSequence()->rotationMatters.test(nodeIndex))
            jrot++;
         if (thread->getSequence()->translationMatters.test(nodeIndex))
            jtrans++;
         if (thread->getSequence()->scaleMatters.test(nodeIndex))
            jscale++;
         continue;
      }

      MatrixF mat(true);
      if (thread->getSequence()->rotationMatters.test(nodeIndex))
      {
         QuatF q1,q2;
         mShape->getRotation(*thread->getSequence(),thread->keyNum1,jrot,&q1);
         mShape->getRotation(*thread->getSequence(),thread->keyNum2,jrot,&q2);
         QuatF quat;
         TSTransform::interpolate(q1,q2,thread->keyPos,&quat);
         TSTransform::setMatrix(quat,&mat);
         jrot++;
      }

      if (thread->getSequence()->translationMatters.test(nodeIndex))
      {
         const Point3F & p1 = mShape->getTranslation(*thread->getSequence(),thread->keyNum1,jtrans);
         const Point3F & p2 = mShape->getTranslation(*thread->getSequence(),thread->keyNum2,jtrans);
         Point3F p;
         TSTransform::interpolate(p1,p2,thread->keyPos,&p);
         mat.setColumn(3,p);
         jtrans++;
      }

      if (thread->getSequence()->scaleMatters.test(nodeIndex))
      {
         if (thread->getSequence()->animatesUniformScale())
         {
            F32 s1 = mShape->getUniformScale(*thread->getSequence(),thread->keyNum1,jscale);
            F32 s2 = mShape->getUniformScale(*thread->getSequence(),thread->keyNum2,jscale);
            F32 scale = TSTransform::interpolate(s1,s2,thread->keyPos);
            TSTransform::applyScale(scale,&mat);
         }
         else if (animatesAlignedScale())
         {
            Point3F s1 = mShape->getAlignedScale(*thread->getSequence(),thread->keyNum1,jscale);
            Point3F s2 = mShape->getAlignedScale(*thread->getSequence(),thread->keyNum2,jscale);
            Point3F scale;
            TSTransform::interpolate(s1,s2,thread->keyPos,&scale);
            TSTransform::applyScale(scale,&mat);
         }
         else
         {
            TSScale s1,s2;
            mShape->getArbitraryScale(*thread->getSequence(),thread->keyNum1,jscale,&s1);
            mShape->getArbitraryScale(*thread->getSequence(),thread->keyNum2,jscale,&s2);
            TSScale scale;
            TSTransform::interpolate(s1,s2,thread->keyPos,&scale);
            TSTransform::applyScale(scale,&mat);
         }
         jscale++;
      }

      // apply blend transform
      smNodeLocalTransforms[nodeIndex].mul(mat);
      smNodeLocalTransformDirty.set(nodeIndex);
   }
}
Example #28
0
Mat4 Mat4::getNegated() const
{
    Mat4 mat(*this);
    mat.negate();
    return mat;
}
Example #29
0
  bool AdFront2 :: SameSide (const Point<2> & lp1, const Point<2> & lp2, 
                             const Array<int> * testfaces) const
  {
    int cnt = 0;

    if (testfaces)
      {
        for (int ii = 0; ii < testfaces->Size(); ii++)
          if (lines[(*testfaces)[ii]].Valid())
            {
              int i = (*testfaces)[ii];
              const Point<3> & p13d = points[lines[i].L().I1()].P();
              const Point<3> & p23d = points[lines[i].L().I2()].P();
              
              Point<2> p1(p13d(0), p13d(1));
              Point<2> p2(p23d(0), p23d(1));
              
              // p1 + alpha v = lp1 + beta vl
              Vec<2> v = p2-p1;
              Vec<2> vl = lp2 - lp1;
              Mat<2,2> mat, inv;
              Vec<2> rhs, sol;
              mat(0,0) = v(0);
              mat(1,0) = v(1);
              mat(0,1) = -vl(0);
              mat(1,1) = -vl(1);
              rhs = lp1-p1;
              
              if (Det(mat) == 0) continue;
              CalcInverse (mat, inv);
              sol = inv * rhs;
              if (sol(0) >= 0 && sol(0) <= 1 & sol(1) >= 0 && sol(1) <= 1)
                { cnt++; }
            }

      }
    else
      {
        for (int i = 0; i < lines.Size(); i++)
          if (lines[i].Valid())
            {
              const Point<3> & p13d = points[lines[i].L().I1()].P();
              const Point<3> & p23d = points[lines[i].L().I2()].P();
              
              Point<2> p1(p13d(0), p13d(1));
              Point<2> p2(p23d(0), p23d(1));
              
              // p1 + alpha v = lp1 + beta vl
              Vec<2> v = p2-p1;
              Vec<2> vl = lp2 - lp1;
              Mat<2,2> mat, inv;
              Vec<2> rhs, sol;
              mat(0,0) = v(0);
              mat(1,0) = v(1);
              mat(0,1) = -vl(0);
              mat(1,1) = -vl(1);
              rhs = lp1-p1;
              
              if (Det(mat) == 0) continue;
              CalcInverse (mat, inv);
              sol = inv * rhs;
              if (sol(0) >= 0 && sol(0) <= 1 & sol(1) >= 0 && sol(1) <= 1)
                { cnt++; }
            }
      }
    return ((cnt % 2) == 0);
  }
Example #30
0
Vector3 Matrix3::Row(int i) const
{
	return Vector3(mat(i,1),mat(i,2),mat(i,3));
}