static int test_nad27_ll(double lat, double lon,
double expected_lat, double expected_lon)
{
double lats[1];
double lons[1];
double hts[1];
char input_projection_info[255];
char output_projection_info[255];
projPJ input_projection, output_projection;
sprintf(input_projection_info, "+proj=latlong +datum=NAD27");
sprintf(output_projection_info, "+proj=latlong +datum=NAD83");
input_projection = pj_init_plus(input_projection_info);
output_projection = pj_init_plus(output_projection_info);
lats[0] = lat * D2R;
lons[0] = lon * D2R;
hts[0] = 0;
pj_transform (input_projection, output_projection, 1, 1,
lats, lons, hts);
if (pj_errno != 0) {
asfPrintWarning("libproj error: %s\n", pj_strerrno(pj_errno));
}
pj_free(input_projection);
pj_free(output_projection);
lats[0] *= R2D;
lons[0] *= R2D;
CU_ASSERT(double_equals(lats[0], expected_lat, 6));
CU_ASSERT(double_equals(lons[0], expected_lon, 6));
if (double_equals(lats[0], expected_lat, 6) &&
double_equals(lons[0], expected_lon, 6))
{
//asfPrintStatus("Proj (fwd): %f, %f ... OK\n", lat, lon);
return TRUE;
}
else
{
asfPrintStatus("Proj (fwd): %f, %f ... ERROR\n", lat, lon);
asfPrintStatus("Result: %.10f %.10f (%.10f)\n",
lats[0], lons[0], hts[0]);
asfPrintStatus("Expected: %.10f %.10f\n",
expected_lat, expected_lon);
return FALSE;
}
}
void crf_context_alpha_score(crf_context_t *self) {
double *scale = self->scale_factor->a;
const double *prev = NULL;
const double *trans = NULL;
const double *state = NULL;
const double *state_trans = NULL;
const size_t T = self->num_items;
const size_t L = self->num_labels;
/* Compute the alpha scores on nodes 0, *).
alpha[0][j] = state[0][j]
At this point we have no transition features.
*/
double *cur = alpha_score(self, 0);
state = exp_state_score(self, 0);
double_array_raw_copy(cur, state, L);
double sum = double_array_sum(cur, L);
double scale_t = !double_equals(sum, 0.) ? 1. / sum : 1.;
scale[0] = scale_t;
double_array_mul(cur, scale[0], L);
/* Compute the alpha scores on nodes (t, *).
alpha[t][j] = state[t][j] * \sum_{i} alpha[t-1][i] * state_trans[t][i][j] * trans[i][j]
*/
for (size_t t = 1; t < T; t++) {
prev = alpha_score(self, t - 1);
cur = alpha_score(self, t);
state = exp_state_score(self, t);
double_array_zero(cur, L);
for (size_t i = 0; i < L; i++) {
trans = exp_trans_score(self, i);
state_trans = exp_state_trans_score(self, t, i);
for (size_t j = 0; j < L; j++) {
cur[j] += prev[i] * trans[j] * state_trans[j];
}
}
double_array_mul_array(cur, state, L);
sum = double_array_sum(cur, L);
scale[t] = scale_t = !double_equals(sum, 0.) ? 1. / sum : 1.;
double_array_mul(cur, scale_t, L);
}
/* Compute the logarithm of the normalization factor here.
norm = 1. / (C[0] * C[1] ... * C[T-1])
log(norm) = - \sum_{t = 0}^{T-1} log(C[t])
*/
self->log_norm = -double_array_sum_log(scale, T);
}
static inline bool is_all_zeros(const mat& m) {
for (mat::const_iterator it = m.begin();
it != m.end(); ++it)
if (!double_equals(*it, 0.0))
return false;
return true;
}
vec& LinkedTreeRobot::getQDot(const vec& desired, vec& qdot, vec& axes) {
vec P(3 * _numEffectors);
getEffectorPositions(P); // position of the effectors currently
vec F = clamp(desired - P, 0.5);
mat J;
computeJacobian(desired, J, axes);
mat Jc;
//cout << "++ Going to compute constraint jacobian ++" << endl;
computeConstraintJacobian(desired, axes, Jc);
//cout << "++ Finished computed constraint jacobian ++" << endl;
//cout << "Jc:" << endl << Jc << endl;
vec ga = trans(J) * F;
//cout << "ga:" << endl << ga << endl;
vec gc;
if (Jc.n_elem == 0 || is_all_zeros(Jc)) // no constraints, or all constraints OK
gc.zeros(ga.n_elem);
else {
vec b = -Jc * ga;
//cout << "b:" << endl << b << endl;
mat JcT = trans(Jc);
mat A = Jc * JcT;
//cout << "A:" << endl << A << endl;
vec d;
mat U, V;
if (!svd(U, d, V, A))
cerr << "could not compute SVD" << endl;
//cout << "U:" << endl << U << endl;
//cout << "V:" << endl << V << endl;
//cout << "d:" << endl << d << endl;
//vec lambda = V * inv(D) * trans(U) * b; // too naive
vec utb = trans(U) * b;
vec dinv_ut_b(d.n_elem);
dinv_ut_b.zeros();
for (size_t d_idx = 0; d_idx < d.n_elem; d_idx++) {
double elem = d[d_idx];
if (!double_equals(elem, 0.0))
dinv_ut_b[d_idx] = 1.0 / elem * utb[d_idx];
}
gc = JcT * (V * dinv_ut_b); // Jc^T * lambda
}
qdot = ga + gc; // qdot
return qdot;
}
void NetworkCm02Model::updateActionsStateLazy(double now, double /*delta*/)
{
NetworkCm02ActionPtr action;
while ((xbt_heap_size(p_actionHeap) > 0)
&& (double_equals(xbt_heap_maxkey(p_actionHeap), now, sg_surf_precision))) {
action = (NetworkCm02ActionPtr) xbt_heap_pop(p_actionHeap);
XBT_DEBUG("Something happened to action %p", action);
#ifdef HAVE_TRACING
if (TRACE_is_enabled()) {
int n = lmm_get_number_of_cnst_from_var(p_maxminSystem, action->getVariable());
int i;
for (i = 0; i < n; i++){
lmm_constraint_t constraint = lmm_get_cnst_from_var(p_maxminSystem,
action->getVariable(),
i);
NetworkCm02LinkPtr link = static_cast<NetworkCm02LinkPtr>(lmm_constraint_id(constraint));
TRACE_surf_link_set_utilization(link->getName(),
action->getCategory(),
(lmm_variable_getvalue(action->getVariable())*
lmm_get_cnst_weight_from_var(p_maxminSystem,
action->getVariable(),
i)),
action->getLastUpdate(),
now - action->getLastUpdate());
}
}
#endif
// if I am wearing a latency hat
if (action->getHat() == LATENCY) {
XBT_DEBUG("Latency paid for action %p. Activating", action);
lmm_update_variable_weight(p_maxminSystem, action->getVariable(), action->m_weight);
action->heapRemove(p_actionHeap);
action->refreshLastUpdate();
// if I am wearing a max_duration or normal hat
} else if (action->getHat() == MAX_DURATION ||
action->getHat() == NORMAL) {
// no need to communicate anymore
// assume that flows that reached max_duration have remaining of 0
XBT_DEBUG("Action %p finished", action);
action->setRemains(0);
action->finish();
action->setState(SURF_ACTION_DONE);
action->heapRemove(p_actionHeap);
action->gapRemove();
}
}
return;
}
void NetworkIBModel::computeIBfactors(IBNode *root) {
double penalized_bw=0.0;
double num_comm_out = (double) root->ActiveCommsUp.size();
double max_penalty_out=0.0;
//first, compute all outbound penalties to get their max
for (std::vector<ActiveComm*>::iterator it= root->ActiveCommsUp.begin(); it != root->ActiveCommsUp.end(); ++it) {
double my_penalty_out = 1.0;
if(num_comm_out!=1){
if((*it)->destination->nbActiveCommsDown > 2)//number of comms sent to the receiving node
my_penalty_out = num_comm_out * Bs * ys;
else
my_penalty_out = num_comm_out * Bs;
}
max_penalty_out = max(max_penalty_out,my_penalty_out);
}
for (std::vector<ActiveComm*>::iterator it= root->ActiveCommsUp.begin(); it != root->ActiveCommsUp.end(); ++it) {
//compute inbound penalty
double my_penalty_in = 1.0;
int nb_comms = (*it)->destination->nbActiveCommsDown;//total number of incoming comms
if(nb_comms!=1)
my_penalty_in = ((*it)->destination->ActiveCommsDown)[root] //number of comm sent to dest by root node
* Be
* (*it)->destination->ActiveCommsDown.size();//number of different nodes sending to dest
double penalty=max(my_penalty_in,max_penalty_out);
double rate_before_update = (*it)->action->getBound();
//save initial rate of the action
if((*it)->init_rate==-1)
(*it)->init_rate= rate_before_update;
penalized_bw= ! num_comm_out ? (*it)->init_rate : (*it)->init_rate /penalty;
if (!double_equals(penalized_bw, rate_before_update, sg_surf_precision)){
XBT_DEBUG("%d->%d action %p penalty updated : bw now %f, before %f , initial rate %f", root->id,(*it)->destination->id,(*it)->action,penalized_bw, (*it)->action->getBound(), (*it)->init_rate );
lmm_update_variable_bound(p_maxminSystem, (*it)->action->getVariable(), penalized_bw);
}else{
XBT_DEBUG("%d->%d action %p penalty not updated : bw %f, initial rate %f", root->id,(*it)->destination->id,(*it)->action,penalized_bw, (*it)->init_rate );
}
}
XBT_DEBUG("Finished computing IB penalties");
}
static void
test_p2ll(meta_projection *mp, const char *proj_name,
double projY, double projX, double height, int zone,
double lon, double lat, datum_type_t datum)
{
double testLat, testLon, z;
proj_to_latlon(mp, projX, projY, height, &testLat, &testLon, &z);
testLat *= R2D;
testLon *= R2D;
CU_ASSERT(double_equals(lat, testLat, 5));
CU_ASSERT(double_equals(lon, testLon, 5));
if (double_equals(lat, testLat, 5) && double_equals(lon, testLon, 5)) {
//asfPrintStatus("%s proj_to_latlon Testing: %f, %f ... OK\n",
// proj_name, projX, projY);
++n_ok;
} else {
asfPrintStatus("%s proj_to_latlon Testing: %f, %f ... ERROR\n",
proj_name, projX, projY);
asfPrintStatus("Result: %.10f %.10f (%.10f)\n", testLat, testLon, z);
asfPrintStatus("Expected: %.10f %.10f\n", lat, lon);
++n_bad;
}
double x, y;
latlon_to_proj(mp, '?', lat*D2R, lon*D2R, height, &x, &y, &z);
CU_ASSERT(double_equals(x, projX, 5));
CU_ASSERT(double_equals(y, projY, 5));
if (double_equals(x, projX, 5) && double_equals(y, projY, 5)) {
//asfPrintStatus("%s latlon_to_proj Testing: %f, %f ... OK\n",
// proj_name, lat, lon);
++n_ok;
} else {
asfPrintStatus("%s latlon_to_proj Testing: %f, %f ... ERROR\n",
proj_name, lat, lon);
asfPrintStatus("Result: %.10f %.10f (%.10f)\n", x, y, z);
asfPrintStatus("Expected: %.10f %.10f\n", projX, projY);
++n_bad;
}
}
/*********
* Model *
*********/
void CpuModel::updateActionsStateLazy(double now, double /*delta*/)
{
CpuAction *action;
while ((xbt_heap_size(getActionHeap()) > 0)
&& (double_equals(xbt_heap_maxkey(getActionHeap()), now, sg_surf_precision))) {
action = static_cast<CpuAction*>(xbt_heap_pop(getActionHeap()));
XBT_CDEBUG(surf_kernel, "Something happened to action %p", action);
if (TRACE_is_enabled()) {
Cpu *cpu = static_cast<Cpu*>(lmm_constraint_id(lmm_get_cnst_from_var(getMaxminSystem(), action->getVariable(), 0)));
TRACE_surf_host_set_utilization(cpu->getName(), action->getCategory(),
lmm_variable_getvalue(action->getVariable()),
action->getLastUpdate(),
now - action->getLastUpdate());
}
action->finish();
XBT_CDEBUG(surf_kernel, "Action %p finished", action);
/* set the remains to 0 due to precision problems when updating the remaining amount */
action->setRemains(0);
action->setState(SURF_ACTION_DONE);
action->heapRemove(getActionHeap()); //FIXME: strange call since action was already popped
}
if (TRACE_is_enabled()) {
//defining the last timestamp that we can safely dump to trace file
//without losing the event ascending order (considering all CPU's)
double smaller = -1;
ActionList *actionSet = getRunningActionSet();
for(ActionList::iterator it(actionSet->begin()), itend(actionSet->end())
; it != itend ; ++it) {
action = static_cast<CpuAction*>(&*it);
if (smaller < 0) {
smaller = action->getLastUpdate();
continue;
}
if (action->getLastUpdate() < smaller) {
smaller = action->getLastUpdate();
}
}
if (smaller > 0) {
TRACE_last_timestamp_to_dump = smaller;
}
}
return;
}
Matrix matrix_inverse(Matrix M)
{
if(!is_square_matrix(M))
return NULL;
int n = M->r;
Matrix N = matrix_new_empty(n, n);
double det = matrix_determinant(M);
if(double_equals(det, 0))
return NULL;
for(int i = 0; i < n; i++)
for(int j = 0; j < n; j++) {
Matrix C = matrix_cofactor(M, i, j);
if((i + j) % 2 == 0)
N->A[i][j] = matrix_determinant(C) / det;
else
N->A[i][j] = -1 * matrix_determinant(C) / det;
matrix_free(C);
}
Matrix I = matrix_transpose(N);
matrix_free(N);
assert(is_square_matrix(I));
return I;
}
double soft_tfidf_similarity_with_phrases_and_acronyms(size_t num_tokens1, char **tokens1, double *token_scores1, phrase_array *phrases1, uint32_array *ordinal_suffixes1, size_t num_tokens2, char **tokens2, double *token_scores2, phrase_array *phrases2, uint32_array *ordinal_suffixes2, phrase_array *acronym_alignments, phrase_array *multi_word_alignments, soft_tfidf_options_t options, size_t *num_matches) {
if (token_scores1 == NULL || token_scores2 == NULL) return 0.0;
if (num_tokens1 > num_tokens2 || (num_tokens1 == num_tokens2 && sum_token_lengths(num_tokens1, tokens1) > sum_token_lengths(num_tokens2, tokens2))) {
double *tmp_scores = token_scores1;
token_scores1 = token_scores2;
token_scores2 = tmp_scores;
char **tmp_tokens = tokens1;
tokens1 = tokens2;
tokens2 = tmp_tokens;
phrase_array *tmp_phrases = phrases1;
phrases1 = phrases2;
phrases2 = tmp_phrases;
uint32_array *tmp_suffixes = ordinal_suffixes1;
ordinal_suffixes1 = ordinal_suffixes2;
ordinal_suffixes2 = tmp_suffixes;
size_t tmp_num_tokens = num_tokens1;
num_tokens1 = num_tokens2;
num_tokens2 = tmp_num_tokens;
}
size_t len1 = num_tokens1;
size_t len2 = num_tokens2;
double total_sim = 0.0;
uint32_array **t1_tokens_unicode = NULL;
uint32_array **t2_tokens_unicode = NULL;
uint32_array *t1_unicode;
uint32_array *t2_unicode;
int64_array *phrase_memberships_array1 = NULL;
int64_array *phrase_memberships_array2 = NULL;
int64_t *phrase_memberships1 = NULL;
int64_t *phrase_memberships2 = NULL;
int64_array *acronym_memberships_array = NULL;
int64_t *acronym_memberships = NULL;
int64_array *multi_word_memberships_array = NULL;
int64_t *multi_word_memberships = NULL;
t1_tokens_unicode = calloc(len1, sizeof(uint32_array *));
if (t1_tokens_unicode == NULL) {
total_sim = -1.0;
goto return_soft_tfidf_score;
}
for (size_t i = 0; i < len1; i++) {
t1_unicode = unicode_codepoints(tokens1[i]);
if (t1_unicode == NULL) {
total_sim = -1.0;
goto return_soft_tfidf_score;
}
t1_tokens_unicode[i] = t1_unicode;
}
t2_tokens_unicode = calloc(len2, sizeof(uint32_array *));
if (t2_tokens_unicode == NULL) {
total_sim = -1.0;
goto return_soft_tfidf_score;
}
for (size_t j = 0; j < len2; j++) {
t2_unicode = unicode_codepoints(tokens2[j]);
if (t2_unicode == NULL) {
total_sim = -1.0;
goto return_soft_tfidf_score;
}
t2_tokens_unicode[j] = t2_unicode;
}
if (phrases1 != NULL && phrases2 != NULL) {
phrase_memberships_array1 = int64_array_new();
phrase_memberships_array2 = int64_array_new();
token_phrase_memberships(phrases1, phrase_memberships_array1, len1);
token_phrase_memberships(phrases2, phrase_memberships_array2, len2);
if (phrase_memberships_array1->n == len1) {
phrase_memberships1 = phrase_memberships_array1->a;
}
if (phrase_memberships_array2->n == len2) {
phrase_memberships2 = phrase_memberships_array2->a;
}
}
if (acronym_alignments != NULL) {
acronym_memberships_array = int64_array_new();
token_phrase_memberships(acronym_alignments, acronym_memberships_array, len2);
if (acronym_memberships_array->n == len2) {
acronym_memberships = acronym_memberships_array->a;
}
}
if (multi_word_alignments != NULL) {
multi_word_memberships_array = int64_array_new();
token_phrase_memberships(multi_word_alignments, multi_word_memberships_array, len2);
if (multi_word_memberships_array->n == len2) {
multi_word_memberships = multi_word_memberships_array->a;
}
}
uint32_t *suffixes1;
uint32_t *suffixes2;
if (ordinal_suffixes1 != NULL && ordinal_suffixes2 != NULL) {
suffixes1 = ordinal_suffixes1->a;
suffixes2 = ordinal_suffixes2->a;
}
double jaro_winkler_min = options.jaro_winkler_min;
size_t jaro_winkler_min_length = options.jaro_winkler_min_length;
size_t damerau_levenshtein_max = options.damerau_levenshtein_max;
size_t damerau_levenshtein_min_length = options.damerau_levenshtein_min_length;
size_t strict_abbreviation_min_length = options.strict_abbreviation_min_length;
bool possible_affine_gap_abbreviations = options.possible_affine_gap_abbreviations;
double norm1_offset = 0.0;
double norm2_offset = 0.0;
size_t matched_tokens = 0;
for (size_t i = 0; i < len1; i++) {
uint32_array *t1u = t1_tokens_unicode[i];
uint32_array *t2u;
double t1_score = token_scores1[i];
size_t t1_len = t1u->n;
log_debug("t1 = %s\n", tokens1[i]);
double max_sim = 0.0;
size_t min_dist = t1_len;
size_t argmax_sim = 0;
size_t argmin_dist = 0;
double argmin_dist_sim = 0.0;
size_t last_abbreviation = 0;
double last_abbreviation_sim = 0.0;
bool have_abbreviation = false;
bool have_strict_abbreviation = false;
bool have_acronym_match = false;
size_t last_ordinal_suffix = 0;
bool have_ordinal_suffix = false;
phrase_t acronym_phrase = NULL_PHRASE;
bool have_phrase_match = false;
int64_t pm1 = phrase_memberships1 != NULL ? phrase_memberships1[i] : NULL_PHRASE_MEMBERSHIP;
phrase_t p1 = pm1 >= 0 ? phrases1->a[pm1] : NULL_PHRASE;
phrase_t argmax_phrase = NULL_PHRASE;
uint32_t ordinal_suffix_i = 0;
if (suffixes1 != NULL) {
ordinal_suffix_i = suffixes1[i];
}
bool have_multi_word_match = false;
phrase_t multi_word_phrase = NULL_PHRASE;
bool use_jaro_winkler = t1_len >= jaro_winkler_min_length;
bool use_strict_abbreviation_sim = t1_len >= strict_abbreviation_min_length;
bool use_damerau_levenshtein = damerau_levenshtein_max > 0 && t1_len >= damerau_levenshtein_min_length;
log_debug("use_jaro_winkler = %d, use_damerau_levenshtein=%d\n", use_jaro_winkler, use_damerau_levenshtein);
bool have_equal = false;
canonical_match_t best_canonical_phrase_response = CANONICAL_NO_MATCH;
double t2_score;
log_debug("p1.len = %zu, i = %zu, p1.start = %zu\n", p1.len, i, p1.start);
if (p1.len > 0 && i > p1.start) {
log_debug("skipping token\n");
continue;
}
size_t slen1 = strlen(tokens1[i]);
for (size_t j = 0; j < len2; j++) {
t2u = t2_tokens_unicode[j];
log_debug("t2 = %s\n", tokens2[j]);
int64_t pm2 = phrase_memberships2 != NULL ? phrase_memberships2[j] : NULL_PHRASE_MEMBERSHIP;
phrase_t p2 = pm2 >= 0 ? phrases2->a[pm2] : NULL_PHRASE;
uint32_t ordinal_suffix_j = 0;
if (suffixes2 != NULL) {
ordinal_suffix_j = suffixes2[j];
}
canonical_match_t canonical_response = CANONICAL_NO_MATCH;
if (p1.len > 0 && p2.len > 0 && phrases_have_same_canonical(num_tokens1, tokens1, num_tokens2, tokens2, p1, p2, &canonical_response)) {
if (canonical_response > best_canonical_phrase_response) {
log_debug("canonical_response = %d\n", canonical_response);
best_canonical_phrase_response = canonical_response;
argmax_sim = j;
argmax_phrase = p2;
max_sim = 1.0;
have_phrase_match = true;
continue;
}
}
if (unicode_equals(t1u, t2u)) {
max_sim = 1.0;
argmax_sim = j;
have_equal = true;
break;
}
if (acronym_memberships != NULL) {
int64_t acronym_membership = acronym_memberships[j];
log_debug("acronym_membership = %zd\n", acronym_membership);
if (acronym_membership >= 0) {
acronym_phrase = acronym_alignments->a[acronym_membership];
uint32_t acronym_match_index = acronym_phrase.data;
if (acronym_match_index == i) {
max_sim = 1.0;
argmax_sim = j;
have_acronym_match = true;
log_debug("have acronym match\n");
break;
}
}
}
if (multi_word_memberships != NULL) {
int64_t multi_word_membership = multi_word_memberships[j];
log_debug("multi_word_membership = %zd\n", multi_word_membership);
if (multi_word_membership >= 0) {
multi_word_phrase = multi_word_alignments->a[multi_word_membership];
uint32_t multi_word_match_index = multi_word_phrase.data;
if (multi_word_match_index == i) {
max_sim = 1.0;
argmax_sim = j;
have_multi_word_match = true;
log_debug("have multi-word match\n");
break;
}
}
}
double jaro_winkler = jaro_winkler_distance_unicode(t1u, t2u);
if (jaro_winkler > max_sim) {
max_sim = jaro_winkler;
argmax_sim = j;
}
if (use_damerau_levenshtein) {
size_t replace_cost = 0;
ssize_t dist = damerau_levenshtein_distance_unicode(t1u, t2u, replace_cost);
if (dist >= 0 && dist < min_dist) {
min_dist = (size_t)dist;
argmin_dist = j;
argmin_dist_sim = jaro_winkler;
}
}
if (possible_affine_gap_abbreviations) {
bool is_abbreviation = possible_abbreviation_unicode(t1u, t2u);
if (is_abbreviation) {
last_abbreviation = j;
last_abbreviation_sim = jaro_winkler;
have_abbreviation = true;
if (use_strict_abbreviation_sim && possible_abbreviation_unicode_strict(t1u, t2u)) {
last_abbreviation_sim = last_abbreviation_sim > options.strict_abbreviation_sim ? last_abbreviation_sim : options.strict_abbreviation_sim;
}
}
}
if (ordinal_suffix_i > 0) {
size_t slen2 = strlen(tokens2[j]);
if (utf8_common_prefix_len(tokens1[i], tokens2[j], slen2) == slen2) {
last_ordinal_suffix = j;
have_ordinal_suffix = true;
}
} else if (ordinal_suffix_j > 0) {
if (utf8_common_prefix_len(tokens1[i], tokens2[j], slen1) == slen1) {
last_ordinal_suffix = j;
have_ordinal_suffix = true;
}
}
}
// Note: here edit distance, affine gap and abbreviations are only used in the thresholding process.
// Jaro-Winkler is still used to calculate similarity
if (!have_acronym_match && !have_phrase_match && !have_multi_word_match) {
if (have_equal || (use_jaro_winkler && (max_sim > jaro_winkler_min || double_equals(max_sim, jaro_winkler_min)))) {
log_debug("jaro-winkler, max_sim = %f\n", max_sim);
t2_score = token_scores2[argmax_sim];
double jaro_winkler_sim = 0.0;
if (have_abbreviation && argmax_sim == last_abbreviation) {
double abbrev_sim = last_abbreviation_sim > max_sim ? last_abbreviation_sim : max_sim;
log_debug("have abbreviation, max(max_sim, last_abbreviation_sim) = %f\n", abbrev_sim);
double max_score = 0.0;
if (t1_score > t2_score || double_equals(t1_score, t2_score)) {
norm2_offset += (t1_score * t1_score) - (t2_score * t2_score);
log_debug("t1_score >= t2_score, norm2_offset = %f\n", norm2_offset);
max_score = t1_score;
} else {
norm1_offset += (t2_score * t2_score) - (t1_score * t1_score);
log_debug("t2_score > t1_score, norm1_offset = %f\n", norm1_offset);
max_score = t2_score;
}
jaro_winkler_sim = abbrev_sim * max_score * max_score;
} else {
jaro_winkler_sim = max_sim * t1_score * t2_score;
log_debug("t1_score = %f, t2_score = %f, jaro_winkler_sim = %f\n", t1_score, t2_score, jaro_winkler_sim);
}
total_sim += jaro_winkler_sim;
matched_tokens++;
} else if (use_damerau_levenshtein && min_dist <= damerau_levenshtein_max) {
log_debug("levenshtein, argmin_dist_sim = %f\n", argmin_dist_sim);
t2_score = token_scores2[argmin_dist];
if (have_abbreviation && argmin_dist == last_abbreviation) {
argmin_dist_sim = last_abbreviation_sim > argmin_dist_sim ? last_abbreviation_sim : argmin_dist_sim;
}
total_sim += argmin_dist_sim * t1_score * t2_score;
matched_tokens++;
} else if (possible_affine_gap_abbreviations && have_abbreviation) {
log_debug("have abbreviation, last_abbreviation_sim = %f\n", last_abbreviation_sim);
t2_score = token_scores2[last_abbreviation];
total_sim += last_abbreviation_sim * t1_score * t2_score;
matched_tokens++;
} else if (have_ordinal_suffix) {
log_debug("have ordinal suffix from %zu\n", last_ordinal_suffix);
t2_score = token_scores2[last_ordinal_suffix];
total_sim += 1.0 * t1_score * t2_score;
matched_tokens++;
}
} else if (have_phrase_match) {
double p2_score = 0.0;
for (size_t p = argmax_phrase.start; p < argmax_phrase.start + argmax_phrase.len; p++) {
t2_score = token_scores2[p];
p2_score += t2_score * t2_score;
}
double p1_score = 0.0;
for (size_t p = p1.start; p < p1.start + p1.len; p++) {
double t1_score_p = token_scores1[p];
p1_score += t1_score_p * t1_score_p;
}
total_sim += sqrt(p1_score) * sqrt(p2_score);
matched_tokens += p1.len;
log_debug("have_phrase_match\n");
} else if (have_multi_word_match) {
double multi_word_score = 0.0;
for (size_t p = multi_word_phrase.start; p < multi_word_phrase.start + multi_word_phrase.len; p++) {
t2_score = token_scores2[p];
multi_word_score += t2_score * t2_score;
}
double norm_multi_word_score = sqrt(multi_word_score);
double max_multi_word_score = 0.0;
if (t1_score > norm_multi_word_score || double_equals(t1_score, norm_multi_word_score)) {
norm2_offset += (t1_score * t1_score) - multi_word_score;
log_debug("t1_score >= norm_multi_word_score, norm2_offset = %f\n", norm2_offset);
max_multi_word_score = t1_score;
} else {
norm1_offset += multi_word_score - (t1_score * t1_score);
log_debug("norm_multi_word_score > t1_score, norm1_offset = %f\n", norm1_offset);
max_multi_word_score = norm_multi_word_score;
}
log_debug("max_multi_word_score = %f\n", max_multi_word_score);
total_sim += max_multi_word_score * max_multi_word_score;
log_debug("have multi-word match\n");
matched_tokens++;
} else if (have_acronym_match) {
double acronym_score = 0.0;
for (size_t p = acronym_phrase.start; p < acronym_phrase.start + acronym_phrase.len; p++) {
t2_score = token_scores2[p];
acronym_score += t2_score * t2_score;
}
double norm_acronym_score = sqrt(acronym_score);
double max_acronym_score = 0.0;
if (t1_score > norm_acronym_score || double_equals(t1_score, norm_acronym_score)) {
norm2_offset += (t1_score * t1_score) - acronym_score;
log_debug("t1_score >= norm_acronym_score, norm2_offset = %f\n", norm2_offset);
max_acronym_score = t1_score;
} else {
norm1_offset += acronym_score - (t1_score * t1_score);
log_debug("norm_acronym_score > t1_score, norm1_offset = %f\n", norm1_offset);
max_acronym_score = norm_acronym_score;
}
log_debug("max_acronym_score = %f\n", max_acronym_score);
total_sim += max_acronym_score * max_acronym_score;
log_debug("have_acronym_match\n");
matched_tokens++;
}
log_debug("total sim = %f\n", total_sim);
}
log_debug("matched_tokens = %zu\n", matched_tokens);
*num_matches = matched_tokens;
return_soft_tfidf_score:
if (t1_tokens_unicode != NULL) {
for (size_t i = 0; i < len1; i++) {
t1_unicode = t1_tokens_unicode[i];
if (t1_unicode != NULL) {
uint32_array_destroy(t1_unicode);
}
}
free(t1_tokens_unicode);
}
if (t2_tokens_unicode != NULL) {
for (size_t i = 0; i < len2; i++) {
t2_unicode = t2_tokens_unicode[i];
if (t2_unicode != NULL) {
uint32_array_destroy(t2_unicode);
}
}
free(t2_tokens_unicode);
}
if (phrase_memberships_array1 != NULL) {
int64_array_destroy(phrase_memberships_array1);
}
if (phrase_memberships_array2 != NULL) {
int64_array_destroy(phrase_memberships_array2);
}
if (acronym_memberships_array != NULL) {
int64_array_destroy(acronym_memberships_array);
}
if (multi_word_memberships_array != NULL) {
int64_array_destroy(multi_word_memberships_array);
}
double norm = sqrt(double_array_sum_sq(token_scores1, num_tokens1) + norm1_offset) * sqrt(double_array_sum_sq(token_scores2, num_tokens2) + norm2_offset);
log_debug("total_sim = %f, norm1_offset = %f, norm2_offset = %f, norm = %f\n", total_sim, norm1_offset, norm2_offset, norm);
double total_sim_norm = total_sim / norm;
return total_sim_norm > 1.0 ? 1.0 : total_sim_norm;
}
static void
test_p2utm(double projY, double projX, double height, int zone,
double lon, double lat)
{
double testLat, testLon;
UTM2latLon(projX, projY, height, zone, &testLat, &testLon);
CU_ASSERT(double_equals(lat, testLat, 5));
CU_ASSERT(double_equals(lon, testLon, 5));
if (double_equals(lat, testLat, 5) && double_equals(lon, testLon, 5)) {
//asfPrintStatus("UTM2latLon Testing: %f, %f ... OK\n", projX, projY);
++n_ok;
} else {
asfPrintStatus("UTM2latLon Testing: %f, %f ... ERROR\n",
projX, projY);
asfPrintStatus("Result: %.10f %.10f\n", testLat, testLon);
asfPrintStatus("Expected: %.10f %.10f\n", lat, lon);
++n_bad;
}
double x, y;
latLon2UTM(lat, lon, height, &x, &y);
CU_ASSERT(double_equals(x, projX, 5));
CU_ASSERT(double_equals(y, projY, 5));
if (double_equals(x, projX, 5) && double_equals(y, projY, 5)) {
//asfPrintStatus("latLon2UTM Testing: %f, %f ... OK\n", lat, lon);
++n_ok;
} else {
asfPrintStatus("latLon2UTM Testing: %f, %f ... ERROR\n", lat, lon);
asfPrintStatus("Result: %.10f %.10f\n", x, y);
asfPrintStatus("Expected: %.10f %.10f\n", projX, projY);
++n_bad;
}
latLon2UTM_zone(lat, lon, height, zone, &x, &y);
CU_ASSERT(double_equals(x, projX, 5));
CU_ASSERT(double_equals(y, projY, 5));
if (double_equals(x, projX, 5) && double_equals(y, projY, 5)) {
//asfPrintStatus("latLon2UTM_zone Testing: %f, %f ... OK\n", lat, lon);
++n_ok;
} else {
asfPrintStatus("latLon2UTM_zone Testing: %f, %f ... ERROR\n", lat, lon);
asfPrintStatus("Result: %.10f %.10f\n", x, y);
asfPrintStatus("Expected: %.10f %.10f\n", projX, projY);
++n_bad;
}
}
static void
test_proj_direct(const char *input_projection_info, const char *proj_name,
double projX, double projY, double height,
double expected_lat, double expected_lon,
const char *datum)
{
double x[1];
double y[1];
double hts[1];
char output_projection_info[255];
sprintf(output_projection_info, "+proj=latlong +datum=WGS84");
projPJ input_projection, output_projection;
input_projection = pj_init_plus(input_projection_info);
output_projection = pj_init_plus(output_projection_info);
y[0] = projX;
x[0] = projY;
hts[0] = height;
pj_transform (input_projection, output_projection, 1, 1, x, y, hts);
if (pj_errno != 0) {
asfPrintWarning("libproj error: %s\n", pj_strerrno(pj_errno));
}
x[0] *= R2D;
y[0] *= R2D;
CU_ASSERT(double_equals(x[0], expected_lat, 6));
CU_ASSERT(double_equals(y[0], expected_lon, 6));
if (double_equals(x[0], expected_lat, 6) &&
double_equals(y[0], expected_lon, 6))
{
//asfPrintStatus("%s %s (fwd): %f, %f ... OK\n",
// proj_name, datum, projX, projY);
++n_ok;
}
else
{
asfPrintStatus("%s (fwd): %s %f, %f ... ERROR\n",
proj_name, datum, projX, projY);
asfPrintStatus("Result: %.10f %.10f (%.10f)\n",
x[0], y[0], hts[0]);
asfPrintStatus("Expected: %.10f %.10f\n",
expected_lat, expected_lon);
++n_bad;
}
// now project the other way
y[0] = expected_lon*D2R;
x[0] = expected_lat*D2R;
hts[0] = height;
pj_transform(output_projection, input_projection, 1, 1, x, y, hts);
if (pj_errno != 0) {
asfPrintWarning("libproj error: %s\n", pj_strerrno(pj_errno));
}
pj_free(input_projection);
pj_free(output_projection);
CU_ASSERT(double_equals(y[0], projX, 5));
CU_ASSERT(double_equals(x[0], projY, 5));
if (double_equals(y[0], projX, 5) &&
double_equals(x[0], projY, 5))
{
//asfPrintStatus("%s %s (rev): %f, %f ... OK\n",
// proj_name, datum, expected_lon, expected_lat);
++n_ok;
}
else
{
asfPrintStatus("%s (rev): %s %f, %f ... ERROR\n",
proj_name, datum, expected_lon, expected_lat);
asfPrintStatus("Result: %.10f %.10f (%.10f)\n",
y[0], x[0], hts[0]);
asfPrintStatus("Expected: %.10f %.10f\n",
projX, projY);
++n_bad;
}
}
static void
test_wrapper(const char * proj_name, project_parameters_t *pps,
project_fn_t *project_fwd,
project_fn_t *project_rev,
project_fn_arr_t *project_fwd_arr,
project_fn_arr_t *project_rev_arr,
double projY, double projX, double height,
double lon, double lat, datum_type_t datum)
{
{
double x, y, z;
project_fwd(pps, lat*D2R, lon*D2R, height, &x, &y, &z, datum);
CU_ASSERT(double_equals(x, projX, 5));
CU_ASSERT(double_equals(y, projY, 5));
if (double_equals(x, projX, 5) && double_equals(y, projY, 5)) {
//asfPrintStatus("%s Wrapper (fwd): %f, %f ... OK\n",
// proj_name, lat, lon);
++n_ok;
} else {
asfPrintStatus("%s Wrapper (fwd): %f, %f ... ERROR\n",
proj_name, lat, lon);
asfPrintStatus("Result: %.10f %.10f (%.10f)\n", x, y, z);
asfPrintStatus("Expected: %.10f %.10f\n", projX, projY);
++n_bad;
}
// test the backwards projection, too
double testLat, testLon;
project_rev(pps, projX, projY, height, &testLat, &testLon, &z, datum);
testLat *= R2D;
testLon *= R2D;
CU_ASSERT(double_equals(lat, testLat, 5));
CU_ASSERT(double_equals(lon, testLon, 5));
if (double_equals(lat, testLat, 5) && double_equals(lon, testLon, 5)) {
//asfPrintStatus("%s Wrapper (rev): %f, %f ... OK\n",
// proj_name, projX, projY);
++n_ok;
} else {
asfPrintStatus("%s Wrapper (rev): %f, %f ... ERROR\n",
proj_name, projX, projY);
asfPrintStatus("Result: %.10f %.10f (%.10f)\n",
testLat, testLon, z);
asfPrintStatus("Expected: %.10f %.10f\n", lat, lon);
++n_bad;
}
}
// also test the array versions
{
double *x, *y, *z;
double lats[1], lons[1], hts[1];
lats[0] = lat*D2R;
lons[0] = lon*D2R;
hts[0] = height;
x = y = z = NULL;
project_fwd_arr(pps, lats, lons, hts, &x, &y, &z, 1, datum);
CU_ASSERT(double_equals(x[0], projX, 5));
CU_ASSERT(double_equals(y[0], projY, 5));
if (double_equals(x[0], projX, 5) && double_equals(y[0], projY, 5)) {
//asfPrintStatus("%s Array (fwd): %f, %f ... OK\n",
// proj_name, lat, lon);
++n_ok;
} else {
asfPrintStatus("%s Array (fwd): %f, %f ... ERROR\n",
proj_name, lat, lon);
asfPrintStatus("Result: %.10f %.10f (%.10f)\n",
x[0], y[0], z[0]);
asfPrintStatus("Expected: %.10f %.10f\n", projX, projY);
++n_bad;
}
free(x);
free(y);
free(z);
}
{
double x[1], y[1], z[1];
double *lats, *lons, *hts;
x[0] = projX;
y[0] = projY;
z[0] = height;
lats = lons = hts = NULL;
project_rev_arr(pps, x, y, z, &lats, &lons, &hts, 1, datum);
lats[0] *= R2D;
lons[0] *= R2D;
CU_ASSERT(double_equals(lats[0], lat, 5));
CU_ASSERT(double_equals(lons[0], lon, 5));
if (double_equals(lats[0], lat, 5) &&
double_equals(lons[0], lon, 5))
{
//asfPrintStatus("%s Array (rev): %f, %f ... OK\n",
// proj_name, projX, projY);
++n_ok;
} else {
asfPrintStatus("%s Array (rev): %f, %f ... ERROR\n",
proj_name, projX, projY);
asfPrintStatus("Result: %.10f %.10f (%.10f)\n",
lats[0], lons[0], hts[0]);
asfPrintStatus("Expected: %.10f %.10f\n", lat, lon);
++n_bad;
}
free(lats);
free(lons);
free(hts);
}
// another array version test, this time caller allocates the memory
// just do the forward transform on this one
{
double x[1], y[1], z[1];
double lats[1], lons[1], hts[1];
lats[0] = lat*D2R;
lons[0] = lon*D2R;
hts[0] = height;
double *px = x; double **xx = &px;
double *py = y; double **yy = &py;
double *pz = z; double **zz = &pz;
project_fwd_arr(pps, lats, lons, hts, xx, yy, zz, 1, datum);
CU_ASSERT(double_equals(x[0], projX, 5));
CU_ASSERT(double_equals(y[0], projY, 5));
if (double_equals(x[0], projX, 5) && double_equals(y[0], projY, 5)) {
//asfPrintStatus("%s Array 2 (fwd): %f, %f ... OK\n",
// proj_name, lat, lon);
++n_ok;
} else {
asfPrintStatus("%s Array 2 (fwd): %f, %f ... ERROR\n",
proj_name, lat, lon);
asfPrintStatus("Result: %.10f %.10f (%.10f)\n",
x[0], y[0], z[0]);
asfPrintStatus("Expected: %.10f %.10f\n", projX, projY);
++n_bad;
}
}
}
void INode::renderTree(Context& ctx) const {
for (size_t i = 0; i < _states.size(); i++) {
// start point
vec3 startpoint = ctx.getCurrentOrigin();
// calculate end point of joint
vec3 endpoint = _states[i]->getEndpoint(ctx);
// draw the link
//glBegin(GL_LINES);
// glColor3d(1.0, 1.0, 1.0);
// glVertex3d(startpoint[0], startpoint[1], startpoint[2]);
// glVertex3d(endpoint[0], endpoint[1], endpoint[2]);
//glEnd();
// calculate orthonormal basis for cylinder on joint
vec3 u, v, n;
_states[i]->getBasis(ctx, u, v, n);
// check if basis is really orthonormal
assert(double_equals(dot(u, v), 0));
assert(double_equals(dot(u, n), 0));
assert(double_equals(dot(v, n), 0));
assert(double_equals(norm(u, 2), 1));
assert(double_equals(norm(v, 2), 1));
assert(double_equals(norm(n, 2), 1));
//cout << "pos:" << endl << pos << endl;
//cout << "u:" << endl << u << endl;
//cout << "v:" << endl << v << endl;
//cout << "n:" << endl << n << endl;
vec3 x = makeVec3(1, 0, 0);
vec3 y = makeVec3(0, 1, 0);
vec3 z = makeVec3(0, 0, 1);
double ux = dot(x, u);
double uy = dot(y, u);
double uz = dot(z, u);
double vx = dot(x, v);
double vy = dot(y, v);
double vz = dot(z, v);
double nx = dot(x, n);
double ny = dot(y, n);
double nz = dot(z, n);
// change of orthonormal basis from uvn -> xyz
GLdouble m[16];
m[0] = ux;
m[1] = uy;
m[2] = uz;
m[3] = 0;
m[4] = vx;
m[5] = vy;
m[6] = vz;
m[7] = 0;
m[8] = nx;
m[9] = ny;
m[10] = nz;
m[11] = 0;
m[12] = 0; m[13] = 0; m[14] = 0; m[15] = 1;
mat44 A;
A << ux << vx << nx << 0 << endr
<< uy << vy << ny << 0 << endr
<< uz << vz << nz << 0 << endr
<< 0 << 0 << 0 << 1 << endr;
//if (!double_equals(det(A), 1))
// cout << "A is: " << endl << A << endl;
//cout << "det(A): " << det(A) << endl;
const double dA = det(A);
if (!double_equals(dA, 1)) {
cerr << "ERROR: det(A) = " << dA << endl;
throw runtime_error("determinant not 1 for rotation matrix");
}
//cout << "--" << endl;
//for (int iii = 0; iii < 16; iii++) {
// cout << m[iii] << endl;
//}
//cout << "--" << endl;
if (isRootNode())
glColor3d(0.0, 0.0, 0.8);
else if (isFixed())
glColor3d(0.0, 1.0, 1.0);
else
glColor3d(0.0, 0.0, 1.0);
glPushMatrix();
glTranslated(startpoint[0], startpoint[1], startpoint[2]);
if (isRootNode())
glutSolidSphere(0.1, 20, 20);
else
glutSolidSphere(0.08, 20, 20);
glPopMatrix();
GLUquadricObj *quadric = gluNewQuadric();
glPushMatrix();
glColor3d(0.0, 1.0, 0.0);
glTranslated(startpoint[0], startpoint[1], startpoint[2]);
glMultMatrixd(m);
gluCylinder(quadric, 0.05, 0.05, _states[i]->getLength(), 32, 32);
glPopMatrix();
gluDeleteQuadric(quadric);
// recurse into child
_states[i]->pushContext(ctx);
_kids[i]->renderTree(ctx);
ctx.popContext();
}
}
int insect(Circle c1,Circle c2)
{
double d,a,b,c,p,q,r;
double cos_value[2], sin_value[2];
if (double_equals(c1.p.x, c2.p.x)&& double_equals(c1.p.y,c2.p.y)&& double_equals(c1.r, c2.r))
{
return 0;
}
d = distance(c1.p, c2.p);
if (d > c1.r + c2.r||d < fabs(c1.r - c2.r))
{
return 0;
}
a = 2.0 * c1.r * (c1.p.x - c2.p.x);
b = 2.0 * c1.r * (c1.p.y - c2.p.y);
c = c2.r * c2.r - c1.r * c1.r- distance_sqr(c1.p,c2.p);
p = a * a + b * b;
q = -2.0 * a * c;
if(p==0)return 0;
if (double_equals(d,c1.r +c2.r)|| double_equals(d, fabs(c1.r - c2.r)))
{
cos_value[0] = -q / p / 2.0;
sin_value[0] = sqrt(1 - cos_value[0] * cos_value[0]);
I0->x = c1.r * cos_value[0] + c1.p.x;
I0->y = c1.r * sin_value[0] + c1.p.y;
if (!double_equals(distance_sqr(*I0, c2.p),c2.r * c2.r))
{
I0->y = c1.p.y - c1.r * sin_value[0];
}
return 1;
}
r = c * c - b * b;
cos_value[0] = (sqrt(q * q - 4.0 * p * r) - q) / p / 2.0;
cos_value[1] = (-sqrt(q * q - 4.0 * p * r) - q) / p / 2.0;
sin_value[0] = sqrt(1 - cos_value[0] * cos_value[0]);
sin_value[1] = sqrt(1 - cos_value[1] * cos_value[1]);
I0->x = c1.r * cos_value[0] + c1.p.x;
I1->x = c1.r * cos_value[1] + c1.p.x;
I0->y = c1.r * sin_value[0] + c1.p.y;
I1->y = c1.r * sin_value[1] + c1.p.y;
if (!double_equals(distance_sqr(*I0, c2.p),c2.r * c2.r))
{
I0->y = c1.p.y - c1.r * sin_value[0];
}
if (!double_equals(distance_sqr(*I1, c2.p),c2.r * c2.r))
{
I1->y = c1.p.y - c1.r * sin_value[1];
}
if (double_equals(I0->y, I1->y)&& double_equals(I0->x, I1->x))
{//改到这里了
if (I0->y > 0)
{
I1->y = -I1->y;
} else
{
I0->y = -I0->y;
}
}
return 2;
}
/**
* Run this Test
*/
void PWMBoardTest::spin()
{
double rate = 5.0d;
ros::Rate loop_rate(rate); //herz;
double pwmSpeed = 0.0d;
//Increment of each speed step
double stepSize = 0.25;
//Scale factor for setting speed at a lower rate
//then the loop
double modStepRate = 0.2;
//For determining how many times the
//loop has "looped"
double cntRate = 0.0d;
//Positive incrementing first
if (maxPWM > 0)
{
//First, climb to max value
bool toMax = true;
//Set initial speed
setSpeed(0, 0);
while (ros::ok())
{
//Set speed to pwm speed
setSpeed(pwmSpeed, pwmSpeed);
ROS_DEBUG("Current PWM/SPEED value %f",pwmSpeed);
//Check for errors
check_EM_STOP();
//Check if it is time for setting new speed
if (double_equals(cntRate, rate))
{
//Check if max is not reached
if (!(double_equals(pwmSpeed, maxPWM)) && (toMax)){
pwmSpeed += stepSize;
printf("Stepping up Speed, new value: %f till %f \n: ", pwmSpeed, maxPWM);
}
//Max is reached
else
{
//Dont go to max value anymore
toMax = false;
//Check if 0 is not reached
if (!double_equals(pwmSpeed, 0.0))
{
setSpeed(pwmSpeed,pwmSpeed);
pwmSpeed -= stepSize;
printf("Stepping down Speed from here: %f %f \n", pwmSpeed, 0.0);
}
//0 is reached, done
else
{
printf("Done \n");
break;
}
}
//Reset countrate
cntRate = 0.0d;
std::cout << "cntrate " << cntRate << std::endl;
}
//Now it not the time for changing speed
else
{
//Increment times loop has looped
cntRate += modStepRate;
std::cout << "cntrate " << cntRate << std::endl;
}
loop_rate.sleep();
}
}
//Negative incrementing(decrementing) first
else
{
//First go to low value
bool toMin = true;
//Set initial speed
setSpeed(0, 0);
ROS_DEBUG("Current PWM/SPEED value %f",pwmSpeed);
while (ros::ok())
{
//Set speed to pwm speed
setSpeed(pwmSpeed, pwmSpeed);
ROS_DEBUG("Current PWM/SPEED value %f",pwmSpeed);
//Check for errors
check_EM_STOP();
//Check if it is time for setting new speed
if (double_equals(cntRate, rate))
{
//Check if min is not reached
if (!(double_equals(pwmSpeed, maxPWM)) && toMin){
pwmSpeed -= stepSize;
printf("Stepping down Speed, new value: %f till %f \n: ", pwmSpeed, maxPWM);
}
//Min is reached
else
{
//Go only up now
toMin = false;
//Check if 0 is not reached
if (!double_equals(pwmSpeed, 0.0))
{
setSpeed(pwmSpeed,pwmSpeed);
pwmSpeed += stepSize;
printf("Stepping up Speed from here: %f %f \n", pwmSpeed, 0.0);
}
//0 is reached, done
else
{
printf("Done \n");
break;
}
}
//Reset countrate
cntRate = 0.0d;
std::cout << "cntrate " << cntRate << std::endl;
}
//Now it not the time for changing speed
else
{
//Increment times loop has looped
cntRate += modStepRate;
std::cout << "cntrate " << cntRate << std::endl;
}
loop_rate.sleep();
}
}
setSpeed(0, 0);
ROS_DEBUG("Current PWM/SPEED value %f",pwmSpeed);
}
void generic_update_actions_state_lazy(double now, double delta, surf_model_t model)
{
surf_action_lmm_t action;
while ((xbt_heap_size(model->model_private->action_heap) > 0)
&& (double_equals(xbt_heap_maxkey(model->model_private->action_heap), now))) {
action = xbt_heap_pop(model->model_private->action_heap);
XBT_DEBUG("Something happened to action %p", action);
#ifdef HAVE_TRACING
if (TRACE_is_enabled()) {
if(model == surf_cpu_model){
surf_resource_t cpu =
lmm_constraint_id(lmm_get_cnst_from_var
(model->model_private->maxmin_system,
action->variable, 0));
TRACE_surf_host_set_utilization(cpu->name,
((surf_action_t)action)->category,
lmm_variable_getvalue(action->variable),
action->last_update,
now - action->last_update);
}
else{
int n = lmm_get_number_of_cnst_from_var(model->model_private->maxmin_system, action->variable);
unsigned int i;
for (i = 0; i < n; i++){
lmm_constraint_t constraint = lmm_get_cnst_from_var(model->model_private->maxmin_system,
action->variable,
i);
link_CM02_t link = lmm_constraint_id(constraint);
TRACE_surf_link_set_utilization(link->lmm_resource.generic_resource.name,
((surf_action_t)action)->category,
(lmm_variable_getvalue(action->variable)*
lmm_get_cnst_weight_from_var(model->model_private->maxmin_system,
action->variable,
i)),
action->last_update,
now - action->last_update);
}
}
}
#endif
if(model == surf_cpu_model){
action->generic_action.finish = surf_get_clock();
update_resource_energy(model, action);
/* set the remains to 0 due to precision problems when updating the remaining amount */
action->generic_action.remains = 0;
surf_action_state_set((surf_action_t) action, SURF_ACTION_DONE);
surf_action_lmm_heap_remove(model->model_private->action_heap,action); //FIXME: strange call since action was already popped
}
else{
// if I am wearing a latency hat
if (action->hat == LATENCY) {
XBT_DEBUG("Latency paid for action %p. Activating", action);
lmm_update_variable_weight(model->model_private->maxmin_system, action->variable,
((surf_action_network_CM02_t)(action))->weight);
surf_action_lmm_heap_remove(model->model_private->action_heap,action);
action->last_update = surf_get_clock();
// if I am wearing a max_duration or normal hat
} else if (action->hat == MAX_DURATION ||
action->hat == NORMAL) {
// no need to communicate anymore
// assume that flows that reached max_duration have remaining of 0
action->generic_action.finish = surf_get_clock();
XBT_DEBUG("Action %p finished", action);
action->generic_action.remains = 0;
((surf_action_t)action)->finish = surf_get_clock();
model->action_state_set((surf_action_t) action,
SURF_ACTION_DONE);
surf_action_lmm_heap_remove(model->model_private->action_heap,action);
if (model->gap_remove && model == surf_network_model)
model->gap_remove(action);
}
}
}
#ifdef HAVE_TRACING
if (TRACE_is_enabled() && model == surf_cpu_model) {
//defining the last timestamp that we can safely dump to trace file
//without losing the event ascending order (considering all CPU's)
double smaller = -1;
xbt_swag_t running_actions = model->states.running_action_set;
xbt_swag_foreach(action, running_actions) {
if (smaller < 0) {
smaller = action->last_update;
continue;
}
if (action->last_update < smaller) {
smaller = action->last_update;
}
}
if (smaller > 0) {
TRACE_last_timestamp_to_dump = smaller;
}
}
