Vector3 Matrix3::Column(int j) const { return Vector3(mat(1,j),mat(2,j),mat(3,j)); }
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]); } }
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); } }
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; }); }
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())); }
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 }
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; }
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); }
//---------------------------------------------------------------------------- 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; }
float& Matrix3::operator()(int i, int j) { return mat(i,j); }
//access operation float Matrix3::operator()(int i, int j) const { return mat(i,j); }
Mat4 Mat4::getTransposed() const { Mat4 mat(*this); mat.transpose(); return mat; }
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 ) ); }
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; }
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); }
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; } } }
/* TODO */ Dmatrix Dmatrix::operator * (const Dmatrix & matrix) { Dmatrix mat(matrix) ; return mat ; }
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); }
/** 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; }
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 }
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); } }
Mat4 Mat4::getNegated() const { Mat4 mat(*this); mat.negate(); return mat; }
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); }
Vector3 Matrix3::Row(int i) const { return Vector3(mat(i,1),mat(i,2),mat(i,3)); }