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;
}
*/
}
