void TetMeshRenderUtil<Scalar>::initTetrahedronRenderUtil(bool auto_compute_normal)
{
unsigned int point_num = mesh_->vertNum();
std::vector<Vector<Scalar, 3>> pos_vec(point_num);
for (unsigned int i = 0; i < point_num; ++i)
pos_vec[i] = mesh_->vertPos(i);
unsigned int ele_num = mesh_->eleNum();
std::vector<unsigned int> indices_vec(4 * ele_num);
for(unsigned int i = 0; i < ele_num; ++i)
{
indices_vec[4 * i] = mesh_->eleVertIndex(i, 0);
indices_vec[4 * i + 1] = mesh_->eleVertIndex(i, 1);
indices_vec[4 * i + 2] = mesh_->eleVertIndex(i, 2);
indices_vec[4 * i + 3] = mesh_->eleVertIndex(i, 3);
}
tet_render_util_->setTetrahedrons(pos_vec, indices_vec, auto_compute_normal);
}
//#include <iostream>
float lexical_model_feature_function(const hypothesis* hypo, unsigned int id)
{
configuration* config = configuration::get_instance();
model* system_model = config->get_model();
language_model* lm = system_model->get_language_model(id);
lexical_model* lex = static_cast<lexical_model*>(lm);
const partial_rule* srule = hypo->get_partial_rule();
const rule* trule = hypo->get_rule();
information* info = hypo->get_input();
auto& rule_body = trule->get_target_rule_body();
unsigned int rule_body_size = rule_body.size();
const alignment* align = trule->get_alignment();
const std::vector<const symbol*>& source = *info->get_sentence();
std::vector<const symbol*> source_rule_body;
unsigned int source_size = source.size();
unsigned int source_rule_size;
unsigned int window_size = lex->get_window_size();
unsigned int rwin = window_size / 2;
unsigned int lwin = window_size - rwin - 1;
auto span = hypo->get_span();
float score = 0.0;
get_source_rule(srule, source_rule_body);
source_rule_size = source_rule_body.size();
// get source alignment
std::vector<std::vector<unsigned int>> src_align_vec;
src_align_vec.resize(source_rule_size);
auto& full_align = align->get_alignment();
unsigned int full_align_size = full_align.size();
for (unsigned int i = 0; i < full_align_size; i++) {
auto& align_pair = full_align[i];
src_align_vec[align_pair.first].push_back(align_pair.second);
}
// get absolute position of source symbol
std::vector<unsigned int> pos_vec(source_rule_size, 0);
pos_vec[0] = span.first;
for (unsigned int i = 0; i < rule_body_size; i++) {
const symbol* sym = rule_body[i];
if (sym->is_nonterminal()) {
unsigned int tntid = align->get_target_nonterminal_index(i);
hypothesis* prev_hypo = hypo->get_previous_hypothesis(tntid);
auto pspan = prev_hypo->get_span();
unsigned int sntid;
unsigned int index;
sntid = align->get_nonterminal_map(tntid, direction::target);
index = align->get_source_nonterminal_position(sntid);
pos_vec[index] = pspan.second;
}
}
for (unsigned int i = 0; i < source_rule_size; i++) {
if (i && pos_vec[i] == 0) {
pos_vec[i] = pos_vec[i - 1] + 1;
}
}
std::vector<const std::string*> input;
symbol_table* symtab = symbol_table::get_instance();
const symbol* bos = symtab->search_symbol("<s>");
const symbol* eos = symtab->search_symbol("</s>");
const symbol* null = symtab->search_symbol("<null>");
input.reserve(window_size + 1);
for (unsigned int i = 0; i < source_rule_size; i++) {
const symbol* sym = source_rule_body[i];
unsigned int pos = pos_vec[i];
auto& align_vec = src_align_vec[i];
unsigned int vec_size = align_vec.size();
if (sym->is_nonterminal()) {
// get previous score
unsigned int sntid = align->get_source_nonterminal_index(i);
hypothesis* prev_hypo = hypo->get_previous_hypothesis(sntid);
const feature* prev_fea = prev_hypo->get_feature(id);
score += prev_fea->get_score();
continue;
}
for (unsigned int j = 0; j < lwin; j++) {
int index = pos - lwin + j;
if (index < 0)
input.push_back(bos->get_name());
else
input.push_back(source[index]->get_name());
}
input.push_back(source[pos]->get_name());
for (unsigned int j = 0; j < rwin; j++) {
int index = pos + 1 + j;
if (index >= static_cast<int>(source_size))
input.push_back(eos->get_name());
else
input.push_back(source[index]->get_name());
}
if (vec_size == 0) {
input.push_back(null->get_name());
} else if (vec_size == 1) {
unsigned int tgt_pos = align_vec[0];
const symbol* target_symbol = rule_body[tgt_pos];
if (target_symbol->is_nonterminal()) {
unsigned int tid;
const hypothesis* h;
const feature* fea;
tid = align->get_target_nonterminal_index(tgt_pos);
h = hypo->get_previous_hypothesis(tid);
fea = h->get_feature(id);
score += fea->get_score();
input.clear();
continue;
} else {
input.push_back(target_symbol->get_name());
}
} else {
std::string word;
std::vector<std::string> str_vec;
str_vec.reserve(vec_size);
const symbol* sym;
for (unsigned int j = 0; j < vec_size; j++) {
str_vec.push_back(*rule_body[align_vec[j]]->get_name());
}
string_join(str_vec, "***", word);
sym = symtab->search_symbol(word);
input.push_back(sym->get_name());
}
score += lex->probability(input.data());
input.clear();
}
/*
for (unsigned int i = 0; i < source_rule_size; i++)
std::cout << *source_rule_body[i]->get_name() << " ";
std::cout << score << std::endl;*/
return score;
}
static void get_affiliation(const hypothesis* h, std::vector<unsigned int>& v,
unsigned int id)
{
const rule* r = h->get_rule();
const partial_rule* srule = h->get_partial_rule();
auto& rule_body = r->get_target_rule_body();
const alignment* align = r->get_alignment();
unsigned int rule_body_size = rule_body.size();
unsigned int source_rule_size = srule->get_length();
auto span = h->get_span();
unsigned int indx = 0;
unsigned int source_index;
// get target alignment
std::vector<std::vector<unsigned int>> tgt_align_vec;
tgt_align_vec.resize(rule_body_size);
auto& full_align = align->get_alignment();
unsigned int full_align_size = full_align.size();
//std::cout << "source: ";
//print_partial_rule(srule);
//std::cout << "target: ";
//print_rule(r);
//std::cout << "alignment: ";
for (unsigned int i = 0; i < full_align_size; i++) {
auto& align_pair = full_align[i];
tgt_align_vec[align_pair.second].push_back(align_pair.first);
//std::cout << align_pair.first << "-" << align_pair.second << " ";
}
//std::cout << std::endl;
// get absolute position of source symbol
std::vector<unsigned int> pos_vec(source_rule_size, 0);
pos_vec[0] = span.first;
for (unsigned int i = 0; i < rule_body_size; i++) {
const symbol* sym = rule_body[i];
if (sym->is_nonterminal()) {
unsigned int tntid = align->get_target_nonterminal_index(i);
hypothesis* prev_hypo = h->get_previous_hypothesis(tntid);
auto pspan = prev_hypo->get_span();
unsigned int sntid;
unsigned int index;
sntid = align->get_nonterminal_map(tntid, direction::target);
index = align->get_source_nonterminal_position(sntid);
pos_vec[index] = pspan.second;
}
}
for (unsigned int i = 0; i < source_rule_size; i++) {
if (i && pos_vec[i] == 0) {
pos_vec[i] = pos_vec[i - 1] + 1;
}
}
// find all affiliation of rule
for (unsigned int i = 0; i < rule_body_size; i++) {
const symbol* sym = rule_body[i];
if (sym->is_nonterminal()) {
hypothesis* prev_hypo = h->get_previous_hypothesis(indx++);
const feature* prev_fea = prev_hypo->get_feature(id);
state* prev_state = prev_fea->get_state();
jm_state* prev_jm_state = static_cast<jm_state*>(prev_state);
const auto& prev_affiliation = prev_jm_state->get_affiliation();
auto iter_begin = prev_affiliation->begin();
auto iter_end = prev_affiliation->end();
for (auto iter = iter_begin; iter != iter_end; ++iter)
v.push_back(*iter);
} else {
bool found = false;
const std::vector<unsigned int>* align_vec = &tgt_align_vec[i];
if (align_vec->size() == 0) {
for (unsigned int len = 1; len < rule_body_size; len++) {
int pos1 = i + len;
int pos2 = i - len;
// right side
if (pos1 < static_cast<int>(rule_body_size)) {
const symbol* s = rule_body[pos1];
if (s->is_nonterminal()) {
jm_state* pstate;
hypothesis* phypo;
const feature* fea;
unsigned int ntid;
ntid = align->get_target_nonterminal_index(pos1);
phypo = h->get_previous_hypothesis(ntid);
fea = phypo->get_feature(id);
pstate = static_cast<jm_state*>(fea->get_state());
const auto* pavec = pstate->get_affiliation();
if (pavec->size() != 0) {
source_index = pavec->front();
found = true;
break;
}
} else {
align_vec = &tgt_align_vec[pos1];
if (align_vec->size() != 0)
break;
}
}
// left side
if (pos2 >= 0) {
const symbol* s = rule_body[pos2];
if (s->is_nonterminal()) {
jm_state* pstate;
hypothesis* phypo;
const feature* fea;
unsigned int ntid;
ntid = align->get_target_nonterminal_index(pos2);
phypo = h->get_previous_hypothesis(ntid);
fea = phypo->get_feature(id);
pstate = static_cast<jm_state*>(fea->get_state());
const auto* pavec = pstate->get_affiliation();
if (pavec->size() != 0) {
source_index = pavec->back();
found = true;
break;
}
} else {
align_vec = &tgt_align_vec[pos2];
if (align_vec->size() != 0)
break;
}
}
if (pos1 >= static_cast<int>(rule_body_size) && pos2 < 0)
break;
}
}
if (!found) {
if (align_vec->size() == 0)
throw std::runtime_error("no affiliation found");
unsigned int index = (*align_vec)[align_vec->size() / 2];
source_index = pos_vec[index];
}
v.push_back(source_index);
}
}
}
//=============================================================================
void ImplicitSolver::solve (float dt, float mass,
std::vector<Particle> &particles)
{
int num_particles = particles.size ();
/// Build the Jacobian matrix from the sparse set of elements
Eigen::SparseMatrix<float> J (2 * num_particles, 2 * num_particles);
J.setFromTriplets (triplets_.begin (), triplets_.end ());
/// Build up the position, velocity and force vectors
Eigen::VectorXf pos_vec (Eigen::VectorXf::Zero (2 * num_particles)),
velocity_vec (Eigen::VectorXf::Zero (2 * num_particles)),
force_vec (Eigen::VectorXf::Zero (2 * num_particles));
for (size_t p_i = 0; p_i < num_particles; ++p_i)
{
pos_vec (2 * p_i + 0) = particles[p_i].position[0];
pos_vec (2 * p_i + 1) = particles[p_i].position[1];
velocity_vec (2 * p_i + 0) = particles[p_i].velocity[0];
velocity_vec (2 * p_i + 1) = particles[p_i].velocity[1];
force_vec (2 * p_i + 0) = particles[p_i].force[0];
force_vec (2 * p_i + 1) = particles[p_i].force[1];
}
/// Kick out the fixed particles by creating a sparse selection matrix
std::vector<Eigen::Triplet<float> > triplets_selection;
int valid_particle_index = 0;
for (size_t p_i = 0; p_i < num_particles; ++p_i)
if (!particles[p_i].locked)
{
triplets_selection.push_back (Eigen::Triplet<float> (2 * p_i + 0, 2 * valid_particle_index + 0, 1.f));
triplets_selection.push_back (Eigen::Triplet<float> (2 * p_i + 1, 2 * valid_particle_index + 1, 1.f));
valid_particle_index ++;
}
Eigen::SparseMatrix<float> mat_selection (2 * num_particles, 2 * valid_particle_index);
mat_selection.setFromTriplets (triplets_selection.begin (), triplets_selection.end ());
/// Sparse identity matrix
Eigen::SparseMatrix<float> Id (2 * valid_particle_index, 2 * valid_particle_index);
Id.setIdentity ();
/// Apply the selection matrix on each vector and the Jacobian matrix
pos_vec = mat_selection.transpose () * pos_vec;
velocity_vec = mat_selection.transpose () * velocity_vec;
force_vec = mat_selection.transpose () * force_vec;
J = mat_selection.transpose () * J * mat_selection;
/// Build the right and left hand sides of the linear system
Eigen::SparseMatrix<float> A = Id - dt * dt / mass * J;
Eigen::VectorXf b;
b = dt * velocity_vec + dt * dt / mass * force_vec + (Id - dt * dt / mass * J) * pos_vec;
/// Solve the system and use the selection matrix again to arrange the new positions in a vector
linear_solver_.analyzePattern (A);
linear_solver_.compute (A);
Eigen::VectorXf new_pos = mat_selection * linear_solver_.solve (b);
/// Extract the positions from the solution vector and set the new positions and velocities inside the particle structures
for (size_t p_i = 0; p_i < num_particles; ++p_i)
{
if (!particles[p_i].locked)
{
vec2 pos_update (new_pos (2 * p_i + 0), new_pos (2 * p_i + 1));
particles[p_i].velocity = (pos_update - particles[p_i].position) / dt;
particles[p_i].position = pos_update;
}
}
}
int main(int argc, const char * argv[]) {
//double mu = 4.46275472004e5; //[m^3/s^2]
//double radius = 16000.0;//6378136.3;
//EGM2008 data
double matlabMu = 3.986004418000000e+14;
double mu = matlabMu; //3986004.415e8; //[m^3/s^2]
double radius = 6378137.0;//6378136.3; //[m]
unsigned int degree = 121;
double pos[3], acc[3];
std::vector<double> pos_vec(3), acc_vec(3);
std::string coefficient_file = "/Users/manu/Library/Mobile Documents/com~apple~CloudDocs/CU-Boulder/AVS-Lab/Spherical Harmonics/coefficients/GGM03S.txt";
pos[0] = 7000000.0; //32.0;//6378136.3;
pos[1] = 0.0;
pos[2] = 0.0;
pos_vec[0] = pos[0];
pos_vec[1] = pos[1];
pos_vec[2] = pos[2];
coeffLoaderCSV* loader = new coeffLoaderCSV();
sphericalHarmonics* sphericalHarm = new sphericalHarmonics(loader, coefficient_file, degree, mu, radius);
//Copy test
sphericalHarmonics* otherSpherical = new sphericalHarmonics(*sphericalHarm);
sphericalHarmonics oneAnotherSpherical = *sphericalHarm; //Copy!
#ifdef DEBUG
sphericalHarm->printCoefficients();
sphericalHarm->computeField(pos, degree, acc, true);
printf("Position [%.15f,%.15f,%.15f]. Field: [%.15f,%.15f,%.15f].\n", pos[0], pos[1], pos[2], acc[0], acc[1], acc[2]);
sphericalHarm->getFieldVector(pos_vec, degree, acc_vec, true);
printf("Position [%f,%f,%f]. Field Vector: [%f,%f,%f].\n", pos_vec[0], pos_vec[1], pos_vec[2], acc_vec[0], acc_vec[1], acc_vec[2]);
otherSpherical->computeField(pos, degree, acc, true);
printf("Position [%.15f,%.15f,%.15f]. Field: [%.15f,%.15f,%.15f].\n", pos[0], pos[1], pos[2], acc[0], acc[1], acc[2]);
otherSpherical->getFieldVector(pos_vec, degree, acc_vec, true);
printf("Position [%f,%f,%f]. Field Vector: [%f,%f,%f].\n", pos_vec[0], pos_vec[1], pos_vec[2], acc_vec[0], acc_vec[1], acc_vec[2]);
oneAnotherSpherical.computeField(pos, degree, acc, true);
printf("Position [%.15f,%.15f,%.15f]. Field: [%.15f,%.15f,%.15f].\n", pos[0], pos[1], pos[2], acc[0], acc[1], acc[2]);
oneAnotherSpherical.getFieldVector(pos_vec, degree, acc_vec, true);
printf("Position [%f,%f,%f]. Field Vector: [%f,%f,%f].\n", pos_vec[0], pos_vec[1], pos_vec[2], acc_vec[0], acc_vec[1], acc_vec[2]);
#else
double R = 7000000.0; // [m]
unsigned int samples = 10;
for (unsigned int i = 0; i < samples; i++)
{
for (unsigned int j = 0; j < samples; j++)
{
double latitude = -M_PI/2.0 + M_PI / samples * i;
double longitude = 2.0 * M_PI / samples * j;
pos[0] = R * cos(latitude) * cos(longitude);
pos[1] = R * cos(latitude)* sin(longitude);
pos[2] = R * sin(latitude);
pos_vec[0] = pos[0];
pos_vec[1] = pos[1];
pos_vec[2] = pos[2];
sphericalHarm->computeField(pos, degree, acc, true);
printf("Latitude: %f, Longitude: %f\n", latitude*180.0/M_PI, longitude*180.0/M_PI);
printf("Position [%f,%f,%f]. Field: [%f,%f,%f].\n", pos[0], pos[1], pos[2], acc[0], acc[1], acc[2]);
sphericalHarm->getFieldVector(pos_vec, degree, acc_vec, true);
printf("Latitude: %f, Longitude: %f\n", latitude*180.0/M_PI, longitude*180.0/M_PI);
printf("Position [%f,%f,%f]. Field Vector: [%f,%f,%f].\n", pos_vec[0], pos_vec[1], pos_vec[2], acc_vec[0], acc_vec[1], acc_vec[2]);
}
}
#endif
delete sphericalHarm;
delete loader;
return 0;
}
