void AddBox(Geometry*g, vec3 a,vec3 b,ivec3 br) { Geometry tmp; if(a.x>b.x)swap(a.x,b.x); if(a.y>b.y)swap(a.y,b.y); if(a.z>b.z)swap(a.z,b.z); vec3 s = b - a; vec3 a_x(s.x/br.x,0,0); vec3 a_y(0,s.y/br.x,0); vec3 a_z(0,0,s.z/br.x); /* for(int i=0;i<=br.x;i++) for(int j=0;j<=br.x;j++) { AddQuad(&tmp,a,a+a_y,a+a_x+a_y,a+a_x); AddQuad(&tmp,b,b-a_x,b-a_x-a_y,b-a_y); AddQuad(&tmp,a,a+a_x,a+a_x+a_z,a+a_z); AddQuad(&tmp,b,b-a_z,b-a_x-a_z,b-a_x); AddQuad(&tmp,b,b-a_y,b-a_z-a_y,b-a_z); AddQuad(&tmp,a,a+a_z,a+a_z+a_y,a+a_y); }*/ tmp.SetNormOutOf((a+b)*0.5f); g->Add(tmp); }
Collisionhandler::OBB Collisionhandler::createOBB(const Entity &a) { Collisionhandler::OBB a_OBB; vmml::Vector2f a_x(cosf(a.getRotation()), sinf(a.getRotation())); vmml::Vector2f a_z(-sinf(a.getRotation()), cosf(a.getRotation())); a_x *= a.getWidth() / 2; a_z *= a.getLength() / 2; a_OBB.corner[0] = vmml::Vector2f(a.getX(), a.getZ()) - a_x - a_z; a_OBB.corner[1] = vmml::Vector2f(a.getX(), a.getZ()) + a_x - a_z; a_OBB.corner[2] = vmml::Vector2f(a.getX(), a.getZ()) + a_x + a_z; a_OBB.corner[3] = vmml::Vector2f(a.getX(), a.getZ()) - a_x + a_z; a_OBB.axis[0] = a_OBB.corner[1] - a_OBB.corner[0]; a_OBB.axis[1] = a_OBB.corner[3] - a_OBB.corner[0]; for (int i = 0; i < 2; i++) { a_OBB.axis[i] /= a_OBB.axis[i].squared_length(); a_OBB.origin[i] = a_OBB.corner[0].dot(a_OBB.axis[i]); } return a_OBB; }
void AddBox(Geometry*g, vec3 a,vec3 b) { Geometry tmp; if(a.x>b.x)swap(a.x,b.x); if(a.y>b.y)swap(a.y,b.y); if(a.z>b.z)swap(a.z,b.z); vec3 s = b - a; vec3 a_x(s.x,0,0); vec3 a_y(0,s.y,0); vec3 a_z(0,0,s.z); AddQuad(&tmp,a,a+a_y,a+a_x+a_y,a+a_x); AddQuad(&tmp,a,a+a_x,a+a_x+a_z,a+a_z); AddQuad(&tmp,a,a+a_z,a+a_z+a_y,a+a_y); AddQuad(&tmp,b,b-a_x,b-a_x-a_y,b-a_y); AddQuad(&tmp,b,b-a_z,b-a_x-a_z,b-a_x); AddQuad(&tmp,b,b-a_y,b-a_z-a_y,b-a_z); tmp.SetNormOutOf((a+b)*0.5f); g->Add(tmp); }
void fi::VPDetectionWrapper::validateVanishingPoint(const std::vector<std::vector< Eigen::Vector2f> > &computed_vp_hypothesis, const Eigen::Matrix3f &cam_calib, Eigen::Vector3f &final_robust_vp_x, Eigen::Vector3f &final_robust_vp_y) { Eigen::Matrix3f inv_cam_calib = cam_calib.inverse(); //trans from vps to rays through camera axis, see Z+H Chapter 8, more on single view geometry! unsigned int num_vps = computed_vp_hypothesis.size(); std::vector< Eigen::Vector3f> computed_vp_hypothesis_x; std::vector< Eigen::Vector3f> computed_vp_hypothesis_y; std::vector< Eigen::Vector3f> computed_vp_hypothesis_z; for (unsigned int i = 0; i < num_vps; i++) { std::vector<Eigen::Vector2f> a_vp = computed_vp_hypothesis.at(i); Eigen::Vector2f a_x = a_vp.at(0); Eigen::Vector3f x_h, n_x; x_h(0) = a_x(0); x_h(1) = a_x(1); x_h(2) = 1; n_x = inv_cam_calib * x_h; n_x = n_x.normalized(); computed_vp_hypothesis_x.push_back(n_x); Eigen::Vector2f a_y = a_vp.at(1); Eigen::Vector3f y_h, n_y; y_h(0) = a_y(0); y_h(1) = a_y(1); y_h(2) = 1; n_y = inv_cam_calib * y_h; n_y = n_y.normalized(); computed_vp_hypothesis_y.push_back(n_y); Eigen::Vector2f a_z = a_vp.at(2); Eigen::Vector3f z_h, n_z; z_h(0) = a_z(0); z_h(1) = a_z(1); z_h(2) = 1; n_z = inv_cam_calib * z_h; n_z = n_z.normalized(); computed_vp_hypothesis_z.push_back(n_z); } std::vector<Eigen::Vector3f> in_liers_x; std::vector<Eigen::Vector3f> in_liers_y; std::vector<Eigen::Vector3f> in_liers_z; bool found_inliers_x = getRansacInliers(computed_vp_hypothesis_x, in_liers_x); bool found_inliers_y = getRansacInliers(computed_vp_hypothesis_y, in_liers_y); bool found_inliers_z = getRansacInliers(computed_vp_hypothesis_z, in_liers_z); Eigen::VectorXf optimized_vp_x; Eigen::VectorXf optimized_vp_y; Eigen::VectorXf optimized_vp_z; leastQuaresVPFitting(in_liers_x, optimized_vp_x); leastQuaresVPFitting(in_liers_y, optimized_vp_y); leastQuaresVPFitting(in_liers_z, optimized_vp_z); std::cout<<"Vanishing Points Validated"<<std::endl; //test the angles and see if OK otherwise check again if truelly orthogonal Eigen::Vector3f vp_x (optimized_vp_x[3], optimized_vp_x[4], optimized_vp_x[5]);; Eigen::Vector3f vp_y (optimized_vp_y[3], optimized_vp_y[4], optimized_vp_y[5]); Eigen::Vector3f vp_z (optimized_vp_z[3], optimized_vp_z[4], optimized_vp_z[5]); Eigen::Vector3f vp_x_centroid (optimized_vp_x[0], optimized_vp_x[1], optimized_vp_x[2]); Eigen::Vector3f vp_y_centroid (optimized_vp_y[0], optimized_vp_y[1], optimized_vp_y[2]); Eigen::Vector3f vp_z_centroid (optimized_vp_z[0], optimized_vp_z[1], optimized_vp_z[2]); float angle_value_radiens_cxy = angleBetweenVectors(vp_x_centroid, vp_y_centroid); float angle_value_degrees_cxy = pcl::rad2deg(angle_value_radiens_cxy); float angle_value_radiens_cxz = angleBetweenVectors(vp_x_centroid, vp_z_centroid); float angle_value_degrees_cxz = pcl::rad2deg(angle_value_radiens_cxz); float angle_value_radiens_cyz = angleBetweenVectors(vp_y_centroid, vp_z_centroid); float angle_value_degrees_cyz = pcl::rad2deg(angle_value_radiens_cyz); float angle_value_radiens_xy = angleBetweenVectors(vp_x, vp_y); float angle_value_degrees_xy = pcl::rad2deg(angle_value_radiens_xy); float angle_value_radiens_xz = angleBetweenVectors(vp_x, vp_z); float angle_value_degrees_xz = pcl::rad2deg(angle_value_radiens_xz); float angle_value_radiens_yz = angleBetweenVectors(vp_y, vp_z); float angle_value_degrees_yz = pcl::rad2deg(angle_value_radiens_yz); //collect only the mean vps final_robust_vp_x = optimized_vp_x.tail<3> (); final_robust_vp_y = optimized_vp_y.tail<3> (); //final_robust_vp_x = vp_x_centroid; //final_robust_vp_y = vp_y_centroid; }
void assemble( DeviceType & device, TimeStepQuantitiesT & old_quantities, TimeStepQuantitiesT & quantities, viennashe::config const & conf, viennashe::she::unknown_she_quantity<VertexT, EdgeT> const & quan, MatrixType & A, VectorType & b, bool use_timedependence, bool quan_valid) { typedef typename DeviceType::mesh_type MeshType; typedef typename viennagrid::result_of::facet<MeshType>::type FacetType; typedef typename viennagrid::result_of::cell<MeshType>::type CellType; typedef typename viennagrid::result_of::const_facet_range<MeshType>::type FacetContainer; typedef typename viennagrid::result_of::iterator<FacetContainer>::type FacetIterator; typedef typename viennagrid::result_of::const_cell_range<MeshType>::type CellContainer; typedef typename viennagrid::result_of::iterator<CellContainer>::type CellIterator; typedef typename viennagrid::result_of::const_facet_range<CellType>::type FacetOnCellContainer; typedef typename viennagrid::result_of::iterator<FacetOnCellContainer>::type FacetOnCellIterator; typedef typename viennagrid::result_of::const_coboundary_range<MeshType, FacetType, CellType>::type CellOnFacetContainer; typedef typename viennagrid::result_of::iterator<CellOnFacetContainer>::type CellOnFacetIterator; typedef viennashe::math::sparse_matrix<double> CouplingMatrixType; typedef typename viennashe::she::timestep_quantities<DeviceType>::unknown_quantity_type SpatialUnknownType; std::vector< scattering_base<DeviceType> * > scattering_processes; if (conf.with_traps()) { if (! conf.with_electrons() || ! conf.with_holes()) throw viennashe::unavailable_feature_exception("Trapping without considering electrons or holes is not supported!"); if ( conf.get_electron_equation() != viennashe::EQUATION_SHE) throw viennashe::unavailable_feature_exception("Trapping without SHE for electrons is not supported!"); if ( conf.get_hole_equation() != viennashe::EQUATION_SHE) throw viennashe::unavailable_feature_exception("Trapping without SHE for holes is not supported!"); } // try // { MeshType const & mesh = device.mesh(); SpatialUnknownType const & potential = quantities.get_unknown_quantity(viennashe::quantity::potential()); SpatialUnknownType const & old_potential = old_quantities.get_unknown_quantity(viennashe::quantity::potential()); //TODO: Take old timestep viennashe::she::unknown_she_quantity<VertexT, EdgeT> const & old_quan = old_quantities.she_quantity(quan.get_name()); // // Set up scatter matrices: // const std::size_t L_max = static_cast<std::size_t>(conf.max_expansion_order()); const std::size_t num_harmonics = std::size_t(L_max+1) * std::size_t(L_max+1); CouplingMatrixType scatter_op_in(num_harmonics, num_harmonics); CouplingMatrixType scatter_op_out(num_harmonics, num_harmonics); for (std::size_t i=0; i < std::size_t(L_max+1) * std::size_t(L_max+1); ++i) scatter_op_out(i,i) += 1.0; scatter_op_in(0,0) += 1.0; //// preprocessing: compute coefficients a_{l,m}^{l',m'} and b_{l,m}^{l',m'} std::size_t Lmax = static_cast<std::size_t>(conf.max_expansion_order()); //maximum expansion order std::size_t coupling_rows = static_cast<std::size_t>((Lmax+1) * (Lmax+1)); std::size_t coupling_cols = coupling_rows; log::debug<log_assemble_all>() << "* assemble_all(): Computing coupling matrices..." << std::endl; CouplingMatrixType identity(coupling_rows, coupling_cols); for (std::size_t i=0; i<coupling_rows; ++i) for (std::size_t j=0; j<coupling_cols; ++j) identity(i,j) = (i == j) ? 1.0 : 0.0; CouplingMatrixType a_x(coupling_rows, coupling_cols); CouplingMatrixType a_y(coupling_rows, coupling_cols); CouplingMatrixType a_z(coupling_rows, coupling_cols); CouplingMatrixType b_x(coupling_rows, coupling_cols); CouplingMatrixType b_y(coupling_rows, coupling_cols); CouplingMatrixType b_z(coupling_rows, coupling_cols); //note: interchanged coordinates fill_coupling_matrices(a_x, a_y, a_z, b_x, b_y, b_z, static_cast<int>(Lmax)); CouplingMatrixType a_x_transposed = a_x.trans(); CouplingMatrixType a_y_transposed = a_y.trans(); CouplingMatrixType a_z_transposed = a_z.trans(); CouplingMatrixType b_x_transposed = b_x.trans(); CouplingMatrixType b_y_transposed = b_y.trans(); CouplingMatrixType b_z_transposed = b_z.trans(); if (log_assemble_all::enabled && log_assemble_all::debug) { log::debug<log_assemble_all>() << "a_x: " << a_x << std::endl; log::debug<log_assemble_all>() << "a_y: " << a_y << std::endl; log::debug<log_assemble_all>() << "a_z: " << a_z << std::endl; log::debug<log_assemble_all>() << "b_x: " << b_x << std::endl; log::debug<log_assemble_all>() << "b_y: " << b_y << std::endl; log::debug<log_assemble_all>() << "b_z: " << b_z << std::endl; log::debug<log_assemble_all>() << "identity: " << identity << std::endl; log::debug<log_assemble_all>() << "scatter_op_out: " << scatter_op_out << std::endl; log::debug<log_assemble_all>() << "scatter_op_in: " << scatter_op_in << std::endl; } // // Setup vector of scattering processes: // if (conf.scattering().acoustic_phonon().enabled()) { log::debug<log_assemble_all>() << "assemble(): Acoustic phonon scattering is ENABLED!" << std::endl; scattering_processes.push_back(new acoustic_phonon_scattering<DeviceType>(device, conf)); } if (conf.scattering().optical_phonon().enabled()) { log::debug<log_assemble_all>() << "assemble(): Optical phonon scattering is ENABLED!" << std::endl; scattering_processes.push_back(new optical_phonon_scattering<DeviceType>(device, conf, conf.energy_spacing())); } if (conf.scattering().ionized_impurity().enabled()) { log::debug<log_assemble_all>() << "assemble(): Ionized impurity scattering is ENABLED!" << std::endl; scattering_processes.push_back(new ionized_impurity_scattering<DeviceType>(device, conf)); } if (conf.scattering().impact_ionization().enabled()) { // Warn the user if we already know that he/she is going to simulate bullshit. if ( ! conf.with_holes() || conf.get_hole_equation() != viennashe::EQUATION_SHE ) log::warn() << std::endl << "WARNING: II scattering enabled, but 'BTE for holes' is disabled! Expect inconsistent results!" << std::endl; if ( ! conf.with_electrons() || conf.get_electron_equation() != viennashe::EQUATION_SHE ) log::warn() << std::endl << "WARNING: II scattering enabled, but 'BTE for electrons' is disabled! Expect inconsistent results!" << std::endl; scattering_processes.push_back(new impact_ionization_scattering<DeviceType>(device, conf)); } if (conf.with_traps() && conf.scattering().trapped_charge().enabled()) { log::debug<log_assemble_all>() << "assemble(): Trapped charge scattering is ENABLED!" << std::endl; scattering_processes.push_back(new trapped_charge_scattering<DeviceType, TimeStepQuantitiesT>(device, conf, quantities)); } typedef typename viennashe::electric_field_wrapper<DeviceType, SpatialUnknownType> ElectricFieldAccessor; ElectricFieldAccessor Efield(device, potential); if (conf.scattering().surface().enabled()) { log::debug<log_assemble_all>() << "assemble(): Surface roughness scattering is ENABLED!" << std::endl; scattering_processes.push_back(new surface_scattering<DeviceType, ElectricFieldAccessor>(device, conf, Efield)); } // // Assemble SHE system: // - scattering operators on vertices // - free streaming operator on vertices // - scattering operators on edges // - free streaming operator on edges // - any other stuff (traps on cells, etc.) // if (quan_valid && conf.scattering().electron_electron() && conf.with_electrons()) { log::debug<log_assemble_all>() << "assemble(): Electron electron scattering is ENABLED!" << std::endl; assemble_ee_scattering(device, conf, quan, old_quan, A, b); } // // Step 1: Assemble on even nodes // log::debug<log_assemble_all>() << "* assemble_all(): Even unknowns..." << std::endl; CellContainer cells(mesh); for (CellIterator cit = cells.begin(); cit != cells.end(); ++cit) { log::debug<log_assemble_all>() << "* assemble_all(): Assembling on cell " << *cit << std::endl; for (std::size_t index_H = 0; index_H < quan.get_value_H_size(); ++index_H) { if (log_assemble_all::enabled) log::debug<log_assemble_all>() << "* assemble_all(): Assembling on energy index " << index_H << std::endl; assemble_boundary_on_box(device, conf, quan, A, b, *cit, index_H, identity); if (viennashe::materials::is_conductor(device.get_material(*cit))) continue; // // Scattering operator Q{f} // assemble_scattering_operator_on_box( scattering_processes, device, conf, quan, A, b, *cit, index_H, scatter_op_in, scatter_op_out); } // // Free streaming operator L{f} // // iterate over neighbor cells holding the odd unknowns: FacetOnCellContainer facets_on_cell(*cit); for (FacetOnCellIterator focit = facets_on_cell.begin(); focit != facets_on_cell.end(); ++focit) { if (log_assemble_all::enabled) log::debug<log_assemble_all>() << "* assemble_all(): Assembling coupling with facet " << *focit << std::endl; CellType const *other_cell_ptr = util::get_other_cell_of_facet(mesh, *focit, *cit); if (!other_cell_ptr) continue; //Facet is on the boundary of the simulation domain -> homogeneous Neumann conditions CouplingMatrixType coupling_matrix_diffusion = coupling_matrix_in_direction(a_x, a_y, a_z, *cit, *other_cell_ptr, quan.get_carrier_type_id()); // note that the sign change due to MEDS is included in the choice of the normal vector direction (order of vertex vs. other_vertex): // - B \cdot n for even unknowns, // + B \cdot n for odd unknowns CouplingMatrixType coupling_matrix_drift = coupling_matrix_in_direction(b_x, b_y, b_z, *other_cell_ptr, *cit, quan.get_carrier_type_id()); for (std::size_t index_H = 0; index_H < quan.get_value_H_size(); ++index_H) { assemble_free_streaming_operator_on_box( device, conf, quan, A, b, *cit, *focit, index_H, coupling_matrix_diffusion, coupling_matrix_drift, false); } } //for edges // // Time dependence df/dt (and possibly df/dH * dH/dt) // if (use_timedependence) { for (std::size_t index_H = 0; index_H < quan.get_value_H_size(); ++index_H) { viennashe::she::assemble_timederivative(device, conf, quan, old_quan, A, b, *cit, index_H, identity, potential, old_potential); } } } //for cells // // Step 2: Assemble on odd 'nodes' (i.e. facets). TODO: Resolve code duplication w.r.t. above // log::info<log_assemble_all>() << "* assemble_all(): Odd unknowns..." << std::endl; FacetContainer facets(mesh); for (FacetIterator fit = facets.begin(); fit != facets.end(); ++fit) { if (log_assemble_all::enabled) log::debug<log_assemble_all>() << "* assemble_all(): Assembling on facet " << *fit << std::endl; for (std::size_t index_H = 0; index_H < quan.get_value_H_size(); ++index_H) { if (log_assemble_all::enabled) log::debug<log_assemble_all>() << "* assemble_all(): Assembling on energy index " << index_H << std::endl; // // Scattering operator Q{f} // assemble_scattering_operator_on_box(scattering_processes, device, conf, quan, A, b, *fit, index_H, scatter_op_in, scatter_op_out); } // // Free streaming operator L{f} // // iterate over cells of facet CellOnFacetContainer cells_on_facet(mesh, fit.handle()); for (CellOnFacetIterator cofit = cells_on_facet.begin(); cofit != cells_on_facet.end(); ++cofit) { if (log_assemble_all::enabled) log::debug<log_assemble_all>() << "* assemble_all(): Assembling coupling with cell " << *cofit << std::endl; CellType const *other_cell_ptr = util::get_other_cell_of_facet(mesh, *fit, *cofit); if (!other_cell_ptr) continue; //Facet is on the boundary of the simulation domain -> homogeneous Neumann conditions CouplingMatrixType coupling_matrix_diffusion = coupling_matrix_in_direction(a_x_transposed, a_y_transposed, a_z_transposed, *other_cell_ptr, *cofit, quan.get_carrier_type_id()); // note that the sign change due to MEDS is included in the choice of the normal vector direction (order of vertex vs. other_vertex): // - B \cdot n for even unknowns, // + B \cdot n for odd unknowns CouplingMatrixType coupling_matrix_drift = coupling_matrix_in_direction(b_x_transposed, b_y_transposed, b_z_transposed, *other_cell_ptr, *cofit, quan.get_carrier_type_id()); for (std::size_t index_H = 0; index_H < quan.get_value_H_size(); ++index_H) { assemble_free_streaming_operator_on_box( device, conf, quan, A, b, *cofit, *fit, index_H, coupling_matrix_diffusion, coupling_matrix_drift, true); } } //for vertices // // Time dependence df/dt (and possibly df/dH * dH/dt) // if (use_timedependence) { for (std::size_t index_H = 0; index_H < quan.get_value_H_size(); ++index_H) { viennashe::she::assemble_timederivative(device, conf, quan, old_quan, A, b, *fit, index_H, identity, potential, old_potential); } } } //for facets // Assemble traps on cells (to be integrated into the assembly above): if (conf.with_traps()) { log::debug<log_assemble_all>() << "assemble(): Assembly for traps ..." << std::endl; viennashe::she::assemble_traps(device, quantities, conf, quan, A, b); } // // Cleanup: // for (std::size_t i=0; i<scattering_processes.size(); ++i) { if ( scattering_processes[i] ) delete scattering_processes[i]; scattering_processes[i] = 0; } /* } catch (...) { // // Cleanup: // for (std::size_t i=0; i<scattering_processes.size(); ++i) if ( scattering_processes[i] ) delete scattering_processes[i]; // Rethrow throw; } */ }