int rp_set_params(rp_app_params_t *p, int len)
{
int i;
int fpga_update = 1;
int params_change = 0;
int awg_params_change = 0;
TRACE("%s()\n", __FUNCTION__);
if(len > PARAMS_NUM) {
fprintf(stderr, "Too many parameters, max=%d\n", PARAMS_NUM);
return -1;
}
pthread_mutex_lock(&rp_main_params_mutex);
for(i = 0; i < len || p[i].name != NULL; i++) {
int p_idx = -1;
int j = 0;
/* Search for correct parameter name in defined parameters */
while(rp_main_params[j].name != NULL) {
int p_strlen = strlen(p[i].name);
if(p_strlen != strlen(rp_main_params[j].name)) {
j++;
continue;
}
if(!strncmp(p[i].name, rp_main_params[j].name, p_strlen)) {
p_idx = j;
break;
}
j++;
}
if(p_idx == -1) {
fprintf(stderr, "Parameter %s not found, ignoring it\n", p[i].name);
continue;
}
if(rp_main_params[p_idx].read_only)
continue;
if(rp_main_params[p_idx].value != p[i].value) {
if(p_idx < PARAMS_AWG_PARAMS)
params_change = 1;
if(p_idx >= PARAMS_AWG_PARAMS)
awg_params_change = 1;
if(rp_main_params[p_idx].fpga_update)
fpga_update = 1;
}
if(rp_main_params[p_idx].min_val > p[i].value) {
fprintf(stderr, "Incorrect parameters value: %f (min:%f), "
" correcting it\n", p[i].value, rp_main_params[p_idx].min_val);
p[i].value = rp_main_params[p_idx].min_val;
} else if(rp_main_params[p_idx].max_val < p[i].value) {
fprintf(stderr, "Incorrect parameters value: %f (max:%f), "
" correcting it\n", p[i].value, rp_main_params[p_idx].max_val);
p[i].value = rp_main_params[p_idx].max_val;
}
rp_main_params[p_idx].value = p[i].value;
}
transform_from_iface_units(&rp_main_params[0]);
pthread_mutex_unlock(&rp_main_params_mutex);
/* Set parameters in HW/FPGA only if they have changed */
if(params_change || (params_init == 0)) {
pthread_mutex_lock(&rp_main_params_mutex);
/* Xmin & Xmax public copy to be served to clients */
rp_main_params[GUI_XMIN].value = p[MIN_GUI_PARAM].value;
rp_main_params[GUI_XMAX].value = p[MAX_GUI_PARAM].value;
transform_acq_params(rp_main_params);
pthread_mutex_unlock(&rp_main_params_mutex);
/* First do health check and then send it to the worker! */
int mode = rp_main_params[TRIG_MODE_PARAM].value;
int time_range = rp_main_params[TIME_RANGE_PARAM].value;
int time_unit = 2;
/* Get info from FPGA module about clocks/decimation, ...*/
int dec_factor = osc_fpga_cnv_time_range_to_dec(time_range);
float smpl_period = c_osc_fpga_smpl_period * dec_factor;
/* t_delay - trigger delay in seconds */
float t_delay = rp_main_params[TRIG_DLY_PARAM].value;
float t_unit_factor = 1; /* to convert to seconds */
/* Our time window with current settings:
* - time_delay is added later, when we check if it is correct
* setting
*/
float t_min = 0;
float t_max = ((OSC_FPGA_SIG_LEN-1) * smpl_period);
params_init = 1;
/* in time units time_unit, needs to be converted */
float t_start = rp_main_params[MIN_GUI_PARAM].value;
float t_stop = rp_main_params[MAX_GUI_PARAM].value;
int t_start_idx;
int t_stop_idx;
int t_step_idx = 0;
/* If auto-set algorithm was requested do not set other parameters */
if(rp_main_params[AUTO_FLAG_PARAM].value == 1) {
auto_in_progress = 1;
forcex_state = 0;
rp_osc_clean_signals();
rp_osc_worker_change_state(rp_osc_auto_set_state);
/* AUTO_FLAG_PARAM is cleared when Auto-set algorithm finishes */
/* Wait for auto-set algorithm to finish or timeout */
int timeout = 10000000; // [us]
const int step = 50000; // [us]
rp_osc_worker_state_t state;
while (timeout > 0) {
rp_osc_worker_get_state(&state);
if (state != rp_osc_auto_set_state) {
break;
}
usleep(step);
timeout -= step;
}
if (timeout <= 0) {
fprintf(stderr, "AUTO: Timeout waiting for AUTO-set algorithm to finish.\n");
}
auto_in_progress = 0;
return 0;
}
/* If AUTO trigger mode, reset trigger delay */
if(mode == 0)
t_delay = 0;
if(dec_factor < 0) {
fprintf(stderr, "Incorrect time range: %d\n", time_range);
return -1;
}
/* Pick time unit and unit factor corresponding to current time range. */
if((time_range == 0) || (time_range == 1)) {
time_unit = 0;
t_unit_factor = 1e6;
} else if((time_range == 2) || (time_range == 3)) {
time_unit = 1;
t_unit_factor = 1e3;
}
rp_main_params[TIME_UNIT_PARAM].value = time_unit;
TRACE("PC: time_(R,U) = (%d, %d)\n", time_range, time_unit);
/* Check if trigger delay in correct range, otherwise correct it
* Correct trigger delay is:
* t_delay >= -t_max
* t_delay <= OSC_FPGA_MAX_TRIG_DELAY
*/
if(t_delay < -t_max) {
t_delay = -t_max;
} else if(t_delay > (OSC_FPGA_TRIG_DLY_MASK * smpl_period)) {
t_delay = OSC_FPGA_TRIG_DLY_MASK * smpl_period;
} else {
t_delay = round(t_delay / smpl_period) * smpl_period;
}
t_min = t_min + t_delay;
t_max = t_max + t_delay;
rp_main_params[TRIG_DLY_PARAM].value = t_delay;
/* Convert to seconds */
t_start = t_start / t_unit_factor;
t_stop = t_stop / t_unit_factor;
TRACE("PC: t_stop = %.9f\n", t_stop);
/* Select correct time window with this settings:
* time window is defined from:
* ([ 0 - 16k ] * smpl_period) + trig_delay */
/* round to correct/possible values - convert to nearest index
* and back
*/
t_start_idx = round(t_start / smpl_period);
t_stop_idx = round(t_stop / smpl_period);
t_start = (t_start_idx * smpl_period);
t_stop = (t_stop_idx * smpl_period);
if(t_start < t_min)
t_start = t_min;
if(t_stop > t_max)
t_stop = t_max;
if(t_stop <= t_start )
t_stop = t_max;
/* Correct the window according to possible decimations - always
* provide at least the data demanded by the user (ceil() instead
* of round())
*/
t_start_idx = round(t_start / smpl_period);
t_stop_idx = round(t_stop / smpl_period);
if((((t_stop_idx-t_start_idx)/(float)(SIGNAL_LENGTH-1))) >= 1) {
t_step_idx = ceil((t_stop_idx-t_start_idx)/(float)(SIGNAL_LENGTH-1));
int max_step = OSC_FPGA_SIG_LEN/SIGNAL_LENGTH;
if(t_step_idx > max_step)
t_step_idx = max_step;
t_stop = t_start + SIGNAL_LENGTH * t_step_idx * smpl_period;
}
TRACE("PC: t_stop (rounded) = %.9f\n", t_stop);
/* write back and convert to set units */
rp_main_params[MIN_GUI_PARAM].value = t_start;
rp_main_params[MAX_GUI_PARAM].value = t_stop;
rp_osc_worker_update_params((rp_app_params_t *)&rp_main_params[0],
fpga_update);
/* check if we need to change state */
switch(mode) {
case 0:
/* auto */
rp_osc_worker_change_state(rp_osc_auto_state);
break;
case 1:
/* normal */
rp_osc_worker_change_state(rp_osc_normal_state);
break;
case 2:
/* single - clear last ok buffer */
rp_osc_worker_change_state(rp_osc_idle_state);
rp_osc_clean_signals();
break;
default:
return -1;
}
if(rp_main_params[SINGLE_BUT_PARAM].value == 1) {
rp_main_params[SINGLE_BUT_PARAM].value = 0;
rp_osc_clean_signals();
rp_osc_worker_change_state(rp_osc_single_state);
}
}
if(awg_params_change) {
/* Correct frequencies if needed */
rp_main_params[GEN_SIG_FREQ_CH1].value =
rp_gen_limit_freq(rp_main_params[GEN_SIG_FREQ_CH1].value,
rp_main_params[GEN_SIG_TYPE_CH1].value);
rp_main_params[GEN_SIG_FREQ_CH2].value =
rp_gen_limit_freq(rp_main_params[GEN_SIG_FREQ_CH2].value,
rp_main_params[GEN_SIG_TYPE_CH2].value);
if(generate_update(&rp_main_params[0]) < 0) {
return -1;
}
}
return 0;
}
string search_similar (void * webserver_request)
{
Webserver_Request * request = (Webserver_Request *) webserver_request;
Database_Volatile database_volatile = Database_Volatile ();
int myIdentifier = filter_string_user_identifier (request);
string bible = request->database_config_user()->getBible ();
if (request->query.count ("b")) {
bible = request->query ["b"];
}
if (request->query.count ("load")) {
int book = Ipc_Focus::getBook (request);
int chapter = Ipc_Focus::getChapter (request);
int verse = Ipc_Focus::getVerse (request);
// Text of the focused verse in the active Bible.
// Remove all punctuation from it.
string versetext = search_logic_get_bible_verse_text (bible, book, chapter, verse);
vector <string> punctuation = filter_string_explode (Database_Config_Bible::getSentenceStructureEndPunctuation (bible), ' ');
for (auto & sign : punctuation) {
versetext = filter_string_str_replace (sign, "", versetext);
}
punctuation = filter_string_explode (Database_Config_Bible::getSentenceStructureMiddlePunctuation (bible), ' ');
for (auto & sign : punctuation) {
versetext = filter_string_str_replace (sign, "", versetext);
}
versetext = filter_string_trim (versetext);
database_volatile.setValue (myIdentifier, "searchsimilar", versetext);
return versetext;
}
if (request->query.count ("words")) {
string words = request->query ["words"];
words = filter_string_trim (words);
database_volatile.setValue (myIdentifier, "searchsimilar", words);
vector <string> vwords = filter_string_explode (words, ' ');
// Include items if there are no more search hits than 30% of the total number of verses in the Bible.
size_t maxcount = round (0.3 * search_logic_get_verse_count (bible));
// Store how often a verse occurs in an array.
// The keys are the identifiers of the search results.
// The values are how often the identifiers occur in the entire focused verse.
map <int, int> identifiers;
for (auto & word : vwords) {
// Find out how often this word occurs in the Bible. Skip if too often.
vector <Passage> passages = search_logic_search_bible_text (bible, word);
if (passages.size () > maxcount) continue;
// Store the identifiers and their count.
for (auto & passage : passages) {
int id = filter_passage_to_integer (passage);
if (identifiers.count (id)) identifiers [id]++;
else identifiers [id] = 1;
}
}
// Sort on occurrence from high to low.
// Skip identifiers that only occur once.
vector <int> ids;
vector <int> counts;
for (auto & element : identifiers) {
int id = element.first;
int count = element.second;
if (count <= 1) continue;
ids.push_back (id);
counts.push_back (count);
}
quick_sort (counts, ids, 0, counts.size());
reverse (ids.begin(), ids.end());
// Output the passage identifiers to the browser.
string output;
for (auto & id : ids) {
if (!output.empty ()) output.append ("\n");
output.append (convert_to_string (id));
}
return output;
}
if (request->query.count ("id")) {
int id = convert_to_int (request->query ["id"]);
// Get the Bible and passage for this identifier.
Passage passage = filter_integer_to_passage (id);
string bible = request->database_config_user()->getBible ();
// string bible = passage.bible;
int book = passage.book;
int chapter = passage.chapter;
string verse = passage.verse;
// Get the plain text.
string text = search_logic_get_bible_verse_text (bible, book, chapter, convert_to_int (verse));
// Get search words.
vector <string> words = filter_string_explode (database_volatile.getValue (myIdentifier, "searchsimilar"), ' ');
// Format it.
string link = filter_passage_link_for_opening_editor_at (book, chapter, verse);
text = filter_string_markup_words (words, text);
string output = "<div>" + link + " " + text + "</div>";
// Output to browser.
return output;
}
string page;
Assets_Header header = Assets_Header (translate("Search"), request);
header.setNavigator ();
header.addBreadCrumb (menu_logic_search_menu (), menu_logic_search_text ());
page = header.run ();
Assets_View view;
view.set_variable ("bible", bible);
string script = "var searchBible = \"" + bible + "\";";
view.set_variable ("script", script);
page += view.render ("search", "similar");
page += Assets_Page::footer ();
return page;
}
ossimRefPtr<ossimImageData> ossimTileToIplFilter::getTile(const ossimIrect& tileRect,
ossim_uint32 resLevel)
{
cout << "Getting Tile !" << endl;
// Check input data sources for valid and null tiles
ossimImageSource *imageSource = PTR_CAST(ossimImageSource, getInput(0));
ossimRefPtr<ossimImageData> imageSourceData;
if (imageSource)
imageSourceData = imageSource->getTile(tileRect, resLevel);
if (!isSourceEnabled())
return imageSourceData;
if (!theTile.valid())
{
if(getInput(0))
{
theTile = ossimImageDataFactory::instance()->create(this, this);
theTile->initialize();
}
}
if (!imageSourceData.valid() || !theTile.valid())
return ossimRefPtr<ossimImageData>();
theTile->setOrigin(tileRect.ul());
if (theTile->getImageRectangle() != tileRect)
{
theTile->setImageRectangle(tileRect);
theTile->initialize();
}
IplImage *input = cvCreateImage(cvSize(tileRect.width(), tileRect.height()),IPL_DEPTH_8U,3);
IplImage *output = cvCreateImage(cvSize(tileRect.width(),tileRect.height()),IPL_DEPTH_8U,3);
cvZero(input);
cvZero(output);
// If 16 or 32 bits, downsample to 8 bits
ossimScalarType inputType = imageSourceData->getScalarType();
if(inputType == OSSIM_UINT16 || inputType == OSSIM_USHORT11)
CopyTileToIplImage(static_cast<ossim_uint16>(0), imageSourceData, input, tileRect);
else
CopyTileToIplImage(static_cast<ossim_uint8>(0), imageSourceData, input, tileRect);
cvCopy(input, output);
int bins = 256;
int hsize[] = {bins};
float binVal;
float sum=0;
int firstIndexFlag = 1;
/*// Create histogram of image
CvHistogram *hist;
hist = cvCreateHist(1, hsize, CV_HIST_ARRAY, 0, 1);
cvCalcHist(&input, hist, 0, 0);
cvNormalizeHist(hist, 100);
binVal = cvQueryHistValue_1D(hist,1);
*/
// Determine the actual height and width of each tile
ossimIrect fullImageRect;
fullImageRect = imageSource->getBoundingRect(0);
ossim_int32 tileHeight, tileWidth, imageWidth, imageHeight;
tileHeight = tileRect.height();
tileWidth = tileRect.width();
imageWidth = fullImageRect.width();
imageHeight = fullImageRect.height();
ossim_int32 totRows, totCols;
totRows = (ossim_uint32)round(imageHeight / tileHeight);
totCols = (ossim_uint32)round(imageWidth / tileWidth);
ossimIpt upperLeftTile = tileRect.ul();
if ((upperLeftTile.x + 1) > fullImageRect.ul().x + totCols * tileWidth)
tileWidth = imageWidth - totCols * tileWidth;
if ((upperLeftTile.y + 1) > fullImageRect.ul().y + totRows * tileHeight)
tileHeight = imageHeight - totRows * tileHeight;
//Begin Ship Detect Algorithim
// Create sub-image to ignore zeros created by OSSIM
// ie, the tile is 512x512 but on the edges, the information is only in 512x10
CvRect subRect = cvRect(0, 0, tileWidth, tileHeight);
IplImage *subImg = cvCreateImage(cvSize(tileWidth, tileHeight),IPL_DEPTH_8U,3);
cvSetImageROI(input, subRect);
cvCopy(input, subImg);
cvResetImageROI(input);
showImage(subImg,input);
cvReleaseImage(&input);
cvReleaseImage(&output);
return theTile;
}
void az_draw_particle(const az_particle_t *particle, az_clock_t clock) {
assert(particle->kind != AZ_PAR_NOTHING);
assert(particle->age <= particle->lifetime);
switch (particle->kind) {
case AZ_PAR_NOTHING: AZ_ASSERT_UNREACHABLE();
case AZ_PAR_BOOM:
glBegin(GL_TRIANGLE_FAN); {
with_color_alpha(particle->color, 0);
glVertex2d(0, 0);
const double ratio = particle->age / particle->lifetime;
with_color_alpha(particle->color, 1 - ratio * ratio);
const double radius = particle->param1 * ratio;
for (int i = 0; i <= 16; ++i) {
glVertex2d(radius * cos(i * AZ_PI_EIGHTHS),
radius * sin(i * AZ_PI_EIGHTHS));
}
} glEnd();
break;
case AZ_PAR_BEAM: {
const double alpha = (particle->lifetime <= 0.0 ? 1.0 :
1.0 - particle->age / particle->lifetime);
glBegin(GL_QUAD_STRIP); {
with_color_alpha(particle->color, 0);
glVertex2d(0, particle->param2);
glVertex2d(particle->param1, particle->param2);
with_color_alpha(particle->color, alpha);
glVertex2d(0, 0);
glVertex2d(particle->param1, 0);
with_color_alpha(particle->color, 0);
glVertex2d(0, -particle->param2);
glVertex2d(particle->param1, -particle->param2);
} glEnd();
glBegin(GL_TRIANGLE_FAN); {
with_color_alpha(particle->color, alpha);
glVertex2d(particle->param1, 0);
with_color_alpha(particle->color, 0);
for (int i = -90; i <= 90; i += 30) {
glVertex2d(particle->param1 +
particle->param2 * cos(AZ_DEG2RAD(i)) * 0.75,
particle->param2 * sin(AZ_DEG2RAD(i)));
}
} glEnd();
} break;
case AZ_PAR_CHARGED_BOOM: {
const double factor = particle->age / particle->lifetime;
const double major = sqrt(factor) * particle->param1;
const double minor = (1 - factor) * particle->param2;
const double alpha = 1 - factor;
glBegin(GL_QUAD_STRIP); {
const double outer = major + minor;
for (int i = 0; i <= 360; i += 20) {
with_color_alpha(particle->color, 0);
glVertex2d(outer * cos(AZ_DEG2RAD(i)), outer * sin(AZ_DEG2RAD(i)));
with_color_alpha(particle->color, alpha);
glVertex2d(major * cos(AZ_DEG2RAD(i)), major * sin(AZ_DEG2RAD(i)));
}
} glEnd();
glBegin(GL_QUAD_STRIP); {
const double inner = fmax(0, major - minor);
const double beta = alpha * (1 - fmin(major, minor) / minor);
for (int i = 0; i <= 360; i += 20) {
with_color_alpha(particle->color, alpha);
glVertex2d(major * cos(AZ_DEG2RAD(i)), major * sin(AZ_DEG2RAD(i)));
with_color_alpha(particle->color, beta);
glVertex2d(inner * cos(AZ_DEG2RAD(i)), inner * sin(AZ_DEG2RAD(i)));
}
} glEnd();
} break;
case AZ_PAR_EMBER:
glBegin(GL_TRIANGLE_FAN); {
with_color_alpha(particle->color, 1);
glVertex2f(0, 0);
with_color_alpha(particle->color, 0);
const double radius =
particle->param1 * (1.0 - particle->age / particle->lifetime);
for (int i = 0; i <= 360; i += 30) {
glVertex2d(radius * cos(AZ_DEG2RAD(i)), radius * sin(AZ_DEG2RAD(i)));
}
} glEnd();
break;
case AZ_PAR_EXPLOSION:
glBegin(GL_QUAD_STRIP); {
const double tt = 1.0 - particle->age / particle->lifetime;
const double inner_alpha = tt * tt;
const double outer_alpha = tt;
const double inner_radius = particle->param1 * (1.0 - tt * tt * tt);
const double outer_radius = particle->param1;
for (int i = 0; i <= 360; i += 6) {
const double c = cos(AZ_DEG2RAD(i)), s = sin(AZ_DEG2RAD(i));
with_color_alpha(particle->color, inner_alpha);
glVertex2d(inner_radius * c, inner_radius * s);
with_color_alpha(particle->color, outer_alpha);
glVertex2d(outer_radius * c, outer_radius * s);
}
} glEnd();
break;
case AZ_PAR_FIRE_BOOM: {
const int i_step = 10;
const double liveness = 1.0 - particle->age / particle->lifetime;
const double x_radius = particle->param1;
const double y_radius = particle->param1 * liveness;
for (int i = 0; i < 180; i += i_step) {
glBegin(GL_TRIANGLE_STRIP); {
const int limit = 180 * liveness;
for (int j = 0; j < limit; j += 20) {
glColor4f(1.0, 0.75 * j / limit, 0.0,
0.35 + 0.25 * sin(AZ_DEG2RAD(i)) * sin(AZ_DEG2RAD(j)) -
0.35 * j / limit);
const double x = x_radius * cos(AZ_DEG2RAD(j));
glVertex2d(x, y_radius * cos(AZ_DEG2RAD(i)) *
sin(AZ_DEG2RAD(j)));
glVertex2d(x, y_radius * cos(AZ_DEG2RAD(i + i_step)) *
sin(AZ_DEG2RAD(j)));
}
glVertex2d(x_radius * cos(AZ_DEG2RAD(limit)),
y_radius * cos(AZ_DEG2RAD(i + i_step/2)) *
sin(AZ_DEG2RAD(limit)));
} glEnd();
}
} break;
case AZ_PAR_ICE_BOOM: {
const double t0 = particle->age / particle->lifetime;
const double t1 = 1.0 - t0;
glBegin(GL_TRIANGLE_FAN); {
with_color_alpha(particle->color, 0);
glVertex2f(0, 0);
with_color_alpha(particle->color, t1 * t1 * t1);
for (int i = 0; i <= 360; i += 6) {
glVertex2d(particle->param1 * cos(AZ_DEG2RAD(i)),
particle->param1 * sin(AZ_DEG2RAD(i)));
}
} glEnd();
glPushMatrix(); {
const double rx = 0.65 * particle->param1;
const double ry = sqrt(3.0) * rx / 3.0;
const double cx = fmin(1, 4 * t0) * rx;
for (int i = 0; i < 6; ++i) {
glBegin(GL_TRIANGLE_FAN); {
with_color_alpha(particle->color, t1);
glVertex2d(cx, 0);
with_color_alpha(particle->color, t1 * t1);
glVertex2d(cx + rx, 0); glVertex2d(cx, ry);
glVertex2d(cx - rx, 0); glVertex2d(cx, -ry);
glVertex2d(cx + rx, 0);
} glEnd();
glRotatef(60, 0, 0, 1);
}
} glPopMatrix();
} break;
case AZ_PAR_LIGHTNING_BOLT:
if (particle->age >= particle->param2) {
const int num_steps = az_imax(2, round(particle->param1 / 10.0));
const double step = particle->param1 / num_steps;
az_random_seed_t seed = { clock / 5, 194821.0 * particle->angle };
az_vector_t prev = {0, 0};
for (int i = 1; i <= num_steps; ++i) {
const az_vector_t next =
(i == num_steps ? (az_vector_t){particle->param1, 0} :
(az_vector_t){3.0 * az_rand_sdouble(&seed) + i * step,
10.0 * az_rand_sdouble(&seed)});
const az_vector_t side =
az_vwithlen(az_vrot90ccw(az_vsub(next, prev)), 4);
glBegin(GL_TRIANGLE_STRIP); {
with_color_alpha(particle->color, 0);
az_gl_vertex(az_vadd(prev, side));
az_gl_vertex(az_vadd(next, side));
glColor4f(1, 1, 1, 0.5);
az_gl_vertex(prev); az_gl_vertex(next);
with_color_alpha(particle->color, 0);
az_gl_vertex(az_vsub(prev, side));
az_gl_vertex(az_vsub(next, side));
} glEnd();
prev = next;
}
draw_bolt_glowball(particle->color, particle->param1, clock);
}
draw_bolt_glowball(particle->color, 0, clock);
break;
case AZ_PAR_OTH_FRAGMENT:
glRotated(particle->age * AZ_RAD2DEG(particle->param2), 0, 0, 1);
glBegin(GL_TRIANGLES); {
const double radius =
(particle->param1 >= 0.0 ?
particle->param1 * (1.0 - particle->age / particle->lifetime) :
-particle->param1 * (particle->age / particle->lifetime));
const az_color_t color = particle->color;
for (int i = 0; i < 3; ++i) {
const az_clock_t clk = clock + 2 * i;
glColor4ub((az_clock_mod(6, 1, clk) < 3 ? color.r : color.r / 4),
(az_clock_mod(6, 1, clk + 2) < 3 ? color.g : color.g / 4),
(az_clock_mod(6, 1, clk + 4) < 3 ? color.b : color.b / 4),
color.a);
glVertex2d(radius * cos(AZ_DEG2RAD(i * 120)),
radius * sin(AZ_DEG2RAD(i * 120)));
}
} glEnd();
break;
case AZ_PAR_NPS_PORTAL: {
const double progress = particle->age / particle->lifetime;
const double scale = 1 - 2 * fabs(progress - 0.5);
const double tscale = fmax(0, 1 - pow(2.25 * progress - 1.25, 2));
// Tendrils:
for (int i = 0; i < 10; ++i) {
const double theta = AZ_DEG2RAD(i * 36);
const az_vector_t tip =
az_vadd(az_vpolar(1.1 * particle->param1 * tscale, theta),
az_vpolar(15 * tscale,
particle->age * AZ_DEG2RAD(180) * ((i % 3) + 1)));
const az_vector_t ctrl1 =
az_vadd(az_vpolar(0.8 * particle->param1 * tscale, theta),
az_vpolar(10 * sin(particle->age * AZ_DEG2RAD(400)),
theta + AZ_HALF_PI));
const az_vector_t ctrl2 =
az_vadd(az_vpolar(0.4 * particle->param1 * tscale, theta),
az_vpolar(10 * cos(particle->age * AZ_DEG2RAD(400)),
theta + AZ_HALF_PI));
az_draw_oth_tendril(AZ_VZERO, ctrl1, ctrl2, tip, 5 * tscale,
1.0f, clock);
}
// Portal:
glBegin(GL_TRIANGLE_FAN); {
const GLfloat r = (az_clock_mod(6, 1, clock) < 3 ? 1.0f : 0.25f);
const GLfloat g = (az_clock_mod(6, 1, clock + 2) < 3 ? 1.0f : 0.25f);
const GLfloat b = (az_clock_mod(6, 1, clock + 4) < 3 ? 1.0f : 0.75f);
glColor4f(r, g, b, 1.0f);
glVertex2f(0, 0);
glColor4f(r, g, b, 0.15f);
const double radius = particle->param1 * scale;
for (int i = 0; i <= 360; i += 10) {
glVertex2d(radius * cos(AZ_DEG2RAD(i)), radius * sin(AZ_DEG2RAD(i)));
}
} glEnd();
} break;
case AZ_PAR_ROCK:
glScaled(particle->param1, particle->param1, 1);
glRotated(particle->age * AZ_RAD2DEG(particle->param2), 0, 0, 1);
glBegin(GL_TRIANGLE_FAN); {
const double progress = particle->age / particle->lifetime;
const az_color_t black = {0, 0, 0, 255};
az_gl_color(az_transition_color(particle->color, black, progress));
glVertex2f(0, 0);
az_gl_color(az_transition_color(particle->color, black,
0.7 + 0.3 * progress));
glVertex2f(4, 0); glVertex2f(1, 4); glVertex2f(-1, 5);
glVertex2f(-2, 0); glVertex2f(-1, -2); glVertex2f(1, -3);
glVertex2f(4, 0);
} glEnd();
break;
case AZ_PAR_SHARD:
glScaled(particle->param1, particle->param1, 1);
glRotated(particle->age * AZ_RAD2DEG(particle->param2), 0, 0, 1);
glBegin(GL_TRIANGLES); {
az_color_t color = particle->color;
const double alpha = 1.0 - particle->age / particle->lifetime;
with_color_alpha(color, alpha);
glVertex2f(2, 3);
color.r *= 0.6; color.g *= 0.6; color.b *= 0.6;
with_color_alpha(color, alpha);
glVertex2f(-2, 4);
color.r *= 0.6; color.g *= 0.6; color.b *= 0.6;
with_color_alpha(color, alpha);
glVertex2f(0, -4);
} glEnd();
break;
case AZ_PAR_SPARK:
glBegin(GL_TRIANGLE_FAN); {
glColor4f(1, 1, 1, 0.8);
glVertex2f(0, 0);
with_color_alpha(particle->color, 0);
const double radius =
particle->param1 * (1.0 - particle->age / particle->lifetime);
const double theta_offset = particle->param2 * particle->age;
for (int i = 0; i <= 360; i += 45) {
const double rho = (i % 2 ? 1.0 : 0.5) * radius;
const double theta = AZ_DEG2RAD(i) + theta_offset;
glVertex2d(rho * cos(theta), rho * sin(theta));
}
} glEnd();
break;
case AZ_PAR_SPLOOSH:
glBegin(GL_TRIANGLE_FAN); {
with_color_alpha(particle->color, 1);
glVertex2f(0, 0);
with_color_alpha(particle->color, 0);
const GLfloat height =
particle->param1 * sin(AZ_PI * particle->age / particle->lifetime);
const GLfloat semiwidth = particle->param2;
glVertex2f(0, semiwidth);
glVertex2f(0.3f * height, 0.5f * semiwidth);
glVertex2f(height, 0);
glVertex2f(0.3f * height, -0.5f * semiwidth);
glVertex2f(0, -semiwidth);
glVertex2f(-0.05f * height, 0);
glVertex2f(0, semiwidth);
} glEnd();
break;
case AZ_PAR_TRAIL: {
const double scale = 1.0 - particle->age / particle->lifetime;
const double alpha = scale * scale * scale;
const double semiwidth = alpha * particle->param2;
glBegin(GL_TRIANGLE_STRIP); {
with_color_alpha(particle->color, 0);
glVertex2d(0, semiwidth);
glVertex2d(particle->param1, semiwidth);
with_color_alpha(particle->color, alpha);
glVertex2d(0, 0);
glVertex2d(particle->param1, 0);
with_color_alpha(particle->color, 0);
glVertex2d(0, -semiwidth);
glVertex2d(particle->param1, -semiwidth);
} glEnd();
} break;
}
}
void gravity_fft_grid2p(struct particle* p){
// I'm sorry to say I have to keep these traps. Something's wrong if these traps are called.
int x = (int) floor((p->x / boxsize_x + 0.5) * root_nx);
int y = (int) floor((p->y / boxsize_y + 0.5) * root_ny);
// Formally, pos.x is in the interval [-size/2 , size/2 [. Therefore, x and y should be in [0 , grid_NJ-1]
// xp1, xm1... might be out of bound. They are however the relevant coordinates for the interpolation.
int xp1 = x + 1;
int xm1 = x - 1;
int ym1 = y - 1;
int yp1 = y + 1;
// Target according to boundary conditions.
// Although xTarget and yTarget are not relevant here, they will be relevant with shear
// We have to use all these fancy variables since y depends on x because of the shearing path
// Any nicer solution is welcome
int xTarget = x;
int xp1Target = xp1;
int xm1Target = xm1;
int ym1_xm1Target = (ym1 + root_ny) % root_ny;
int ym1_xTarget = ym1_xm1Target;
int ym1_xp1Target = ym1_xm1Target;
int y_xm1Target = y % root_ny;
int y_xTarget = y_xm1Target;
int y_xp1Target = y_xm1Target;
int yp1_xm1Target = yp1 % root_ny;
int yp1_xTarget = yp1_xm1Target;
int yp1_xp1Target = yp1_xm1Target;
double tx, ty;
// Shearing patch trick
// This is only an **approximate** mapping
// one should use an exact interpolation scheme here (Fourier like).
if(xp1Target>=root_nx) {
xp1Target -= root_nx; // X periodicity
y_xp1Target = y_xp1Target + round((shift_shear/boxsize_y) * root_ny);
y_xp1Target = (y_xp1Target + root_ny) % root_ny; // Y periodicity
yp1_xp1Target = yp1_xp1Target + round((shift_shear/boxsize_y) * root_ny);
yp1_xp1Target = (yp1_xp1Target + root_ny) % root_ny;
ym1_xp1Target = ym1_xp1Target + round((shift_shear/boxsize_y) * root_ny);
ym1_xp1Target = (ym1_xp1Target + root_ny) % root_ny;
}
if(xm1Target<0) {
xm1Target += root_nx;
y_xm1Target = y_xm1Target - round((shift_shear/boxsize_y) * root_ny);
y_xm1Target = (y_xm1Target + root_ny) % root_ny; // Y periodicity
yp1_xm1Target = yp1_xm1Target - round((shift_shear/boxsize_y) * root_ny);
yp1_xm1Target = (yp1_xm1Target + root_ny) % root_ny;
ym1_xm1Target = ym1_xm1Target - round((shift_shear/boxsize_y) * root_ny);
ym1_xm1Target = (ym1_xm1Target + root_ny) % root_ny;
}
tx = ((double)xm1 +0.5) * boxsize_x / root_nx -0.5*boxsize_x - p->x;
ty = ((double)ym1 +0.5) * boxsize_y / root_ny -0.5*boxsize_y - p->y;
p->ax += fx[(root_ny+2) * xm1Target + ym1_xm1Target] * W(-tx/dx)*W(-ty/dy);
p->ay += fy[(root_ny+2) * xm1Target + ym1_xm1Target] * W(-tx/dx)*W(-ty/dy);
tx = ((double)x +0.5) * boxsize_x / root_nx -0.5*boxsize_x - p->x;
ty = ((double)ym1 +0.5) * boxsize_y / root_ny -0.5*boxsize_y - p->y;
p->ax += fx[(root_ny+2) * xTarget + ym1_xTarget] * W(-tx/dx)*W(-ty/dy);
p->ay += fy[(root_ny+2) * xTarget + ym1_xTarget] * W(-tx/dx)*W(-ty/dy);
tx = ((double)xp1 +0.5) * boxsize_x / root_nx -0.5*boxsize_x - p->x;
ty = ((double)ym1 +0.5) * boxsize_y / root_ny -0.5*boxsize_y - p->y;
p->ax += fx[(root_ny+2) * xp1Target + ym1_xp1Target] * W(-tx/dx)*W(-ty/dy);
p->ay += fy[(root_ny+2) * xp1Target + ym1_xp1Target] * W(-tx/dx)*W(-ty/dy);
tx = ((double)xm1 +0.5) * boxsize_x / root_nx -0.5*boxsize_x - p->x;
ty = ((double)y +0.5) * boxsize_y / root_ny -0.5*boxsize_y - p->y;
p->ax += fx[(root_ny+2) * xm1Target + y_xm1Target ] * W(-tx/dx)*W(-ty/dy);
p->ay += fy[(root_ny+2) * xm1Target + y_xm1Target ] * W(-tx/dx)*W(-ty/dy);
tx = ((double)x +0.5) * boxsize_x / root_nx -0.5*boxsize_x - p->x;
ty = ((double)y +0.5) * boxsize_y / root_ny -0.5*boxsize_y - p->y;
p->ax += fx[(root_ny+2) * xTarget + y_xTarget ] * W(-tx/dx)*W(-ty/dy);
p->ay += fy[(root_ny+2) * xTarget + y_xTarget ] * W(-tx/dx)*W(-ty/dy);
tx = ((double)xp1 +0.5) * boxsize_x / root_nx -0.5*boxsize_x - p->x;
ty = ((double)y +0.5) * boxsize_y / root_ny -0.5*boxsize_y - p->y;
p->ax += fx[(root_ny+2) * xp1Target + y_xp1Target ] * W(-tx/dx)*W(-ty/dy);
p->ay += fy[(root_ny+2) * xp1Target + y_xp1Target ] * W(-tx/dx)*W(-ty/dy);
tx = ((double)xm1 +0.5) * boxsize_x / root_nx -0.5*boxsize_x - p->x;
ty = ((double)yp1 +0.5) * boxsize_y / root_ny -0.5*boxsize_y - p->y;
p->ax += fx[(root_ny+2) * xm1Target + yp1_xm1Target] * W(-tx/dx)*W(-ty/dy);
p->ay += fy[(root_ny+2) * xm1Target + yp1_xm1Target] * W(-tx/dx)*W(-ty/dy);
tx = ((double)x +0.5) * boxsize_x / root_nx -0.5*boxsize_x - p->x;
ty = ((double)yp1 +0.5) * boxsize_y / root_ny -0.5*boxsize_y - p->y;
p->ax += fx[(root_ny+2) * xTarget + yp1_xTarget] * W(-tx/dx)*W(-ty/dy);
p->ay += fy[(root_ny+2) * xTarget + yp1_xTarget] * W(-tx/dx)*W(-ty/dy);
tx = ((double)xp1 +0.5) * boxsize_x / root_nx -0.5*boxsize_x - p->x;
ty = ((double)yp1 +0.5) * boxsize_y / root_ny -0.5*boxsize_y - p->y;
p->ax += fx[(root_ny+2) * xp1Target + yp1_xp1Target] * W(-tx/dx)*W(-ty/dy);
p->ay += fy[(root_ny+2) * xp1Target + yp1_xp1Target] * W(-tx/dx)*W(-ty/dy);
}
int main(int argc, char **argv)
{
struct jpeg_decompress_struct dinfo;
struct jpeg_compress_struct cinfo;
struct my_error_mgr jcerr,jderr;
JSAMPARRAY buf = NULL;
jvirt_barray_ptr *coef_arrays = NULL;
char marker_str[256];
char tmpfilename[MAXPATHLEN],tmpdir[MAXPATHLEN];
char newname[MAXPATHLEN], dest_path[MAXPATHLEN];
volatile int i;
int c,j, tmpfd, searchcount, searchdone;
int opt_index = 0;
long insize = 0, outsize = 0, lastsize = 0;
int oldquality;
double ratio;
struct stat file_stat;
jpeg_saved_marker_ptr cmarker;
unsigned char *outbuffer = NULL;
size_t outbuffersize;
char *outfname = NULL;
FILE *infile = NULL, *outfile = NULL;
int marker_in_count, marker_in_size;
int compress_err_count = 0;
int decompress_err_count = 0;
long average_count = 0;
double average_rate = 0.0, total_save = 0.0;
if (rcsid)
; /* so compiler won't complain about "unused" rcsid string */
umask(077);
signal(SIGINT,own_signal_handler);
signal(SIGTERM,own_signal_handler);
/* initialize decompression object */
dinfo.err = jpeg_std_error(&jderr.pub);
jpeg_create_decompress(&dinfo);
jderr.pub.error_exit=my_error_exit;
jderr.pub.output_message=my_output_message;
jderr.jump_set = 0;
/* initialize compression object */
cinfo.err = jpeg_std_error(&jcerr.pub);
jpeg_create_compress(&cinfo);
jcerr.pub.error_exit=my_error_exit;
jcerr.pub.output_message=my_output_message;
jcerr.jump_set = 0;
if (argc<2) {
if (!quiet_mode) fprintf(stderr,PROGRAMNAME ": file arguments missing\n"
"Try '" PROGRAMNAME " --help' for more information.\n");
exit(1);
}
/* parse command line parameters */
while(1) {
opt_index=0;
if ((c=getopt_long(argc,argv,"d:hm:nstqvfVpPoT:S:b",long_options,&opt_index))
== -1)
break;
switch (c) {
case 'm':
{
int tmpvar;
if (sscanf(optarg,"%d",&tmpvar) == 1) {
quality=tmpvar;
if (quality < 0) quality=0;
if (quality > 100) quality=100;
}
else
fatal("invalid argument for -m, --max");
}
break;
case 'd':
if (realpath(optarg,dest_path)==NULL || !is_directory(dest_path)) {
fatal("invalid argument for option -d, --dest");
}
strncat(dest_path,DIR_SEPARATOR_S,sizeof(dest_path)-strlen(dest_path)-1);
if (verbose_mode)
fprintf(stderr,"Destination directory: %s\n",dest_path);
dest=1;
break;
case 'v':
verbose_mode++;
break;
case 'h':
print_usage();
exit(0);
break;
case 'q':
quiet_mode=1;
break;
case 't':
totals_mode=1;
break;
case 'n':
noaction=1;
break;
case 'f':
force=1;
break;
case 'b':
csv=1;
quiet_mode=1;
break;
case '?':
break;
case 'V':
print_version();
exit(0);
break;
case 'o':
overwrite_mode=1;
break;
case 'p':
preserve_mode=1;
break;
case 'P':
preserve_perms=1;
break;
case 's':
save_exif=0;
save_iptc=0;
save_com=0;
save_icc=0;
save_xmp=0;
break;
case 'T':
{
int tmpvar;
if (sscanf(optarg,"%d",&tmpvar) == 1) {
threshold=tmpvar;
if (threshold < 0) threshold=0;
if (threshold > 100) threshold=100;
}
else fatal("invalid argument for -T, --threshold");
}
break;
case 'S':
{
unsigned int tmpvar;
if (sscanf(optarg,"%u",&tmpvar) == 1) {
if (tmpvar > 0 && tmpvar < 100 && optarg[strlen(optarg)-1] == '%' ) {
target_size=-tmpvar;
} else {
target_size=tmpvar;
}
quality=100;
}
else fatal("invalid argument for -S, --size");
}
break;
}
}
/* check for '-' option indicating input is from stdin... */
i=1;
while (argv[i]) {
if (argv[i][0]=='-' && argv[i][1]==0) stdin_mode=1;
i++;
}
if (stdin_mode) { stdout_mode=1; force=1; }
if (stdout_mode) { logs_to_stdout=0; }
if (all_normal && all_progressive)
fatal("cannot specify both --all-normal and --all-progressive");
if (verbose_mode) {
if (quality>=0 && target_size==0)
fprintf(stderr,"Image quality limit set to: %d\n",quality);
if (threshold>=0)
fprintf(stderr,"Compression threshold (%%) set to: %d\n",threshold);
if (all_normal)
fprintf(stderr,"All output files will be non-progressive\n");
if (all_progressive)
fprintf(stderr,"All output files will be progressive\n");
if (target_size > 0)
fprintf(stderr,"Target size for output files set to: %u Kbytes.\n",
target_size);
if (target_size < 0)
fprintf(stderr,"Target size for output files set to: %u%%\n",
-target_size);
}
/* loop to process the input files */
i=1;
do {
if (stdin_mode) {
infile=stdin;
} else {
if (!argv[i][0]) continue;
if (argv[i][0]=='-') continue;
if (strlen(argv[i]) >= MAXPATHLEN) {
warn("skipping too long filename: %s",argv[i]);
continue;
}
if (!noaction) {
/* generate tmp dir & new filename */
if (dest) {
STRNCPY(tmpdir,dest_path,sizeof(tmpdir));
STRNCPY(newname,dest_path,sizeof(newname));
if (!splitname(argv[i],tmpfilename,sizeof(tmpfilename)))
fatal("splitname() failed for: %s",argv[i]);
strncat(newname,tmpfilename,sizeof(newname)-strlen(newname)-1);
} else {
if (!splitdir(argv[i],tmpdir,sizeof(tmpdir)))
fatal("splitdir() failed for: %s",argv[i]);
STRNCPY(newname,argv[i],sizeof(newname));
}
}
retry_point:
if (!is_file(argv[i],&file_stat)) {
if (is_directory(argv[i]))
warn("skipping directory: %s",argv[i]);
else
warn("skipping special file: %s",argv[i]);
continue;
}
if ((infile=fopen(argv[i],"rb"))==NULL) {
warn("cannot open file: %s", argv[i]);
continue;
}
}
if (setjmp(jderr.setjmp_buffer)) {
/* error handler for decompress */
jpeg_abort_decompress(&dinfo);
fclose(infile);
if (buf) FREE_LINE_BUF(buf,dinfo.output_height);
if (!quiet_mode || csv)
fprintf(LOG_FH,csv ? ",,,,,error\n" : " [ERROR]\n");
decompress_err_count++;
jderr.jump_set=0;
continue;
} else {
jderr.jump_set=1;
}
if (!retry && (!quiet_mode || csv)) {
fprintf(LOG_FH,csv ? "%s," : "%s ",(stdin_mode?"stdin":argv[i])); fflush(LOG_FH);
}
/* prepare to decompress */
global_error_counter=0;
jpeg_save_markers(&dinfo, JPEG_COM, 0xffff);
for (j=0;j<=15;j++)
jpeg_save_markers(&dinfo, JPEG_APP0+j, 0xffff);
jpeg_stdio_src(&dinfo, infile);
jpeg_read_header(&dinfo, TRUE);
/* check for Exif/IPTC/ICC/XMP markers */
marker_str[0]=0;
marker_in_count=0;
marker_in_size=0;
cmarker=dinfo.marker_list;
while (cmarker) {
marker_in_count++;
marker_in_size+=cmarker->data_length;
if (cmarker->marker == EXIF_JPEG_MARKER &&
!memcmp(cmarker->data,EXIF_IDENT_STRING,EXIF_IDENT_STRING_SIZE))
strncat(marker_str,"Exif ",sizeof(marker_str)-strlen(marker_str)-1);
if (cmarker->marker == IPTC_JPEG_MARKER)
strncat(marker_str,"IPTC ",sizeof(marker_str)-strlen(marker_str)-1);
if (cmarker->marker == ICC_JPEG_MARKER &&
!memcmp(cmarker->data,ICC_IDENT_STRING,ICC_IDENT_STRING_SIZE))
strncat(marker_str,"ICC ",sizeof(marker_str)-strlen(marker_str)-1);
if (cmarker->marker == XMP_JPEG_MARKER &&
!memcmp(cmarker->data,XMP_IDENT_STRING,XMP_IDENT_STRING_SIZE))
strncat(marker_str,"XMP ",sizeof(marker_str)-strlen(marker_str)-1);
cmarker=cmarker->next;
}
if (verbose_mode > 1)
fprintf(LOG_FH,"%d markers found in input file (total size %d bytes)\n",
marker_in_count,marker_in_size);
if (!retry && (!quiet_mode || csv)) {
fprintf(LOG_FH,csv ? "%dx%d,%dbit,%c," : "%dx%d %dbit %c ",(int)dinfo.image_width,
(int)dinfo.image_height,(int)dinfo.num_components*8,
(dinfo.progressive_mode?'P':'N'));
if (!csv) {
fprintf(LOG_FH,"%s",marker_str);
if (dinfo.saw_Adobe_marker) fprintf(LOG_FH,"Adobe ");
if (dinfo.saw_JFIF_marker) fprintf(LOG_FH,"JFIF ");
}
fflush(LOG_FH);
}
if ((insize=file_size(infile)) < 0)
fatal("failed to stat() input file");
/* decompress the file */
if (quality>=0 && !retry) {
jpeg_start_decompress(&dinfo);
/* allocate line buffer to store the decompressed image */
buf = malloc(sizeof(JSAMPROW)*dinfo.output_height);
if (!buf) fatal("not enough memory");
for (j=0;j<dinfo.output_height;j++) {
buf[j]=malloc(sizeof(JSAMPLE)*dinfo.output_width*
dinfo.out_color_components);
if (!buf[j]) fatal("not enough memory");
}
while (dinfo.output_scanline < dinfo.output_height) {
jpeg_read_scanlines(&dinfo,&buf[dinfo.output_scanline],
dinfo.output_height-dinfo.output_scanline);
}
} else {
coef_arrays = jpeg_read_coefficients(&dinfo);
}
if (!retry && !quiet_mode) {
if (global_error_counter==0) fprintf(LOG_FH," [OK] ");
else fprintf(LOG_FH," [WARNING] ");
fflush(LOG_FH);
}
fclose(infile);
infile=NULL;
if (dest && !noaction) {
if (file_exists(newname) && !overwrite_mode) {
warn("target file already exists: %s\n",newname);
jpeg_abort_decompress(&dinfo);
if (buf) FREE_LINE_BUF(buf,dinfo.output_height);
continue;
}
}
if (setjmp(jcerr.setjmp_buffer)) {
/* error handler for compress failures */
jpeg_abort_compress(&cinfo);
jpeg_abort_decompress(&dinfo);
if (!quiet_mode) fprintf(LOG_FH," [Compress ERROR]\n");
if (buf) FREE_LINE_BUF(buf,dinfo.output_height);
compress_err_count++;
jcerr.jump_set=0;
continue;
} else {
jcerr.jump_set=1;
}
lastsize = 0;
searchcount = 0;
searchdone = 0;
oldquality = 200;
binary_search_loop:
/* allocate memory buffer that should be large enough to store the output JPEG... */
if (outbuffer) free(outbuffer);
outbuffersize=insize + 32768;
outbuffer=malloc(outbuffersize);
if (!outbuffer) fatal("not enough memory");
/* setup custom "destination manager" for libjpeg to write to our buffer */
jpeg_memory_dest(&cinfo, &outbuffer, &outbuffersize, 65536);
if (quality>=0 && !retry) {
/* lossy "optimization" ... */
cinfo.in_color_space=dinfo.out_color_space;
cinfo.input_components=dinfo.output_components;
cinfo.image_width=dinfo.image_width;
cinfo.image_height=dinfo.image_height;
jpeg_set_defaults(&cinfo);
jpeg_set_quality(&cinfo,quality,TRUE);
if ( (dinfo.progressive_mode || all_progressive) && !all_normal )
jpeg_simple_progression(&cinfo);
cinfo.optimize_coding = TRUE;
j=0;
jpeg_start_compress(&cinfo,TRUE);
/* write markers */
write_markers(&dinfo,&cinfo);
/* write image */
while (cinfo.next_scanline < cinfo.image_height) {
jpeg_write_scanlines(&cinfo,&buf[cinfo.next_scanline],
dinfo.output_height);
}
} else {
/* lossless "optimization" ... */
jpeg_copy_critical_parameters(&dinfo, &cinfo);
if ( (dinfo.progressive_mode || all_progressive) && !all_normal )
jpeg_simple_progression(&cinfo);
cinfo.optimize_coding = TRUE;
/* write image */
jpeg_write_coefficients(&cinfo, coef_arrays);
/* write markers */
write_markers(&dinfo,&cinfo);
}
jpeg_finish_compress(&cinfo);
outsize=outbuffersize;
if (target_size != 0 && !retry) {
/* perform (binary) search to try to reach target file size... */
long osize = outsize/1024;
long isize = insize/1024;
long tsize = target_size;
if (tsize < 0) {
tsize=((-target_size)*insize/100)/1024;
if (tsize < 1) tsize=1;
}
if (osize == tsize || searchdone || searchcount >= 8 || tsize > isize) {
if (searchdone < 42 && lastsize > 0) {
if (abs(osize-tsize) > abs(lastsize-tsize)) {
if (verbose_mode) fprintf(LOG_FH,"(revert to %d)",oldquality);
searchdone=42;
quality=oldquality;
goto binary_search_loop;
}
}
if (verbose_mode) fprintf(LOG_FH," ");
} else {
int newquality;
int dif = round(abs(oldquality-quality)/2.0);
if (osize > tsize) {
newquality=quality-dif;
if (dif < 1) { newquality--; searchdone=1; }
if (newquality < 0) { newquality=0; searchdone=2; }
} else {
newquality=quality+dif;
if (dif < 1) { newquality++; searchdone=3; }
if (newquality > 100) { newquality=100; searchdone=4; }
}
oldquality=quality;
quality=newquality;
if (verbose_mode) fprintf(LOG_FH,"(try %d)",quality);
lastsize=osize;
searchcount++;
goto binary_search_loop;
}
}
if (buf) FREE_LINE_BUF(buf,dinfo.output_height);
jpeg_finish_decompress(&dinfo);
if (quality>=0 && outsize>=insize && !retry && !stdin_mode) {
if (verbose_mode) fprintf(LOG_FH,"(retry w/lossless) ");
retry=1;
goto retry_point;
}
retry=0;
ratio=(insize-outsize)*100.0/insize;
if (!quiet_mode || csv)
fprintf(LOG_FH,csv ? "%ld,%ld,%0.2f," : "%ld --> %ld bytes (%0.2f%%), ",insize,outsize,ratio);
average_count++;
average_rate+=(ratio<0 ? 0.0 : ratio);
if ((outsize < insize && ratio >= threshold) || force) {
total_save+=(insize-outsize)/1024.0;
if (!quiet_mode || csv) fprintf(LOG_FH,csv ? "optimized\n" : "optimized.\n");
if (noaction) continue;
if (stdout_mode) {
outfname=NULL;
if (fwrite(outbuffer,outbuffersize,1,stdout) != 1)
fatal("write failed to stdout");
} else {
if (preserve_perms && !dest) {
/* make backup of the original file */
snprintf(tmpfilename,sizeof(tmpfilename),"%s.jpegoptim.bak",newname);
if (verbose_mode > 1 && !quiet_mode)
fprintf(LOG_FH,"creating backup of original image as: %s\n",tmpfilename);
if (file_exists(tmpfilename))
fatal("backup file already exists: %s",tmpfilename);
if (copy_file(newname,tmpfilename))
fatal("failed to create backup of original file");
if ((outfile=fopen(newname,"wb"))==NULL)
fatal("error opening output file: %s", newname);
outfname=newname;
} else {
#ifdef HAVE_MKSTEMPS
/* rely on mkstemps() to create us temporary file safely... */
snprintf(tmpfilename,sizeof(tmpfilename),
"%sjpegoptim-%d-%d.XXXXXX.tmp", tmpdir, (int)getuid(), (int)getpid());
if ((tmpfd = mkstemps(tmpfilename,4)) < 0)
fatal("error creating temp file: mkstemps() failed");
if ((outfile=fdopen(tmpfd,"wb"))==NULL)
#else
/* if platform is missing mkstemps(), try to create at least somewhat "safe" temp file... */
snprintf(tmpfilename,sizeof(tmpfilename),
"%sjpegoptim-%d-%d.%d.tmp", tmpdir, (int)getuid(), (int)getpid(),time(NULL));
tmpfd=0;
if ((outfile=fopen(tmpfilename,"wb"))==NULL)
#endif
fatal("error opening temporary file: %s",tmpfilename);
outfname=tmpfilename;
}
if (verbose_mode > 1 && !quiet_mode)
fprintf(LOG_FH,"writing %lu bytes to file: %s\n",
(long unsigned int)outbuffersize, outfname);
if (fwrite(outbuffer,outbuffersize,1,outfile) != 1)
fatal("write failed to file: %s", outfname);
fclose(outfile);
}
if (outfname) {
if (preserve_mode) {
/* preserve file modification time */
struct utimbuf time_save;
time_save.actime=file_stat.st_atime;
time_save.modtime=file_stat.st_mtime;
if (utime(outfname,&time_save) != 0)
warn("failed to reset output file time/date");
}
if (preserve_perms && !dest) {
/* original file was already replaced, remove backup... */
if (delete_file(tmpfilename))
warn("failed to remove backup file: %s",tmpfilename);
} else {
/* make temp file to be the original file... */
/* preserve file mode */
if (chmod(outfname,(file_stat.st_mode & 0777)) != 0)
warn("failed to set output file mode");
/* preserve file group (and owner if run by root) */
if (chown(outfname,
(geteuid()==0 ? file_stat.st_uid : -1),
file_stat.st_gid) != 0)
warn("failed to reset output file group/owner");
if (verbose_mode > 1 && !quiet_mode)
fprintf(LOG_FH,"renaming: %s to %s\n",outfname,newname);
if (rename_file(outfname,newname)) fatal("cannot rename temp file");
}
}
} else {
if (!quiet_mode || csv) fprintf(LOG_FH,csv ? "skipped\n" : "skipped.\n");
}
} while (++i<argc && !stdin_mode);
if (totals_mode && !quiet_mode)
fprintf(LOG_FH,"Average ""compression"" (%ld files): %0.2f%% (%0.0fk)\n",
average_count, average_rate/average_count, total_save);
jpeg_destroy_decompress(&dinfo);
jpeg_destroy_compress(&cinfo);
return (decompress_err_count > 0 || compress_err_count > 0 ? 1 : 0);;
}
void laserCallback(const sensor_msgs::LaserScan& msg)
{
double angle;
unsigned int i;
// Store robot position in map grid coordinates
unsigned int robot_col = 0; // CHANGE ME
unsigned int robot_row = 0; // CHANGE ME
angle = msg.angle_min;
i=0;
// Go through all the LRF measurements
while(angle <= msg.angle_max)
{
if( msg.ranges[i] > msg.range_min )
{
///////////////////////////////////////////////////////////////////////
// Update map with each sensor reading.
//
// Important variables (already defined and available):
// - (robot_pose.x, robot_pose.y, robot_pose.theta) ==> Robot pose in the
// world frame;
// - msg.ranges[i] ==> LRF i reading (distance from the LRF to the beacon
// detected by the beacon at the i-th angle);
//
///////////////////////////////////////////////////////////////////////
// INSERT YOUR CODE BELOW THIS POINT
///////////////////////////////////////////////////////////////////////
// Create obstacle position in sensor coordinates from msg.ranges[i]
// Transform obstacle position to robot coordinates using local2world
// Get obtained value in world coordinates using again local2world
// Get obtained value in the map grid (columm and row) - must be unsigned
// int.
// Update map using the line iterator for free space
// Update map using the line iterator for occupied space, if applicable
///////////////////////////////////////////////////////////////////////
// INSERT YOUR CODE ABOVE THIS POINT
///////////////////////////////////////////////////////////////////////
}
// Proceed to next laser measure
angle += msg.angle_increment;
i++;
}
// Show map
cv::imshow("Mapa", map);
// Save the map every DELTA_SAVE iterations
iteration++;
if( iteration == DELTA_SAVE )
{
cv::imwrite("mapa.png", map);
iteration = 0;
}
/// Update distance to closest obstacles
// Right obstacle
angle = -1.571; // DEG2RAD(-90)
i = round((angle - msg.angle_min)/msg.angle_increment);
closest_right_obstacle = msg.range_max;
while( angle < -1.309 ) // DEG2RAD(-90+15)
{
if( (msg.ranges[i] < msg.range_max) &&
(msg.ranges[i] > msg.range_min) &&
(msg.ranges[i] < closest_right_obstacle) )
closest_right_obstacle = msg.ranges[i];
i++;
angle += msg.angle_increment;
}
// Front obstacle
angle = -0.785; // DEG2RAD(-45)
i = round((angle - msg.angle_min)/msg.angle_increment);
closest_front_obstacle = msg.range_max;
while( angle < 0.785 ) // DEG2RAD(45)
{
if( (msg.ranges[i] < msg.range_max) &&
(msg.ranges[i] > msg.range_min) &&
(msg.ranges[i] < closest_front_obstacle) )
closest_front_obstacle = msg.ranges[i];
i++;
angle += msg.angle_increment;
}
// Left obstacle
angle = 1.309; // DEG2RAD(90-15)
i = round((angle - msg.angle_min)/msg.angle_increment);
closest_left_obstacle = msg.range_max;
while( angle < 1.571 ) // DEG2RAD(90)
{
if( (msg.ranges[i] < msg.range_max) &&
(msg.ranges[i] > msg.range_min) &&
(msg.ranges[i] < closest_left_obstacle) )
closest_left_obstacle = msg.ranges[i];
i++;
angle += msg.angle_increment;
}
laser_updated = true;
return;
}
void simdetect (
int *detect, /* detector -1 single, 0 multi, 1 proximity, 2 count,... */
double *gsb0val, /* Parameter values (matrix nr= comb of g0,sigma,b nc=3) [naive animal] */
double *gsb1val, /* Parameter values (matrix nr= comb of g0,sigma,b nc=3) [caught before] */
int *cc0, /* number of g0/sigma/b combinations for naive animals */
int *cc1, /* number of g0/sigma/b combinations for caught before */
int *gsb0, /* lookup which g0/sigma/b combination to use for given g, S, K
[naive animal] */
int *gsb1, /* lookup which g0/sigma/b combination to use for given n, S, K
[caught before] */
int *N, /* number of animals */
int *ss, /* number of occasions */
int *kk, /* number of traps */
int *nmix, /* number of classes */
int *knownclass, /* known membership of 'latent' classes */
double *animals, /* x,y points of animal range centres (first x, then y) */
double *traps, /* x,y locations of traps (first x, then y) */
double *dist2, /* distances squared (optional: -1 if unused) */
double *Tsk, /* ss x kk array of 0/1 usage codes or effort */
int *btype, /* code for behavioural response
0 none
1 individual
2 individual, trap-specific
3 trap-specific
*/
int *Markov, /* learned vs transient behavioural response
0 learned
1 Markov
*/
int *binomN, /* number of trials for 'count' detector modelled with binomial */
double *miscparm, /* detection threshold on transformed scale, etc. */
int *fn, /* code 0 = halfnormal, 1 = hazard, 2 = exponential, 3 = uniform */
int *maxperpoly, /* */
int *n, /* number of individuals caught */
int *caught, /* sequence number in session (0 if not caught) */
double *detectedXY, /* x,y locations of detections */
double *signal, /* vector of signal strengths, one per detection */
int *value, /* return value array of trap locations n x s */
int *resultcode
)
{
double d2val;
double p;
int i,j,k,l,s;
int ik;
int nc = 0;
int nk = 0; /* number of detectors (polygons or transects when *detect==6,7) */
int count = 0;
int *caughtbefore;
int *x; /* mixture class of animal i */
double *pmix;
double runif;
int wxi = 0;
int c = 0;
int gpar = 2;
double g0 = 0;
double sigma = 0;
double z = 0;
double Tski = 1.0;
double *work = NULL;
double *noise = NULL; /* detectfn 12,13 only */
int *sortorder = NULL;
double *sortkey = NULL;
/*
*detect may take values -
-1 single-catch traps
0 multi-catch traps
1 binary proximity detectors
2 count proximity detectors
5 signal detectors
6 polygon detectors
7 transect detectors
*/
/*========================================================*/
/* 'single-catch only' declarations */
int tr_an_indx = 0;
int nanimals;
int ntraps;
int *occupied = NULL;
int *intrap = NULL;
struct trap_animal *tran = NULL;
double event_time;
int anum = 0;
int tnum = 0;
int nextcombo;
int finished;
int OK;
/*========================================================*/
/* 'multi-catch only' declarations */
double *h = NULL; /* multi-catch only */
double *hsum = NULL; /* multi-catch only */
double *cump = NULL; /* multi-catch only */
/*========================================================*/
/* 'polygon & transect only' declarations */
int nd = 0;
int cumk[maxnpoly+1];
int sumk; /* total number of vertices */
int g=0;
int *gotcha;
double xy[2];
int n1,n2,t;
double par[3];
int np = 1; /* n points each call of gxy */
double w, ws;
int maxdet=1;
double *cumd = NULL;
struct rpoint *line = NULL;
struct rpoint xyp;
struct rpoint animal;
double lx;
double maxg = 0;
double lambdak; /* temp value for Poisson rate */
double grx; /* temp value for integral gr */
double H;
int J;
int maybecaught;
double dx,dy,d;
double pks;
double sumhaz;
/*========================================================*/
/* 'signal-strength only' declarations */
double beta0;
double beta1;
double muS;
double sdS;
double muN = 0;
double sdN = 1;
double signalvalue;
double noisevalue;
double cut;
double *ex;
/*========================================================*/
/* MAIN LINE */
gotcha = &g;
*resultcode = 1;
caughtbefore = (int *) R_alloc(*N * *kk, sizeof(int));
x = (int *) R_alloc(*N, sizeof(int));
for (i=0; i<*N; i++) x[i] = 0;
pmix = (double *) R_alloc(*nmix, sizeof(double));
/* ------------------------------------------------------ */
/* pre-compute distances */
if (dist2[0] < 0) {
dist2 = (double *) S_alloc(*kk * *N, sizeof(double));
makedist2 (*kk, *N, traps, animals, dist2);
}
else {
squaredist (*kk, *N, dist2);
}
/* ------------------------------------------------------ */
if ((*detect < -1) || (*detect > 7)) return;
if (*detect == -1) { /* single-catch only */
occupied = (int*) R_alloc(*kk, sizeof(int));
intrap = (int*) R_alloc(*N, sizeof(int));
tran = (struct trap_animal *) R_alloc(*N * *kk,
sizeof(struct trap_animal));
/* 2*sizeof(int) + sizeof(double)); */
}
if (*detect == 0) { /* multi-catch only */
h = (double *) R_alloc(*N * *kk, sizeof(double));
hsum = (double *) R_alloc(*N, sizeof(double));
cump = (double *) R_alloc(*kk+1, sizeof(double));
cump[0] = 0;
}
if (*detect == 5) { /* signal only */
maxdet = *N * *ss * *kk;
if (!((*fn == 10) || (*fn == 11)))
error ("simsecr not implemented for this combination of detector & detectfn");
}
if ((*detect == 3) || (*detect == 4) || (*detect == 6) || (*detect == 7)) {
/* polygon or transect */
cumk[0] = 0;
for (i=0; i<maxnpoly; i++) { /* maxnpoly much larger than npoly */
if (kk[i]<=0) break;
cumk[i+1] = cumk[i] + kk[i];
nk++;
}
sumk = cumk[nk];
if ((*detect == 6) || (*detect == 7))
maxdet = *N * *ss * nk * *maxperpoly;
else
maxdet = *N * *ss;
}
else nk = *kk;
if ((*detect == 4) || (*detect == 7)) { /* transect only */
line = (struct rpoint *) R_alloc(sumk, sizeof(struct rpoint));
cumd = (double *) R_alloc(sumk, sizeof(double));
/* coordinates of vertices */
for (i=0; i<sumk; i++) {
line[i].x = traps[i];
line[i].y = traps[i+sumk];
}
/* cumulative distance along line; all transects end on end */
for (k=0; k<nk; k++) {
cumd[cumk[k]] = 0;
for (i=cumk[k]; i<(cumk[k+1]-1); i++) {
cumd[i+1] = cumd[i] + distance(line[i], line[i+1]);
}
}
}
if ((*detect==3) || (*detect==4) || (*detect==5) || (*detect==6) || (*detect==7)) {
work = (double*) R_alloc(maxdet*2, sizeof(double)); /* twice size needed for signal */
sortorder = (int*) R_alloc(maxdet, sizeof(int));
sortkey = (double*) R_alloc(maxdet, sizeof(double));
}
if ((*fn==12) || (*fn==13)) {
noise = (double*) R_alloc(maxdet*2, sizeof(double)); /* twice size needed for signal */
}
GetRNGstate();
gpar = 2;
if ((*fn == 1) || (*fn == 3) || (*fn == 5)|| (*fn == 6) || (*fn == 7) || (*fn == 8) ||
(*fn == 10) || (*fn == 11)) gpar ++;
/* ------------------------------------------------------------------------- */
/* mixture models */
/* may be better to pass pmix */
if (*nmix>1) {
if (*nmix>2)
error("simsecr nmix>2 not implemented");
gpar++; /* these models have one more detection parameter */
for (i=0; i<*nmix; i++) {
wxi = i4(0,0,0,i,*N,*ss,nk);
c = gsb0[wxi] - 1;
pmix[i] = gsb0val[*cc0 * (gpar-1) + c]; /* assuming 4-column gsb */
}
for (i=0; i<*N; i++) {
if (knownclass[i] > 1)
x[i] = knownclass[i] - 2; /* knownclass=2 maps to x=0 etc. */
else
x[i] = rdiscrete(*nmix, pmix) - 1;
}
}
/* ------------------------------------------------------------------------- */
/* zero caught status */
for (i=0; i<*N; i++)
caught[i] = 0;
for (i=0; i<*N; i++)
for (k=0; k < nk; k++)
caughtbefore[k * (*N-1) + i] = 0;
/* ------------------------------------------------------------------------- */
/* MAIN LOOP */
for (s=0; s<*ss; s++) {
/* ------------------ */
/* single-catch traps */
if (*detect == -1) {
/* initialise day */
tr_an_indx = 0;
nanimals = *N;
ntraps = nk;
for (i=0; i<*N; i++) intrap[i] = 0;
for (k=0; k<nk; k++) occupied[k] = 0;
nextcombo = 0;
/* make tran */
for (i=0; i<*N; i++) { /* animals */
for (k=0; k<nk; k++) { /* traps */
Tski = Tsk[s * nk + k];
if (fabs(Tski) > 1e-10) {
getpar (i, s, k, x[i], *N, *ss, nk, *cc0, *cc1, *fn,
bswitch (*btype, *N, i, k, caughtbefore),
gsb0, gsb0val, gsb1, gsb1val, &g0, &sigma, &z);
/* d2val = d2(i,k, animals, traps, *N, nk); */
d2val = d2L(k, i, dist2, nk);
p = pfn(*fn, d2val, g0, sigma, z, miscparm, 1e20); /* effectively inf w2 */
if (fabs(Tski-1) > 1e-10)
p = 1 - pow(1-p, Tski);
event_time = randomtime(p);
if (event_time <= 1) {
tran[tr_an_indx].time = event_time;
tran[tr_an_indx].animal = i; /* 0..*N-1 */
tran[tr_an_indx].trap = k; /* 0..nk-1 */
tr_an_indx++;
}
}
}
}
/* end of make tran */
if (tr_an_indx > 1) probsort (tr_an_indx, tran);
while ((nextcombo < tr_an_indx) && (nanimals>0) && (ntraps>0)) {
finished = 0;
OK = 0;
while ((1-finished)*(1-OK) > 0) { /* until finished or OK */
if (nextcombo >= (tr_an_indx))
finished = 1; /* no more to process */
else {
anum = tran[nextcombo].animal;
tnum = tran[nextcombo].trap;
OK = (1-occupied[tnum]) * (1-intrap[anum]); /* not occupied and not intrap */
nextcombo++;
}
}
if (finished==0) {
/* Record this capture */
occupied[tnum] = 1;
intrap[anum] = tnum+1; /* trap = k+1 */
nanimals--;
ntraps--;
}
}
for (i=0; i<*N; i++) {
if (intrap[i]>0) {
if (caught[i]==0) { /* first capture of this animal */
nc++;
caught[i] = nc; /* nc-th animal to be captured */
for (j=0; j<*ss; j++)
value[*ss * (nc-1) + j] = 0;
}
value[*ss * (caught[i]-1) + s] = intrap[i]; /* trap = k+1 */
}
}
}
/* -------------------------------------------------------------------------- */
/* multi-catch trap; only one site per occasion (drop last dimension of capt) */
else if (*detect == 0) {
for (i=0; i<*N; i++) {
hsum[i] = 0;
for (k=0; k<nk; k++) {
Tski = Tsk[s * nk + k];
if (fabs(Tski) > 1e-10) {
getpar (i, s, k, x[i], *N, *ss, nk, *cc0, *cc1, *fn,
bswitch (*btype, *N, i, k, caughtbefore),
gsb0, gsb0val, gsb1, gsb1val, &g0, &sigma, &z);
/* d2val = d2(i,k, animals, traps, *N, nk); */
d2val = d2L(k, i, dist2, nk);
p = pfn(*fn, d2val, g0, sigma, z, miscparm, 1e20);
h[k * *N + i] = - Tski * log(1 - p);
hsum[i] += h[k * *N + i];
}
}
for (k=0; k<nk; k++) {
cump[k+1] = cump[k] + h[k * *N + i]/hsum[i];
}
if (Random() < (1-exp(-hsum[i]))) {
if (caught[i]==0) { /* first capture of this animal */
nc++;
caught[i] = nc;
for (j=0; j<*ss; j++)
value[*ss * (nc-1) + j] = 0;
}
/* find trap with probability proportional to p
searches cumulative distribution of p */
runif = Random();
k = 0;
while ((runif > cump[k]) && (k<nk)) k++;
value[*ss * (caught[i]-1) + s] = k; /* trap = k+1 */
}
}
}
/* -------------------------------------------------------------------------------- */
/* the 'proximity' group of detectors 1:2 - proximity, count */
else if ((*detect >= 1) && (*detect <= 2)) {
for (i=0; i<*N; i++) {
for (k=0; k<nk; k++) {
Tski = Tsk[s * nk + k];
if (fabs(Tski) > 1e-10) {
getpar (i, s, k, x[i], *N, *ss, nk, *cc0, *cc1, *fn,
bswitch (*btype, *N, i, k, caughtbefore),
gsb0, gsb0val, gsb1, gsb1val, &g0, &sigma, &z);
/* d2val = d2(i,k, animals, traps, *N, nk); */
d2val = d2L(k, i, dist2, nk);
p = pfn(*fn, d2val, g0, sigma, z, miscparm, 1e20);
if (p < -0.1) { PutRNGstate(); return; } /* error */
if (p>0) {
if (*detect == 1) {
if (fabs(Tski-1) > 1e-10)
p = 1 - pow(1-p, Tski);
count = Random() < p; /* binary proximity */
}
else if (*detect == 2) { /* count proximity */
if (*binomN == 1)
count = rcount(round(Tski), p, 1);
else
count = rcount(*binomN, p, Tski);
}
if (count>0) {
if (caught[i]==0) { /* first capture of this animal */
nc++;
caught[i] = nc;
for (j=0; j<*ss; j++)
for (l=0; l<nk; l++)
value[*ss * ((nc-1) * nk + l) + j] = 0;
}
value[*ss * ((caught[i]-1) * nk + k) + s] = count;
}
}
}
}
}
}
/* -------------------------------------------------------------------------------- */
/* exclusive polygon detectors */
else if (*detect == 3) {
/* find maximum distance between animal and detector vertex */
w = 0;
J = cumk[nk];
for (i = 0; i< *N; i++) {
for (j = 0; j < J; j++) {
dx = animals[i] - traps[j];
dy = animals[*N + i] - traps[J + j];
d = sqrt(dx*dx + dy*dy);
if (d > w) w = d;
}
}
for (i=0; i<*N; i++) {
/* this implementation assumes NO VARIATION AMONG DETECTORS */
getpar (i, s, 0, x[i], *N, *ss, nk, *cc0, *cc1, *fn,
bswitch (*btype, *N, i, 0, caughtbefore),
gsb0, gsb0val, gsb1, gsb1val, &g0, &sigma, &z);
maybecaught = Random() < g0;
if (w > (10 * sigma))
ws = 10 * sigma;
else
ws = w;
par[0] = 1;
par[1] = sigma;
par[2] = z;
if (maybecaught) {
gxy (&np, fn, par, &ws, xy); /* simulate location */
xy[0] = xy[0] + animals[i];
xy[1] = xy[1] + animals[*N + i];
for (k=0; k<nk; k++) { /* each polygon */
Tski = Tsk[s * nk + k];
if (fabs(Tski) > 1e-10) {
n1 = cumk[k];
n2 = cumk[k+1]-1;
inside(xy, &n1, &n2, &sumk, traps, gotcha); /* assume closed */
if (*gotcha > 0) {
if (caught[i]==0) { /* first capture of this animal */
nc++;
caught[i] = nc;
for (t=0; t<*ss; t++)
value[*ss * (nc-1) + t] = 0;
}
nd++;
value[*ss * (caught[i]-1) + s] = k+1;
work[(nd-1)*2] = xy[0];
work[(nd-1)*2+1] = xy[1];
sortkey[nd-1] = (double) (s * *N + caught[i]);
break; /* no need to look at more poly */
}
}
}
}
}
}
/* -------------------------------------------------------------------------------- */
/* exclusive transect detectors */
else if (*detect == 4) {
ex = (double *) R_alloc(10 + 2 * maxvertices, sizeof(double));
for (i=0; i<*N; i++) { /* each animal */
animal.x = animals[i];
animal.y = animals[i + *N];
sumhaz = 0;
/* ------------------------------------ */
/* sum hazard */
for (k=0; k<nk; k++) {
Tski = Tsk[s * nk + k];
if (fabs(Tski) > 1e-10) {
getpar (i, s, k, x[i], *N, *ss, nk, *cc0, *cc1, *fn,
bswitch (*btype, *N, i, k, caughtbefore),
gsb0, gsb0val, gsb1, gsb1val, &g0, &sigma, &z);
par[0] = g0;
par[1] = sigma;
par[2] = z;
n1 = cumk[k];
n2 = cumk[k+1]-1;
H = hintegral1(*fn, par);
sumhaz += -log(1 - par[0] * integral1D (*fn, i, 0, par, 1, traps,
animals, n1, n2, sumk, *N, ex) / H);
}
}
/* ------------------------------------ */
for (k=0; k<nk; k++) { /* each transect */
Tski = Tsk[s * nk + k];
if (fabs(Tski) > 1e-10) {
getpar (i, s, k, x[i], *N, *ss, nk, *cc0, *cc1, *fn,
bswitch (*btype, *N, i, k, caughtbefore),
gsb0, gsb0val, gsb1, gsb1val, &g0, &sigma, &z);
par[0] = g0;
par[1] = sigma;
par[2] = z;
n1 = cumk[k];
n2 = cumk[k+1]-1;
H = hintegral1(*fn, par);
lambdak = par[0] * integral1D (*fn, i, 0, par, 1, traps, animals,
n1, n2, sumk, *N, ex) / H;
pks = (1 - exp(-sumhaz)) * (-log(1-lambdak)) / sumhaz;
count = Random() < pks;
maxg = 0;
if (count>0) { /* find maximum - approximate */
for (l=0; l<=100; l++) {
lx = (cumd[n2] - cumd[n1]) * l/100;
xyp = getxy (lx, cumd, line, sumk, n1);
grx = gr (fn, par, xyp, animal);
if (R_FINITE(grx))
maxg = fmax2(maxg, grx);
}
for (l=n1; l<=n2; l++) {
xyp = line[l];
grx = gr (fn, par, xyp, animal);
if (R_FINITE(grx))
maxg = fmax2(maxg, grx);
}
maxg= 1.2 * maxg; /* safety margin */
if (maxg<=0)
Rprintf("maxg error in simsecr\n"); /* not found */
*gotcha = 0;
l = 0;
while (*gotcha == 0) {
lx = Random() * (cumd[n2] - cumd[n1]); /* simulate location */
xyp = getxy (lx, cumd, line, sumk, n1);
grx = gr (fn, par, xyp, animal);
if (Random() < (grx/maxg)) /* rejection sampling */
*gotcha = 1;
l++;
if (l % 10000 == 0)
R_CheckUserInterrupt();
if (l>1e8) *gotcha = 1; /* give up and accept anything!!!! */
}
if (caught[i]==0) { /* first capture of this animal */
nc++;
caught[i] = nc;
for (t=0; t<*ss; t++)
value[*ss * (nc-1) + t] = 0;
}
nd++;
if (nd >= maxdet) {
*resultcode = 2; /* error */
return;
}
value[*ss * (caught[i]-1) + s] = k+1;
work[(nd-1)*2] = xyp.x;
work[(nd-1)*2+1] = xyp.y;
sortkey[nd-1] = (double) (s * *N + caught[i]);
}
if (count>0) break; /* no need to look further */
}
} /* end loop over transects */
} /* end loop over animals */
}
/* -------------------------------------------------------------------------------- */
/* polygon detectors */
else if (*detect == 6) {
for (i=0; i<*N; i++) {
/* this implementation assumes NO VARIATION AMONG DETECTORS */
getpar (i, s, 0, x[i], *N, *ss, nk, *cc0, *cc1, *fn,
bswitch (*btype, *N, i, 0, caughtbefore),
gsb0, gsb0val, gsb1, gsb1val, &g0, &sigma, &z);
count = rcount(*binomN, g0, Tski);
w = 10 * sigma;
par[0] = 1;
par[1] = sigma;
par[2] = z;
for (j=0; j<count; j++) {
gxy (&np, fn, par, &w, xy); /* simulate location */
xy[0] = xy[0] + animals[i];
xy[1] = xy[1] + animals[*N + i];
for (k=0; k<nk; k++) { /* each polygon */
Tski = Tsk[s * nk + k];
if (fabs(Tski) > 1e-10) {
n1 = cumk[k];
n2 = cumk[k+1]-1;
inside(xy, &n1, &n2, &sumk, traps, gotcha); /* assume closed */
if (*gotcha > 0) {
if (caught[i]==0) { /* first capture of this animal */
nc++;
caught[i] = nc;
for (t=0; t<*ss; t++)
for (l=0; l<nk; l++)
value[*ss * ((nc-1) * nk + l) + t] = 0;
}
nd++;
if (nd > maxdet) {
*resultcode = 2;
return; /* error */
}
value[*ss * ((caught[i]-1) * nk + k) + s]++;
work[(nd-1)*2] = xy[0];
work[(nd-1)*2+1] = xy[1];
sortkey[nd-1] = (double) (k * *N * *ss + s * *N + caught[i]);
}
}
}
}
}
}
/* -------------------------------------------------------------------------------- */
/* transect detectors */
else if (*detect == 7) {
ex = (double *) R_alloc(10 + 2 * maxvertices, sizeof(double));
for (i=0; i<*N; i++) { /* each animal */
animal.x = animals[i];
animal.y = animals[i + *N];
for (k=0; k<nk; k++) { /* each transect */
Tski = Tsk[s * nk + k];
if (fabs(Tski) > 1e-10) {
getpar (i, s, k, x[i], *N, *ss, nk, *cc0, *cc1, *fn,
bswitch (*btype, *N, i, k, caughtbefore),
gsb0, gsb0val, gsb1, gsb1val, &g0, &sigma, &z);
par[0] = g0;
par[1] = sigma;
par[2] = z;
n1 = cumk[k];
n2 = cumk[k+1]-1;
H = hintegral1(*fn, par);
lambdak = par[0] * integral1D (*fn, i, 0, par, 1, traps, animals,
n1, n2, sumk, *N, ex) / H;
count = rcount(*binomN, lambdak, Tski); /* numb detections on transect */
maxg = 0;
if (count>0) { /* find maximum - approximate */
for (l=0; l<=100; l++) {
lx = (cumd[n2]-cumd[n1]) * l/100;
xyp = getxy (lx, cumd, line, sumk, n1);
grx = gr (fn, par, xyp, animal);
if (R_FINITE(grx))
maxg = fmax2(maxg, grx);
}
for (l=n1; l<=n2; l++) {
xyp = line[l];
grx = gr (fn, par, xyp, animal);
if (R_FINITE(grx))
maxg = fmax2(maxg, grx);
}
maxg= 1.2 * maxg; /* safety margin */
if (maxg<=0)
Rprintf("maxg error in simsecr\n"); /* not found */
}
for (j=0; j<count; j++) {
*gotcha = 0;
l = 0;
while (*gotcha == 0) {
lx = Random() * (cumd[n2]-cumd[n1]); /* simulate location */
xyp = getxy (lx, cumd, line, sumk, n1);
grx = gr (fn, par, xyp, animal);
if (Random() < (grx/maxg)) /* rejection sampling */
*gotcha = 1;
l++;
if (l % 10000 == 0)
R_CheckUserInterrupt();
if (l>1e8) *gotcha = 1; /* give up and accept anything!!!! */
}
if (caught[i]==0) { /* first capture of this animal */
nc++;
caught[i] = nc;
for (t=0; t<*ss; t++)
for (l=0; l<nk; l++)
value[*ss * ((nc-1) * nk + l) + t] = 0;
}
nd++;
if (nd >= maxdet) {
*resultcode = 2; /* error */
return;
}
value[*ss * ((caught[i]-1) * nk + k) + s]++;
work[(nd-1)*2] = xyp.x;
work[(nd-1)*2+1] = xyp.y;
sortkey[nd-1] = (double) (k * *N * *ss + s * *N + caught[i]);
}
}
} /* end loop over transects */
} /* end loop over animals */
}
/* ------------------------ */
/* signal strength detector */
else if (*detect == 5) {
cut = miscparm[0];
if ((*fn == 12) || (*fn == 13)) {
muN = miscparm[1];
sdN = miscparm[2];
}
for (i=0; i<*N; i++) {
for (k=0; k<nk; k++) {
Tski = Tsk[s * nk + k];
if (fabs(Tski) > 1e-10) {
/* sounds not recaptured */
getpar (i, s, k, x[i], *N, *ss, nk, *cc0, *cc1, *fn,
0, gsb0, gsb0val, gsb0, gsb0val, &beta0, &beta1, &sdS);
/*
if ((*fn == 10) || (*fn == 12))
muS = mufn (i, k, beta0, beta1, animals, traps, *N, nk, 0);
else
muS = mufn (i, k, beta0, beta1, animals, traps, *N, nk, 1);
*/
if ((*fn == 10) || (*fn == 12))
muS = mufnL (k, i, beta0, beta1, dist2, nk, 0);
else
muS = mufnL (k, i, beta0, beta1, dist2, nk, 1);
signalvalue = norm_rand() * sdS + muS;
if ((*fn == 10) || (*fn == 11)) {
if (signalvalue > cut) {
if (caught[i]==0) { /* first capture of this animal */
nc++;
caught[i] = nc;
for (j=0; j<*ss; j++)
for (l=0; l<nk; l++)
value[*ss * ((nc-1) * *kk + l) + j] = 0;
}
nd++;
value[*ss * ((caught[i]-1) * *kk + k) + s] = 1;
work[nd-1] = signalvalue;
sortkey[nd-1] = (double) (k * *N * *ss + s * *N + caught[i]);
}
}
else {
noisevalue = norm_rand() * sdN + muN;
if ((signalvalue - noisevalue) > cut) {
if (caught[i]==0) { /* first capture of this animal */
nc++;
caught[i] = nc;
for (j=0; j<*ss; j++)
for (l=0; l<nk; l++)
value[*ss * ((nc-1) * *kk + l) + j] = 0;
}
nd++;
value[*ss * ((caught[i]-1) * *kk + k) + s] = 1;
work[nd-1] = signalvalue;
noise[nd-1] = noisevalue;
sortkey[nd-1] = (double) (k * *N * *ss + s * *N + caught[i]);
}
}
}
}
}
}
if ((*btype > 0) && (s < (*ss-1))) {
/* update record of 'previous-capture' status */
if (*btype == 1) {
for (i=0; i<*N; i++) {
if (*Markov)
caughtbefore[i] = 0;
for (k=0; k<nk; k++)
caughtbefore[i] = imax2 (value[i3(s, k, i, *ss, nk)], caughtbefore[i]);
}
}
else if (*btype == 2) {
for (i=0; i<*N; i++) {
for (k=0; k<nk; k++) {
ik = k * (*N-1) + i;
if (*Markov)
caughtbefore[ik] = value[i3(s, k, i, *ss, nk)];
else
caughtbefore[ik] = imax2 (value[i3(s, k, i, *ss, nk)],
caughtbefore[ik]);
}
}
}
else {
for (k=0;k<nk;k++) {
if (*Markov)
caughtbefore[k] = 0;
for (i=0; i<*N; i++)
caughtbefore[k] = imax2 (value[i3(s, k, i, *ss, nk)], caughtbefore[k]);
}
}
}
} /* loop over s */
if ((*detect==3) || (*detect==4) || (*detect==5) || (*detect==6) || (*detect==7)) {
for (i=0; i<nd; i++) sortorder[i] = i;
if (nd>0) rsort_with_index (sortkey, sortorder, nd);
if (*detect==5) {
for (i=0; i<nd; i++) signal[i] = work[sortorder[i]];
if ((*fn == 12) || (*fn == 13)) {
for (i=0; i<nd; i++) signal[i+nd] = noise[sortorder[i]];
}
}
else {
for (i=0; i<nd; i++) {
detectedXY[i] = work[sortorder[i]*2];
detectedXY[i+nd] = work[sortorder[i]*2+1];
}
}
}
*n = nc;
PutRNGstate();
*resultcode = 0;
}
void activity_handlers::butcher_finish( player_activity *act, player *p )
{
// Corpses can disappear (rezzing!), so check for that
auto items_here = g->m.i_at( p->pos() );
if( static_cast<int>( items_here.size() ) <= act->index ||
!( items_here[act->index].is_corpse() ) ) {
add_msg(m_info, _("There's no corpse to butcher!"));
return;
}
item &corpse_item = items_here[act->index];
const mtype *corpse = corpse_item.get_mtype();
std::vector<item> contents = corpse_item.contents;
const int age = corpse_item.bday;
g->m.i_rem( p->pos(), act->index );
const int factor = p->butcher_factor();
int pieces = 0;
int skins = 0;
int bones = 0;
int fats = 0;
int sinews = 0;
int feathers = 0;
int wool = 0;
bool stomach = false;
switch (corpse->size) {
case MS_TINY:
pieces = 1;
skins = 1;
bones = 1;
fats = 1;
sinews = 1;
feathers = 2;
wool = 1;
break;
case MS_SMALL:
pieces = 2;
skins = 2;
bones = 4;
fats = 2;
sinews = 4;
feathers = 6;
wool = 2;
break;
case MS_MEDIUM:
pieces = 4;
skins = 4;
bones = 9;
fats = 4;
sinews = 9;
feathers = 11;
wool = 4;
break;
case MS_LARGE:
pieces = 8;
skins = 8;
bones = 14;
fats = 8;
sinews = 14;
feathers = 17;
wool = 8;
break;
case MS_HUGE:
pieces = 16;
skins = 16;
bones = 21;
fats = 16;
sinews = 21;
feathers = 24;
wool = 16;
break;
}
const int skill_level = p->skillLevel( skill_survival );
auto roll_butchery = [&] () {
double skill_shift = 0.0;
///\xrefitem Skill_Effects_Survival "" "" Survival above 3 randomly increases Butcher rolls, below 3 decreases
skill_shift += rng_float( 0, skill_level - 3 );
///\xrefitem Stat_Effects_Dexterity "" "" Dexterity above 8 randomly increases Butcher rolls, slightly, below 8 decreases
skill_shift += rng_float( 0, p->dex_cur - 8 ) / 4.0;
///\xrefitem Stat_Effects_Strength "" "" Strength below 4 randomly decreases Butcher rolls, slightly
if( p->str_cur < 4 ) {
skill_shift -= rng_float( 0, 5 * ( 4 - p->str_cur ) ) / 4.0;
}
if( factor < 0 ) {
skill_shift -= rng_float( 0, -factor / 5.0 );
}
return static_cast<int>( round( skill_shift ) );
};
int practice = std::max( 0, 4 + pieces + roll_butchery());
p->practice( skill_survival, practice );
// Lose some meat, skins, etc if the rolls are low
pieces += std::min( 0, roll_butchery() );
skins += std::min( 0, roll_butchery() - 4 );
bones += std::min( 0, roll_butchery() - 2 );
fats += std::min( 0, roll_butchery() - 4 );
sinews += std::min( 0, roll_butchery() - 8 );
feathers += std::min( 0, roll_butchery() - 1 );
wool += std::min( 0, roll_butchery() );
stomach = roll_butchery() >= 0;
if( bones > 0 ) {
if( corpse->has_material("veggy") ) {
g->m.spawn_item(p->pos(), "plant_sac", bones, 0, age);
add_msg(m_good, _("You harvest some fluid bladders!"));
} else if( corpse->has_flag(MF_BONES) && corpse->has_flag(MF_POISON) ) {
g->m.spawn_item(p->pos(), "bone_tainted", bones / 2, 0, age);
add_msg(m_good, _("You harvest some salvageable bones!"));
} else if( corpse->has_flag(MF_BONES) && corpse->has_flag(MF_HUMAN) ) {
g->m.spawn_item(p->pos(), "bone_human", bones, 0, age);
add_msg(m_good, _("You harvest some salvageable bones!"));
} else if( corpse->has_flag(MF_BONES) ) {
g->m.spawn_item(p->pos(), "bone", bones, 0, age);
add_msg(m_good, _("You harvest some usable bones!"));
}
}
if( sinews > 0 ) {
if( corpse->has_flag(MF_BONES) && !corpse->has_flag(MF_POISON) ) {
g->m.spawn_item(p->pos(), "sinew", sinews, 0, age);
add_msg(m_good, _("You harvest some usable sinews!"));
} else if( corpse->has_material("veggy") ) {
g->m.spawn_item(p->pos(), "plant_fibre", sinews, 0, age);
add_msg(m_good, _("You harvest some plant fibers!"));
}
}
if( stomach ) {
const itype_id meat = corpse->get_meat_itype();
if( meat == "meat" ) {
if( corpse->size == MS_SMALL || corpse->size == MS_MEDIUM ) {
g->m.spawn_item(p->pos(), "stomach", 1, 0, age);
add_msg(m_good, _("You harvest the stomach!"));
} else if( corpse->size == MS_LARGE || corpse->size == MS_HUGE ) {
g->m.spawn_item(p->pos(), "stomach_large", 1, 0, age);
add_msg(m_good, _("You harvest the stomach!"));
}
} else if( meat == "human_flesh" ) {
if( corpse->size == MS_SMALL || corpse->size == MS_MEDIUM ) {
g->m.spawn_item(p->pos(), "hstomach", 1, 0, age);
add_msg(m_good, _("You harvest the stomach!"));
} else if( corpse->size == MS_LARGE || corpse->size == MS_HUGE ) {
g->m.spawn_item(p->pos(), "hstomach_large", 1, 0, age);
add_msg(m_good, _("You harvest the stomach!"));
}
}
}
if( (corpse->has_flag(MF_FUR) || corpse->has_flag(MF_LEATHER) ||
corpse->has_flag(MF_CHITIN)) && skins > 0 ) {
add_msg(m_good, _("You manage to skin the %s!"), corpse->nname().c_str());
int fur = 0;
int leather = 0;
int human_leather = 0;
int chitin = 0;
while (skins > 0 ) {
if( corpse->has_flag(MF_CHITIN) ) {
chitin = rng(0, skins);
skins -= chitin;
skins = std::max(skins, 0);
}
if( corpse->has_flag(MF_FUR) ) {
fur = rng(0, skins);
skins -= fur;
skins = std::max(skins, 0);
}
if( corpse->has_flag(MF_LEATHER) ) {
if( corpse->has_flag(MF_HUMAN) ) {
human_leather = rng(0, skins);
skins -= human_leather;
} else {
leather = rng(0, skins);
skins -= leather;
}
skins = std::max(skins, 0);
}
}
if( chitin > 0 ) {
g->m.spawn_item(p->pos(), "chitin_piece", chitin, 0, age);
}
if( fur > 0 ) {
g->m.spawn_item(p->pos(), "raw_fur", fur, 0, age);
}
if( leather > 0 ) {
g->m.spawn_item(p->pos(), "raw_leather", leather, 0, age);
}
if( human_leather ) {
g->m.spawn_item(p->pos(), "raw_hleather", leather, 0, age);
}
}
if( feathers > 0 ) {
if( corpse->has_flag(MF_FEATHER) ) {
g->m.spawn_item(p->pos(), "feather", feathers, 0, age);
add_msg(m_good, _("You harvest some feathers!"));
}
}
if( wool > 0 ) {
if( corpse->has_flag(MF_WOOL) ) {
g->m.spawn_item(p->pos(), "wool_staple", wool, 0, age);
add_msg(m_good, _("You harvest some wool staples!"));
}
}
if( fats > 0 ) {
if( corpse->has_flag(MF_FAT) && corpse->has_flag(MF_POISON) ) {
g->m.spawn_item(p->pos(), "fat_tainted", fats, 0, age);
add_msg(m_good, _("You harvest some gooey fat!"));
} else if( corpse->has_flag(MF_FAT) ) {
g->m.spawn_item(p->pos(), "fat", fats, 0, age);
add_msg(m_good, _("You harvest some fat!"));
}
}
//Add a chance of CBM recovery. For shocker and cyborg corpses.
//As long as the factor is above -4 (the sinew cutoff), you will be able to extract cbms
if( corpse->has_flag(MF_CBM_CIV) ) {
butcher_cbm_item( "bio_power_storage", p->pos(), age, roll_butchery() );
butcher_cbm_group( "bionics_common", p->pos(), age, roll_butchery() );
}
// Zombie scientist bionics
if( corpse->has_flag(MF_CBM_SCI) ) {
butcher_cbm_item( "bio_power_storage", p->pos(), age, roll_butchery() );
butcher_cbm_group( "bionics_sci", p->pos(), age, roll_butchery() );
}
// Zombie technician bionics
if( corpse->has_flag(MF_CBM_TECH) ) {
butcher_cbm_item( "bio_power_storage", p->pos(), age, roll_butchery() );
butcher_cbm_group( "bionics_tech", p->pos(), age, roll_butchery() );
}
// Substation mini-boss bionics
if( corpse->has_flag(MF_CBM_SUBS) ) {
butcher_cbm_item( "bio_power_storage", p->pos(), age, roll_butchery() );
butcher_cbm_group( "bionics_subs", p->pos(), age, roll_butchery() );
butcher_cbm_group( "bionics_subs", p->pos(), age, roll_butchery() );
}
// Payoff for butchering the zombie bio-op
if( corpse->has_flag(MF_CBM_OP) ) {
butcher_cbm_item( "bio_power_storage_mkII", p->pos(), age, roll_butchery() );
butcher_cbm_group( "bionics_op", p->pos(), age, roll_butchery() );
}
//Add a chance of CBM power storage recovery.
if( corpse->has_flag(MF_CBM_POWER) ) {
butcher_cbm_item( "bio_power_storage", p->pos(), age, roll_butchery() );
}
// Recover hidden items
for( auto &content : contents ) {
if( ( roll_butchery() + 10 ) * 5 > rng( 0, 100 ) ) {
//~ %1$s - item name, %2$s - monster name
add_msg( m_good, _( "You discover a %1$s in the %2$s!" ), content.tname().c_str(),
corpse->nname().c_str() );
g->m.add_item_or_charges( p->pos(), content );
} else if( content.is_bionic() ) {
g->m.spawn_item(p->pos(), "burnt_out_bionic", 1, 0, age);
}
}
if( pieces <= 0 ) {
add_msg(m_bad, _("Your clumsy butchering destroys the meat!"));
} else {
add_msg(m_good, _("You butcher the corpse."));
const itype_id meat = corpse->get_meat_itype();
if( meat == "null" ) {
return;
}
item tmpitem(meat, age);
tmpitem.set_mtype( corpse );
while ( pieces > 0 ) {
pieces--;
g->m.add_item_or_charges(p->pos(), tmpitem);
}
}
}
// radians [-pi, pi]
// speedfrac [0,1]
// torquefrac [0,1]
void
dynamixel_set_joint_goal_default(dynamixel_device_t *device,
int pmask,
double radians,
double speedfrac,
double torquefrac)
{
assert (!device->rotation_mode && (pmask == 0xfff || pmask == 0x3ff));
// Ensure proper ranges
radians = mod2pi(radians);
speedfrac = dmax(0.0, dmin(1.0, dabs(speedfrac)));
torquefrac = dmax(0.0, dmin(1.0, torquefrac));
double min = device->get_min_position_radians(device);
double max = device->get_max_position_radians(device);
radians = dmax(min, dmin(max, radians));
int stop = speedfrac < (1.0/0x3ff);
int posv = ((int) round((radians - min) / (max - min) * pmask)) & pmask;
// in joint-mode, speed == 0 --> maxspeed
int speedv = stop ? 0x1 : (int)(speedfrac * 0x3ff);
int torquev = (int)(torquefrac * 0x3ff);
dynamixel_msg_t *msg = dynamixel_msg_create(7);
msg->buf[0] = 0x1e;
msg->buf[1] = posv & 0xff;
msg->buf[2] = (posv >> 8) & 0xff;
msg->buf[3] = speedv & 0xff;
msg->buf[4] = (speedv >> 8) & 0xff;
msg->buf[5] = torquev & 0xff;
msg->buf[6] = (torquev >> 8) & 0xff;
dynamixel_msg_t *resp = device->write_to_RAM(device, msg, 1);
dynamixel_msg_destroy(msg);
if (resp != NULL);
dynamixel_msg_destroy(resp);
// Handle speed == 0 case (after slowing down, above) by relaying current
// position back to servo. Do not set torque == 0, b/c that is possibly not
// desired...
if (stop) {
msg = dynamixel_msg_create(2);
msg->buf[0] = 0x24;
msg->buf[1] = 2;
resp = device->bus->send_command(device->bus,
device->id,
INST_READ_DATA,
msg,
1);
dynamixel_msg_destroy(msg);
if (resp != NULL) {
dynamixel_msg_destroy(resp);
posv = (resp->buf[1] & 0xff) + ((resp->buf[2] & 0xff) << 8);
msg = dynamixel_msg_create(3);
msg->buf[0] = 0x1e;
msg->buf[1] = posv & 0xff;
msg->buf[2] = (posv > 8) & 0xff;
resp = device->write_to_RAM(device, msg, 1);
}
if (resp != NULL)
dynamixel_msg_destroy(resp);
}
}
QVector<EcgStDescriptor> EcgStAnalyzer::analyze(const EcgStData &data, double sampleFreq)
{
QVector<EcgStDescriptor> result;
int num = data.rData.size();
int snum = data.ecgSamples.size();
if (num < 2 || snum == 0)
return result;
int p = smoothSize;
int w = detectionSize;
double lamdba = morphologyCoeff;
QVector<int> stOn(num);
QVector<int> stEnd(num);
int i;
// detect STend points
for (i = 0; i < num; i++)
{
double ka = data.jData[i];
double kb = data.tEndData[i];
QVector<double> aVal(kb - ka + 1);
for (int k = ka; k <= kb; k++)
{
int ke = std::max(1, std::min(k + p, snum));
int nsr = ke - k + p + 1;
QVector<double> wnd = data.ecgSamples.mid(k - p - 1, nsr);
double sk = EcgUtils::sum(wnd) / nsr;
ke = std::max(1, std::min(k + w - 1, snum));
QVector<double> sqr(ke - k);
for (int j = 0; j < sqr.size(); j++) {
double smp = data.ecgSamples[k + j - 1] - sk;
if (algorithm == ST_QUADRATIC)
sqr[j] = smp * smp;
else
sqr[j] = smp;
}
aVal[k - ka] = EcgUtils::sum(sqr);
}
int kp, kp1, kp2;
double ap1 = EcgUtils::max(aVal, &kp1);
double ap2 = EcgUtils::min(aVal, &kp2);
double at = fabs(ap1) / fabs(ap2);
if ((1.0 / lamdba < at) && (at < lamdba))
kp = std::min(kp1, kp2);
else
kp = fabs(ap1) > fabs(ap2) ? kp1 : kp2;
stEnd[i] = ka + kp;
}
// calculate heart rate
QVector<int> rr = EcgUtils::diff(data.rData);
QVector<double> hr(num);
for (i = 0; i < num - 1; i++)
hr[i] = 60.0 / ((double) rr[i] / sampleFreq);
hr[num - 1] = hr[num - 2];
for (i = 0; i < num; i++)
{
double rt = stEnd[i] - data.rData[i];
double x;
double hrc = hr[i];
if (hrc < 100)
x = 0.4;
else if (hrc < 110)
x = 0.45;
else if (hrc < 120)
x = 0.5;
else
x = 0.55;
int test = (int) round((double) data.rData[i] + x * rt);
stOn[i] = std::min(data.jData[i] + 1, test);
}
// create and classify interval descriptors
for (i = 0; i < num; i++)
{
EcgStDescriptor desc;
desc.STOn = stOn[i];
desc.STEnd = stEnd[i];
desc.STMid = desc.STOn + (int) round((desc.STEnd - desc.STOn) / 2.0);
desc.offset = data.ecgSamples[desc.STMid];
int x1 = desc.STOn;
int x2 = desc.STMid;
double y1 = data.ecgSamples[x1];
double y2 = desc.offset;
double d1 = (y1 - y2) / ((x1 - x2) / sampleFreq);
desc.slope1 = RAD_TO_DEG(atan(d1));
x1 = desc.STMid;
x2 = desc.STEnd;
y1 = desc.offset;
y2 = data.ecgSamples[x2];
double d2 = (y1 - y2) / ((x1 - x2) / sampleFreq);
desc.slope2 = RAD_TO_DEG(atan(d2));
classifyInterval(desc);
result.push_back(desc);
}
return result;
}
double SDFFsequenceElement::extract_real_t_end(){
double ts=emanager->get_sample_timestep();
return round((t_start+t_duration)/ts)*ts;//floor
};
double SDFFsequenceElement::extract_real_t_start(){
double ts=emanager->get_sample_timestep();
return round(t_start/ts)*ts; //ceil
};
void setup_domain (struct geom *g) {
/* - double pass setup; PORE regios determine global
inter-layer spacing dz
*/
int i;
double l_pore=0; /* total length of pore region, center-center */
int q_pore; /* number of gaps between layers */
int q_check, nborders, lastborder;
double dz, dA;
double z1, z2;
enum domaintypes lastdtype;
struct domain *dom;
dA = 0.0; /* distance between two atoms ('bond length') */
q_check = 0; /* internal topology check */
nborders = 0; /* number of interfaces between different domains */
lastdtype = g->domain[0]->type;
/* first pass:
total length of pore region and global dz
also check correct topology: MOUTH -> PORE -> MOUTH
*/
for (i = 0; i < g->ndomains; i++) {
dom = g->domain[i];
if (dom->type != lastdtype) {
nborders++; /* increase nborders for each change M<->P */
lastborder = i - 1; /* last PORE region before second MOUTH */
};
lastdtype = dom->type;
if ( dom->type == PORE) {
l_pore += dom->l;
dA = max(dA, 2 * dom->species->radius);
mesg (SUB2, "setup_domain(): l_pore = %6.3f 2*rA = %4.2f", l_pore, dA);
};
};
/* if nborders != 2 => error!! */
if ( nborders != 2 ) {
fatal_error (2,
"setup_domain (): Fatal error. Topology of model is incorrect. It should\n"
" have been MOUTH->PORE->MOUTH with 2 borders, but there\n"
" were %d borders in %d domains", nborders, g->ndomains);
};
q_pore = (int) l_pore/dA;
if ( q_pore < 1 ) {
fatal_error (2,
"setup_domain (): Fatal error. Number of layers in the PORE is not positive\n"
" (q_pore = %d). (l_pore=%4.2f) < (2*rA=%4.2f)\n",
q_pore, l_pore, dA);
};
dz = l_pore/q_pore;
/* second pass:
number of layers q per domain and z coordinates of lower and upper layer
*/
z2 = -dz;
for (i = 0; i < g->ndomains; i++) {
dom = g->domain[i];
if ( dom->type == PORE)
{
dom->q = round (dom->l / dz);
q_check += dom->q;
dom->dz = dz; /* rather pointless to store dz separately
but I still used the old strucs */
}
else
{
dom->q = round (dom->l / dA);
dom->dz = dz; /* pointless ... */
};
if (dom->q < 0) {
fatal_error (2,
"setup_domain (): Fatal error. Number of layers in domain %d "
"is not positive\n"
" (q = %d). Probably (l=%4.2f) < (2*rA=%4.2f)\n",
i,dom->q, dom->l, dA);
};
z1 = z2 + dz;
z2 = z1 + (dom->q - 1) * dz;
dom->z1 = z1;
dom->z2 = z2;
};
assert (q_check == q_pore);
return;
};
int main(int argc, char*argv[])
{
srand(time(NULL));
// options
unsigned int M = 64; // number of subcarriers
unsigned int cp_len = 16; // cyclic prefix length
unsigned int taper_len = 0; // taper length
unsigned int num_symbols = 1000; // number of data symbols
modulation_scheme ms = LIQUID_MODEM_QPSK;
// get options
int dopt;
while((dopt = getopt(argc,argv,"hM:C:T:N:")) != EOF){
switch (dopt) {
case 'h': usage(); return 0;
case 'M': M = atoi(optarg); break;
case 'C': cp_len = atoi(optarg); break;
case 'T': taper_len = atoi(optarg); break;
case 'N': num_symbols = atoi(optarg); break;
default:
exit(1);
}
}
unsigned int i;
// derived values
unsigned int frame_len = M + cp_len;
// initialize subcarrier allocation
unsigned char p[M];
ofdmframe_init_default_sctype(M, p);
// create frame generator
ofdmframegen fg = ofdmframegen_create(M, cp_len, taper_len, p);
ofdmframegen_print(fg);
modem mod = modem_create(ms);
float complex X[M]; // channelized symbols
float complex buffer[frame_len];// output time series
float * PAPR = (float*) malloc(num_symbols*sizeof(float));
// histogram display
float xmin = -1.29f + 0.70f*log2f(M);
float xmax = 8.97f + 0.34f*log2f(M);
unsigned int num_bins = 30;
float bin_width = (xmax - xmin) / (num_bins);
unsigned int hist[num_bins];
for (i=0; i<num_bins; i++)
hist[i] = 0;
// modulate data symbols
unsigned int j;
unsigned int s;
float PAPR_min = 0.0f;
float PAPR_max = 0.0f;
float PAPR_mean = 0.0f;
for (i=0; i<num_symbols; i++) {
// data symbol
for (j=0; j<M; j++) {
s = modem_gen_rand_sym(mod);
modem_modulate(mod,s,&X[j]);
}
// generate symbol
ofdmframegen_writesymbol(fg, X, buffer);
#if 0
// preamble
if (i==0) ofdmframegen_write_S0a(fg, buffer); // S0 symbol (first)
else if (i==1) ofdmframegen_write_S0b(fg, buffer); // S0 symbol (second)
else if (i==2) ofdmframegen_write_S1( fg, buffer); // S1 symbol
#endif
// compute PAPR
PAPR[i] = ofdmframe_PAPR(buffer, frame_len);
// compute bin index
unsigned int index;
float ihat = num_bins * (PAPR[i] - xmin) / (xmax - xmin);
if (ihat < 0.0f) index = 0;
else index = (unsigned int)ihat;
if (index >= num_bins)
index = num_bins-1;
hist[index]++;
// save min/max PAPR values
if (i==0 || PAPR[i] < PAPR_min) PAPR_min = PAPR[i];
if (i==0 || PAPR[i] > PAPR_max) PAPR_max = PAPR[i];
PAPR_mean += PAPR[i];
}
PAPR_mean /= (float)num_symbols;
// destroy objects
ofdmframegen_destroy(fg);
modem_destroy(mod);
// print results to screen
// find max(hist)
unsigned int hist_max = 0;
for (i=0; i<num_bins; i++)
hist_max = hist[i] > hist_max ? hist[i] : hist_max;
printf("%8s : %6s [%6s]\n", "PAPR[dB]", "count", "prob.");
for (i=0; i<num_bins; i++) {
printf("%8.2f : %6u [%6.4f]", xmin + i*bin_width, hist[i], (float)hist[i] / (float)num_symbols);
unsigned int k;
unsigned int n = round(60 * (float)hist[i] / (float)hist_max);
for (k=0; k<n; k++)
printf("#");
printf("\n");
}
printf("\n");
printf("min PAPR: %12.4f dB\n", PAPR_min);
printf("max PAPR: %12.4f dB\n", PAPR_max);
printf("mean PAPR: %12.4f dB\n", PAPR_mean);
//
// export output file
//
// count subcarrier types
unsigned int M_data = 0;
unsigned int M_pilot = 0;
unsigned int M_null = 0;
ofdmframe_validate_sctype(p, M, &M_null, &M_pilot, &M_data);
FILE * fid = fopen(OUTPUT_FILENAME,"w");
fprintf(fid,"%% %s: auto-generated file\n\n", OUTPUT_FILENAME);
fprintf(fid,"clear all;\n");
fprintf(fid,"close all;\n\n");
fprintf(fid,"M = %u;\n", M);
fprintf(fid,"M_data = %u;\n", M_data);
fprintf(fid,"M_pilot = %u;\n", M_pilot);
fprintf(fid,"M_null = %u;\n", M_null);
fprintf(fid,"cp_len = %u;\n", cp_len);
fprintf(fid,"num_symbols = %u;\n", num_symbols);
fprintf(fid,"PAPR = zeros(1,num_symbols);\n");
for (i=0; i<num_symbols; i++)
fprintf(fid," PAPR(%6u) = %12.4e;\n", i+1, PAPR[i]);
fprintf(fid,"\n");
fprintf(fid,"figure;\n");
fprintf(fid,"hist(PAPR,25);\n");
fprintf(fid,"\n");
fprintf(fid,"figure;\n");
fprintf(fid,"cdf = [(1:num_symbols)-0.5]/num_symbols;\n");
fprintf(fid,"semilogy(sort(PAPR),1-cdf);\n");
fprintf(fid,"xlabel('PAPR [dB]');\n");
fprintf(fid,"ylabel('Complementary CDF');\n");
fclose(fid);
printf("results written to %s\n", OUTPUT_FILENAME);
// free allocated array
free(PAPR);
printf("done.\n");
return 0;
}
int trun(double num, int precision)
{
return round(pow(10,precision)*num)/pow(10,precision);
}
// Assumes lx1 != lx2 || ly1 != ly2.
static bool precise_collision_line(int intersection_left, int intersection_right, int intersection_top, int intersection_bottom,
double x1, double y1,
double xscale1, double yscale1,
double ia1,
unsigned char* pixels1,
int w1, int h1,
int xoffset1, int yoffset1,
int lx1, int ly1, int lx2, int ly2)
{
if (xscale1 != 0.0 && yscale1 != 0.0) {
const double pi_half = M_PI/2.0;
const double arad1 = ia1*M_PI/180.0;
const double cosa1 = cos(-arad1);
const double sina1 = sin(-arad1);
const double cosa90_1 = cos(-arad1 + pi_half);
const double sina90_1 = sin(-arad1 + pi_half);
if (lx1 != lx2 && abs(lx1-lx2) >= abs(ly1-ly2)) { // The slope is defined and in [-1;1].
const int minX = max(min(lx1, lx2), intersection_left),
maxX = min(max(lx1, lx2), intersection_right);
const double denom = lx2 - lx1;
for (int gx = minX; gx <= maxX; gx++)
{
int gy = (int)round((gx - lx1)*(ly2-ly1)/denom + ly1);
if (gy < intersection_top || gy > intersection_bottom) {
continue;
}
// Test for single image.
const int bx1 = (gx - x1);
const int by1 = (gy - y1);
const int px1 = (int)((bx1*cosa1 + by1*sina1)/xscale1 + xoffset1);
const int py1 = (int)((bx1*cosa90_1 + by1*sina90_1)/yscale1 + yoffset1);
const bool p1 = px1 >= 0 && py1 >= 0 && px1 < w1 && py1 < h1 && pixels1[py1*w1 + px1] != 0;
if (p1) {
return true;
}
}
}
else { // ly1 != ly2.
const int minY = max(min(ly1, ly2), intersection_top),
maxY = min(max(ly1, ly2), intersection_bottom);
const double denom = ly2 - ly1;
for (int gy = minY; gy <= maxY; gy++)
{
int gx = (int)round((gy - ly1)*(lx2-lx1)/denom + lx1);
if (gx < intersection_left || gx > intersection_right) {
continue;
}
// Test for single image.
const int bx1 = (gx - x1);
const int by1 = (gy - y1);
const int px1 = (int)((bx1*cosa1 + by1*sina1)/xscale1 + xoffset1);
const int py1 = (int)((bx1*cosa90_1 + by1*sina90_1)/yscale1 + yoffset1);
const bool p1 = px1 >= 0 && py1 >= 0 && px1 < w1 && py1 < h1 && pixels1[py1*w1 + px1] != 0;
if (p1) {
return true;
}
}
}
}
return false;
}
void LslidarN301Decoder::publishScan()
{
sensor_msgs::LaserScan::Ptr scan(new sensor_msgs::LaserScan);
if(sweep_data->scans[0].points.size() <= 1)
return;
scan->header.frame_id = child_frame_id;
scan->angle_max = 0.0;
scan->angle_min = 2.0*M_PI;
scan->angle_increment = (scan->angle_max-scan->angle_min)/3600;
// scan->time_increment = motor_speed_/1e8;
scan->range_min = 0.05;
scan->range_max = 100.0;
scan->ranges.reserve(3600);
scan->intensities.reserve(3600);
std::vector<point_struct> mean_points[3600] = {};
point_struct temp_point;
for(uint16_t i = 0; i < sweep_data->scans[0].points.size(); i++)
{
int degree = round(sweep_data->scans[0].points[i].azimuth * RAD_TO_DEG * 10);
//ROS_INFO("degree = %d", degree);
if (degree >= 3600) degree = 0;
if (degree < 0) degree = 3599;
if((degree<(1800+laser_number))&&(degree>(1800-laser_number)))
{
//ROS_INFO("degree = %d", degree);
temp_point.distance=std::numeric_limits<float>::infinity();
temp_point.intensity = 0;
}
else
{
temp_point.distance = sweep_data->scans[0].points[i].distance;
temp_point.intensity = sweep_data->scans[0].points[i].intensity;
}
mean_points[degree].push_back(temp_point);
}
// calc mean
point_struct mean_point;
for(uint16_t i = 0; i < 3600; i++)
{
// ROS_INFO("%f", sweep_data->scans[0].points[i].azimuth);
if ( mean_points[i].size() == 0)
{
scan->ranges.push_back(0);
scan->intensities.push_back(0);
continue;
}
mean_point = getMeans(mean_points[i]);
scan->ranges.push_back(mean_point.distance);
scan->intensities.push_back(mean_point.intensity);
}
scan->header.stamp = ros::Time::now();
scan_pub.publish(scan);
}
static void volume_changed (GObject * object)
{
double vol;
g_object_get (object, "volume", & vol, NULL);
aud_drct_set_volume_main (round (vol * 100));
}
void toBarChart::paintChart(QPainter *p, QRect &rect)
{
QFontMetrics fm = p->fontMetrics();
if (!Zooming)
{
if (MinAuto)
{
bool first = true;
std::list<std::list<double> >::reverse_iterator i = Values.rbegin();
if (i != Values.rend())
{
for (std::list<double>::iterator j = (*i).begin(); j != (*i).end(); j++)
{
if (first)
{
first = false;
zMinValue = *j;
}
else if (zMinValue > *j)
zMinValue = *j;
}
}
}
if (MaxAuto)
{
bool first = true;
std::list<double> total;
std::list<bool>::iterator e = Enabled.begin();
{
for (std::list<std::list<double> >::iterator i = Values.begin(); i != Values.end(); i++)
{
std::list<double>::iterator k = total.begin();
if (e == Enabled.end() || *e)
{
for (std::list<double>::iterator j = (*i).begin(); j != (*i).end(); j++)
{
if (k == total.end())
{
total.insert(total.end(), *j);
k = total.end();
}
else
{
*k += *j;
k++;
}
}
}
if (e != Enabled.end())
e++;
}
}
for (std::list<double>::iterator i = total.begin(); i != total.end(); i++)
{
if (first)
{
first = false;
zMaxValue = *i;
}
else if (zMaxValue < *i)
zMaxValue = *i;
}
}
if (!MinAuto)
zMinValue = MinValue;
else
{
zMinValue = round(zMinValue, false);
MinValue = zMinValue;
}
if (!MaxAuto)
zMaxValue = MaxValue;
else
{
zMaxValue = round(zMaxValue, true);
MaxValue = zMaxValue;
}
}
paintTitle(p, rect);
paintLegend(p, rect);
paintAxis(p, rect);
std::list<QPolygon> Points;
int cp = 0;
int samples = countSamples();
int zeroy = int(rect.height() - 2 - ( -zMinValue / (zMaxValue - zMinValue) * (rect.height() - 4)));
if (samples > 1)
{
const QMatrix &mtx = p->worldMatrix();
p->setClipRect(int(mtx.dx() + 2), int(mtx.dy() + 2), rect.width() - 3, rect.height() - 3);
if (Zooming)
p->drawText(2, 2, rect.width() - 4, rect.height() - 4,
Qt::AlignLeft | Qt::AlignTop, tr("Zoom"));
std::list<bool>::reverse_iterator e = Enabled.rbegin();
for (std::list<std::list<double> >::reverse_iterator i = Values.rbegin(); i != Values.rend(); i++)
{
if (e == Enabled.rend() || *e)
{
std::list<double> &val = *i;
int count = 0;
int skip = SkipSamples;
QPolygon a(samples + 10);
int x = rect.width() - 2;
for (std::list<double>::reverse_iterator j = val.rbegin(); j != val.rend() && x >=
2;
j++)
{
if (skip > 0)
skip--;
else
{
int val = int(rect.height() - 2 - ((*j - zMinValue) / (zMaxValue - zMinValue) * (rect.height() - 4)));
x = rect.width() - 2 - count * (rect.width() - 4) / (samples - 1);
a.setPoint(count, x, val);
count++;
if (count >= samples)
break;
}
}
a.resize(count * 2);
Points.insert(Points.end(), a);
}
cp++;
if (e != Enabled.rend())
e++;
}
}
std::map<int, int> Bottom;
std::list<bool>::reverse_iterator e = Enabled.rbegin();
for (std::list<QPolygon>::iterator i = Points.begin(); i != Points.end();)
{
while (e != Enabled.rend() && !*e)
{
cp--;
e++;
}
if (e != Enabled.rend())
e++;
cp--;
QPolygon a = *i;
int lx = 0;
int lb = 0;
for (int j = 0; j < a.size() / 2; j++)
{
int x, y;
a.point(j, &x, &y);
if (Bottom.find(x) == Bottom.end())
Bottom[x] = 0;
if (lx != x)
lb = Bottom[x];
a.setPoint(a.size() - 1 - j, x, zeroy - lb);
y -= lb;
a.setPoint(j, x, y);
Bottom[x] = zeroy - y;
lx = x;
}
p->save();
QBrush brush(Utils::toChartBrush(cp));
p->setBrush(brush.color());
p->drawPolygon(a);
if (brush.style() != Qt::SolidPattern)
{
p->setBrush(QBrush(Qt::white, brush.style()));
p->drawPolygon(a);
}
p->restore();
i++;
}
}
/* From R, currently only used for kode = 1, m = 1, n in {0,1,2,3} : */
void dpsifn(double x, int n, int kode, int m, double *ans, int *nz, int *ierr)
{
const static double bvalues[] = { /* Bernoulli Numbers */
1.00000000000000000e+00,
-5.00000000000000000e-01,
1.66666666666666667e-01,
-3.33333333333333333e-02,
2.38095238095238095e-02,
-3.33333333333333333e-02,
7.57575757575757576e-02,
-2.53113553113553114e-01,
1.16666666666666667e+00,
-7.09215686274509804e+00,
5.49711779448621554e+01,
-5.29124242424242424e+02,
6.19212318840579710e+03,
-8.65802531135531136e+04,
1.42551716666666667e+06,
-2.72982310678160920e+07,
6.01580873900642368e+08,
-1.51163157670921569e+10,
4.29614643061166667e+11,
-1.37116552050883328e+13,
4.88332318973593167e+14,
-1.92965793419400681e+16
};
int i, j, k, mm, mx, nn, np, nx, fn;
double arg, den, elim, eps, fln, fx, rln, rxsq,
r1m4, r1m5, s, slope, t, ta, tk, tol, tols, tss, tst,
tt, t1, t2, wdtol, xdmln, xdmy, xinc, xln = 0.0 /* -Wall */,
xm, xmin, xq, yint;
double trm[23], trmr[n_max + 1];
*ierr = 0;
if (n < 0 || kode < 1 || kode > 2 || m < 1) {
*ierr = 1;
return;
}
if (x <= 0.) {
/* use Abramowitz & Stegun 6.4.7 "Reflection Formula"
* psi(k, x) = (-1)^k psi(k, 1-x) - pi^{n+1} (d/dx)^n cot(x)
*/
if (x == round(x)) {
/* non-positive integer : +Inf or NaN depends on n */
for(j=0; j < m; j++) /* k = j + n : */
ans[j] = ((j+n) % 2) ? ML_POSINF : ML_NAN;
return;
}
/* This could cancel badly */
dpsifn(1. - x, n, /*kode = */ 1, m, ans, nz, ierr);
/* ans[j] == (-1)^(k+1) / gamma(k+1) * psi(k, 1 - x)
* for j = 0:(m-1) , k = n + j
*/
/* Cheat for now: only work for m = 1, n in {0,1,2,3} : */
if(m > 1 || n > 3) {/* doesn't happen for digamma() .. pentagamma() */
/* not yet implemented */
*ierr = 4; return;
}
x *= M_PI; /* pi * x */
if (n == 0)
tt = cos(x)/sin(x);
else if (n == 1)
tt = -1/JR_pow_di(sin(x), 2);
else if (n == 2)
tt = 2*cos(x)/JR_pow_di(sin(x), 3);
else if (n == 3)
tt = -2*(2*JR_pow_di(cos(x), 2) + 1.)/JR_pow_di(sin(x), 4);
else /* can not happen! */
tt = ML_NAN;
/* end cheat */
s = (n % 2) ? -1. : 1.;/* s = (-1)^n */
/* t := pi^(n+1) * d_n(x) / gamma(n+1) , where
* d_n(x) := (d/dx)^n cot(x)*/
t1 = t2 = s = 1.;
for(k=0, j=k-n; j < m; k++, j++, s = -s) {
/* k == n+j , s = (-1)^k */
t1 *= M_PI;/* t1 == pi^(k+1) */
if(k >= 2)
t2 *= k;/* t2 == k! == gamma(k+1) */
if(j >= 0) /* by cheat above, tt === d_k(x) */
ans[j] = s*(ans[j] + t1/t2 * tt);
}
if (n == 0 && kode == 2) /* unused from R, but "wrong": xln === 0 :*/
ans[0] += xln;
return;
} /* x <= 0 */
/* else : x > 0 */
*nz = 0;
xln = log(x);
if(kode == 1 && m == 1) {/* the R case --- for very large x: */
double lrg = 1/(2. * DBL_EPSILON);
if(n == 0 && x * xln > lrg) {
ans[0] = -xln;
return;
}
else if(n >= 1 && x > n * lrg) {
ans[0] = exp(-n * xln)/n; /* == x^-n / n == 1/(n * x^n) */
return;
}
}
mm = m;
nx = imin2(-jags_i1mach(15), jags_i1mach(16));/* = 1021 */
r1m5 = jags_d1mach(5);
r1m4 = jags_d1mach(4) * 0.5;
wdtol = fmax2(r1m4, 0.5e-18); /* 1.11e-16 */
/* elim = approximate exponential over and underflow limit */
elim = 2.302 * (nx * r1m5 - 3.0);/* = 700.6174... */
for(;;) {
nn = n + mm - 1;
fn = nn;
t = (fn + 1) * xln;
/* overflow and underflow test for small and large x */
if (fabs(t) > elim) {
if (t <= 0.0) {
*nz = 0;
*ierr = 2;
return;
}
}
else {
if (x < wdtol) {
ans[0] = JR_pow_di(x, -n-1);
if (mm != 1) {
for(k = 1; k < mm ; k++)
ans[k] = ans[k-1] / x;
}
if (n == 0 && kode == 2)
ans[0] += xln;
return;
}
/* compute xmin and the number of terms of the series, fln+1 */
rln = r1m5 * jags_i1mach(14);
rln = fmin2(rln, 18.06);
fln = fmax2(rln, 3.0) - 3.0;
yint = 3.50 + 0.40 * fln;
slope = 0.21 + fln * (0.0006038 * fln + 0.008677);
xm = yint + slope * fn;
mx = (int)xm + 1;
xmin = mx;
if (n != 0) {
xm = -2.302 * rln - fmin2(0.0, xln);
arg = xm / n;
arg = fmin2(0.0, arg);
eps = exp(arg);
xm = 1.0 - eps;
if (fabs(arg) < 1.0e-3)
xm = -arg;
fln = x * xm / eps;
xm = xmin - x;
if (xm > 7.0 && fln < 15.0)
break;
}
xdmy = x;
xdmln = xln;
xinc = 0.0;
if (x < xmin) {
nx = (int)x;
xinc = xmin - nx;
xdmy = x + xinc;
xdmln = log(xdmy);
}
/* generate w(n+mm-1, x) by the asymptotic expansion */
t = fn * xdmln;
t1 = xdmln + xdmln;
t2 = t + xdmln;
tk = fmax2(fabs(t), fmax2(fabs(t1), fabs(t2)));
if (tk <= elim) /* for all but large x */
goto L10;
}
nz++; /* underflow */
mm--;
ans[mm] = 0.;
if (mm == 0)
return;
} /* end{for()} */
nn = (int)fln + 1;
np = n + 1;
t1 = (n + 1) * xln;
t = exp(-t1);
s = t;
den = x;
for(i=1; i <= nn; i++) {
den += 1.;
trm[i] = pow(den, (double)-np);
s += trm[i];
}
ans[0] = s;
if (n == 0 && kode == 2)
ans[0] = s + xln;
if (mm != 1) { /* generate higher derivatives, j > n */
tol = wdtol / 5.0;
for(j = 1; j < mm; j++) {
t /= x;
s = t;
tols = t * tol;
den = x;
for(i=1; i <= nn; i++) {
den += 1.;
trm[i] /= den;
s += trm[i];
if (trm[i] < tols)
break;
}
ans[j] = s;
}
}
return;
L10:
tss = exp(-t);
tt = 0.5 / xdmy;
t1 = tt;
tst = wdtol * tt;
if (nn != 0)
t1 = tt + 1.0 / fn;
rxsq = 1.0 / (xdmy * xdmy);
ta = 0.5 * rxsq;
t = (fn + 1) * ta;
s = t * bvalues[2];
if (fabs(s) >= tst) {
tk = 2.0;
for(k = 4; k <= 22; k++) {
t = t * ((tk + fn + 1)/(tk + 1.0))*((tk + fn)/(tk + 2.0)) * rxsq;
trm[k] = t * bvalues[k-1];
if (fabs(trm[k]) < tst)
break;
s += trm[k];
tk += 2.;
}
}
s = (s + t1) * tss;
if (xinc != 0.0) {
/* backward recur from xdmy to x */
nx = (int)xinc;
np = nn + 1;
if (nx > n_max) {
*nz = 0;
*ierr = 3;
return;
}
else {
if (nn==0)
goto L20;
xm = xinc - 1.0;
fx = x + xm;
/* this loop should not be changed. fx is accurate when x is small */
for(i = 1; i <= nx; i++) {
trmr[i] = pow(fx, (double)-np);
s += trmr[i];
xm -= 1.;
fx = x + xm;
}
}
}
ans[mm-1] = s;
if (fn == 0)
goto L30;
/* generate lower derivatives, j < n+mm-1 */
for(j = 2; j <= mm; j++) {
fn--;
tss *= xdmy;
t1 = tt;
if (fn!=0)
t1 = tt + 1.0 / fn;
t = (fn + 1) * ta;
s = t * bvalues[2];
if (fabs(s) >= tst) {
tk = 4 + fn;
for(k=4; k <= 22; k++) {
trm[k] = trm[k] * (fn + 1) / tk;
if (fabs(trm[k]) < tst)
break;
s += trm[k];
tk += 2.;
}
}
s = (s + t1) * tss;
if (xinc != 0.0) {
if (fn == 0)
goto L20;
xm = xinc - 1.0;
fx = x + xm;
for(i=1 ; i<=nx ; i++) {
trmr[i] = trmr[i] * fx;
s += trmr[i];
xm -= 1.;
fx = x + xm;
}
}
ans[mm - j] = s;
if (fn == 0)
goto L30;
}
return;
L20:
for(i = 1; i <= nx; i++)
s += 1. / (x + (nx - i)); /* avoid disastrous cancellation, PR#13714 */
L30:
if (kode != 2) /* always */
ans[0] = s - xdmln;
else if (xdmy != x) {
xq = xdmy / x;
ans[0] = s - log(xq);
}
return;
} /* dpsifn() */
// Draws a timescale (physical). Note this can fail if the span is short
// enough which can happen when we find no events.
QString VisWidget::drawTimescale(QPainter * painter, unsigned long long start,
unsigned long long span, int margin)
{
// Draw the scale bar
int lineHeight = rect().height() - (timescaleHeight - 1);
painter->setPen(QPen(Qt::black, 1, Qt::SolidLine));
painter->drawLine(margin, lineHeight, rect().width() - margin, lineHeight);
if (!visProcessed)
return "";
painter->setFont(QFont("Helvetica", 10));
QFontMetrics font_metrics = this->fontMetrics();
// Figure out the units and number of ticks ( may handle units later )
QLocale systemlocale = QLocale::system();
QString text = systemlocale.toString(start);
int textWidth = font_metrics.width(text) * 1.4;
// We can't have more than this and fit text
int max_ticks = floor((rect().width() - 2*margin) / 1.0 / textWidth);
int y = lineHeight + timescaleTickHeight + font_metrics.xHeight() + 4;
// We want a round number
unsigned long long tick_span = span / max_ticks; // Not round
int power = floor(log10(tick_span)); // How many zeros
unsigned long long roundfactor = std::max(1.0, pow(10, power));
tick_span = (tick_span / roundfactor) * roundfactor; // Now round
if (tick_span < 1)
tick_span = 1;
// Now we must find the first number after startTime divisible by
// the tick_span. We don't want to just find the same roundness
// because then panning doesn't work.
unsigned long long tick = (tick_span - start % tick_span) + start;
// TODO: MAKE THIS PART OPTIONAL
QString seconds = getUnits(trace->units);
int tick_divisor = 1;
unsigned long long tick_base = 0;
if (!options->absoluteTime)
{
tick_base = tick - tick_span;
int span_unit = (int) floor(log10(tick_span));
tick_divisor = pow(3 * floor(span_unit / 3), 10);
if (!tick_divisor)
tick_divisor = 1;
seconds = systemlocale.toString(tick_base
/ pow((double) trace->units, 10),
'f', trace->units - span_unit)
+ "s + "
+ getUnits(trace->units
- 3 * floor(span_unit / 3))
+ ":";
}
// And now we draw
while (tick < start + span)
{
int x = margin + round((tick - start) / 1.0 / span
* (rect().width() - 2*margin));
painter->drawLine(x, lineHeight, x, lineHeight + timescaleTickHeight);
text = systemlocale.toString((tick - tick_base) / tick_divisor);
textWidth = font_metrics.width(text) / 3;
painter->drawText(x - textWidth, y, text);
tick += tick_span;
}
return seconds;
}
void gravity_fft_p2grid(){
// clean the current density
for(int i = 0 ; i < root_nx * (root_ny + 2) ; i++) {
density_r[i] = 0.0; // density is used to store the surface density
}
for (int i=0; i<N; i++){
struct particle p = particles[i];
// I'm sorry to say I have to keep these traps. Something's wrong if these traps are called.
int x = (int) floor((p.x / boxsize_x + 0.5) * root_nx);
int y = (int) floor((p.y / boxsize_y + 0.5) * root_ny);
// Formally, pos.x is in the interval [-size/2 , size/2 [. Therefore, x and y should be in [0 , grid_NJ-1]
// xp1, xm1... might be out of bound. They are however the relevant coordinates for the interpolation.
int xp1 = x + 1;
int xm1 = x - 1;
int ym1 = y - 1;
int yp1 = y + 1;
// Target according to boundary conditions.
// Although xTarget and yTarget are not relevant here, they will be relevant with shear
// We have to use all these fancy variables since y depends on x because of the shearing path
// Any nicer solution is welcome
int xTarget = x;
int xp1Target = xp1;
int xm1Target = xm1;
int ym1_xm1Target = (ym1 + root_ny) % root_ny;
int ym1_xTarget = ym1_xm1Target;
int ym1_xp1Target = ym1_xm1Target;
int y_xm1Target = y % root_ny;
int y_xTarget = y_xm1Target;
int y_xp1Target = y_xm1Target;
int yp1_xm1Target = yp1 % root_ny;
int yp1_xTarget = yp1_xm1Target;
int yp1_xp1Target = yp1_xm1Target;
double tx, ty;
double q0 = G* p.m /(dx*dy);
// Shearing patch trick
// This is only an **approximate** mapping
// one should use an exact interpolation scheme here (Fourier like).
if(xp1Target>=root_nx) {
xp1Target -= root_nx; // X periodicity
y_xp1Target = y_xp1Target + round((shift_shear/boxsize_y) * root_ny);
y_xp1Target = (y_xp1Target + root_ny) % root_ny; // Y periodicity
yp1_xp1Target = yp1_xp1Target + round((shift_shear/boxsize_y) * root_ny);
yp1_xp1Target = (yp1_xp1Target + root_ny) % root_ny;
ym1_xp1Target = ym1_xp1Target + round((shift_shear/boxsize_y) * root_ny);
ym1_xp1Target = (ym1_xp1Target + root_ny) % root_ny;
}
if(xm1Target<0) {
xm1Target += root_nx;
y_xm1Target = y_xm1Target - round((shift_shear/boxsize_x) * root_ny);
y_xm1Target = (y_xm1Target + root_ny) % root_ny; // Y periodicity
yp1_xm1Target = yp1_xm1Target - round((shift_shear/boxsize_x) * root_ny);
yp1_xm1Target = (yp1_xm1Target + root_ny) % root_ny;
ym1_xm1Target = ym1_xm1Target - round((shift_shear/boxsize_x) * root_ny);
ym1_xm1Target = (ym1_xm1Target + root_ny) % root_ny;
}
// Distribute density to the 9 nearest cells
tx = ((double)xm1 +0.5) * boxsize_x / root_nx -0.5*boxsize_x - p.x;
ty = ((double)ym1 +0.5) * boxsize_y / root_ny -0.5*boxsize_y - p.y;
density_r[(root_ny+2) * xm1Target + ym1_xm1Target] += q0 * W(tx/dx)*W(ty/dy);
tx = ((double)x +0.5) * boxsize_x / root_nx -0.5*boxsize_x - p.x;
ty = ((double)ym1 +0.5) * boxsize_y / root_ny -0.5*boxsize_y - p.y;
density_r[(root_ny+2) * xTarget + ym1_xTarget] += q0 * W(tx/dx)*W(ty/dy);
tx = ((double)xp1 +0.5) * boxsize_x / root_nx -0.5*boxsize_x - p.x;
ty = ((double)ym1 +0.5) * boxsize_y / root_ny -0.5*boxsize_y - p.y;
density_r[(root_ny+2) * xp1Target + ym1_xp1Target] += q0 * W(tx/dx)*W(ty/dy);
tx = ((double)xm1 +0.5) * boxsize_x / root_nx -0.5*boxsize_x - p.x;
ty = ((double)y +0.5) * boxsize_y / root_ny -0.5*boxsize_y - p.y;
density_r[(root_ny+2) * xm1Target + y_xm1Target ] += q0 * W(tx/dx)*W(ty/dy);
tx = ((double)x +0.5) * boxsize_x / root_nx -0.5*boxsize_x - p.x;
ty = ((double)y +0.5) * boxsize_y / root_ny -0.5*boxsize_y - p.y;
density_r[(root_ny+2) * xTarget + y_xTarget ] += q0 * W(tx/dx)*W(ty/dy);
tx = ((double)xp1 +0.5) * boxsize_x / root_nx -0.5*boxsize_x - p.x;
ty = ((double)y +0.5) * boxsize_y / root_ny -0.5*boxsize_y - p.y;
density_r[(root_ny+2) * xp1Target + y_xp1Target ] += q0 * W(tx/dx)*W(ty/dy);
tx = ((double)xm1 +0.5) * boxsize_x / root_nx -0.5*boxsize_x - p.x;
ty = ((double)yp1 +0.5) * boxsize_y / root_ny -0.5*boxsize_y - p.y;
density_r[(root_ny+2) * xm1Target + yp1_xm1Target] += q0 * W(tx/dx)*W(ty/dy);
tx = ((double)x +0.5) * boxsize_x / root_nx -0.5*boxsize_x - p.x;
ty = ((double)yp1 +0.5) * boxsize_y / root_ny -0.5*boxsize_y - p.y;
density_r[(root_ny+2) * xTarget + yp1_xTarget] += q0 * W(tx/dx)*W(ty/dy);
tx = ((double)xp1 +0.5) * boxsize_x / root_nx -0.5*boxsize_x - p.x;
ty = ((double)yp1 +0.5) * boxsize_y / root_ny -0.5*boxsize_y - p.y;
density_r[(root_ny+2) * xp1Target + yp1_xp1Target] += q0 * W(tx/dx)*W(ty/dy);
}
}
void pnorm(struct pnorm_state_t *state, uint16_t length, struct complex_sample *in,
struct complex_sample *out, float *ests, float *gains) {
int i ;
float power;
int32_t temp;
float gain, est;
for( i = 0 ; i < length ; i++ ) {
/* Power IIR filter */
if( state->hold == false ) {
//New estimate of power (normalized)
est = state->est = state->alpha*state->est +
state->invalpha*(in[i].i*in[i].i + in[i].q*in[i].q)/(SAMP_MAX_ABS*SAMP_MAX_ABS);
} else {
est = state->est ;
}
/* Ideal power is 1.0, so to get x to 1.0, we need to multiply by 1/est */
gain = 1.0f/sqrtf(state->est) ;
/* Check below min gain */
if( gain < state->min_gain ) {
gain = state->min_gain ;
}
/* Check above max gain */
if( gain > state->max_gain ) {
gain = state->max_gain ;
}
/* Apply gain */
//Use temp int32_t in case this number goes outside 16bit range
temp = (int32_t) round(in[i].i * gain);
//Clamp
if (temp > CLAMP_VAL_ABS){
temp = CLAMP_VAL_ABS;
}else if (temp < -CLAMP_VAL_ABS){
temp = -CLAMP_VAL_ABS;
}
out[i].i = (int16_t) temp;
temp = (int32_t) round(in[i].q * gain);
//Clamp
if (temp > CLAMP_VAL_ABS){
temp = CLAMP_VAL_ABS;
}else if (temp < -CLAMP_VAL_ABS){
temp = -CLAMP_VAL_ABS;
}
out[i].q = (int16_t) temp;
/* Blank impulse power */
power = (out[i].i * out[i].i + out[i].q * out[i].q)/(float)(SAMP_MAX_ABS*SAMP_MAX_ABS);
if (power >= 10.0f) {
out[i].i = out[i].q = 0;
}
//Write to debug buffers
if (ests != NULL){
ests[i] = est;
}
if (gains != NULL){
gains[i] = gain;
}
}
return ;
}
void Decod_ld8kD(
Word16 parm[], /* (i) : vector of synthesis parameters
parm[0] = bad frame indicator (bfi) */
Word16 voicing, /* (i) : voicing decision from previous frame */
Word16 synth[], /* (o) : synthesis speech */
Word16 A_t[], /* (o) : decoded LP filter in 2 subframes */
Word16 *T0_first /* (o) : decoded pitch lag in first subframe */
)
{
Word16 *Az; /* Pointer on A_t */
Word16 lsp_new[M]; /* LSPs */
Word16 code[L_SUBFR]; /* ACELP codevector */
/* Scalars */
Word16 i, j, i_subfr;
Word16 T0, T0_frac, index;
Word16 bfi;
Word32 L_temp;
Word16 g_p, g_c; /* fixed and adaptive codebook gain */
Word16 bad_pitch; /* bad pitch indicator */
extern Flag Overflow;
/* Test bad frame indicator (bfi) */
bfi = *parm++;
/* Decode the LSPs */
D_lsp(parm, lsp_new, bfi);
parm += 2;
/* Interpolation of LPC for the 2 subframes */
Int_qlpc(lsp_old, lsp_new, A_t);
/* update the LSFs for the next frame */
Copy(lsp_new, lsp_old, M);
/*------------------------------------------------------------------------*
* Loop for every subframe in the analysis frame *
*------------------------------------------------------------------------*
* The subframe size is L_SUBFR and the loop is repeated L_FRAME/L_SUBFR *
* times *
* - decode the pitch delay *
* - decode algebraic code *
* - decode pitch and codebook gains *
* - find the excitation and compute synthesis speech *
*------------------------------------------------------------------------*/
Az = A_t; /* pointer to interpolated LPC parameters */
for (i_subfr = 0; i_subfr < L_FRAME; i_subfr += L_SUBFR)
{
index = *parm++; /* pitch index */
if(i_subfr == 0)
{
if (sub(CODEC_MODE, 1) == 0) {
i = 0;
}
else if (sub(CODEC_MODE, 2) == 0) {
i = *parm++; /* get parity check result */
}
else {
fprintf(stderr, "CODEC mode invalid\n");
exit(-1);
}
bad_pitch = add(bfi, i);
if( bad_pitch == 0)
{
Dec_lag3D(index, PIT_MIN, PIT_MAX, i_subfr, &T0, &T0_frac);
old_T0 = T0;
}
else /* Bad frame, or parity error */
{
T0 = old_T0;
T0_frac = 0;
old_T0 = add( old_T0, 1);
if( sub(old_T0, PIT_MAX) > 0) {
old_T0 = PIT_MAX;
}
}
*T0_first = T0; /* If first frame */
}
else /* second subframe */
{
if( bfi == 0)
{
Dec_lag3D(index, PIT_MIN, PIT_MAX, i_subfr, &T0, &T0_frac);
old_T0 = T0;
}
else
{
T0 = old_T0;
T0_frac = 0;
old_T0 = add( old_T0, 1);
if( sub(old_T0, PIT_MAX) > 0) {
old_T0 = PIT_MAX;
}
}
}
/*-------------------------------------------------*
* - Find the adaptive codebook vector. *
*-------------------------------------------------*/
Pred_lt_3(&exc[i_subfr], T0, T0_frac, L_SUBFR);
/*-------------------------------------------------------*
* - Decode innovative codebook. *
* - Add the fixed-gain pitch contribution to code[]. *
*-------------------------------------------------------*/
if (sub(CODEC_MODE, 1) == 0) {
if(bfi != 0) { /* Bad frame */
parm[0] = Random() & (Word16)0x01ff; /* 9 bits random */
parm[1] = Random() & (Word16)0x0003; /* 2 bits random */
}
Decod_ACELP64(parm[1], parm[0], code);
}
else if (sub(CODEC_MODE, 2) == 0) {
if(bfi != 0) { /* Bad frame */
parm[0] = Random() & (Word16)0x1fff; /* 13 bits random */
parm[1] = Random() & (Word16)0x000f; /* 4 bits random */
}
Decod_ACELP(parm[1], parm[0], code);
}
else {
fprintf(stderr, "CODEC mode invalid\n");
exit(-1);
}
parm +=2;
j = shl(sharp, 1); /* From Q14 to Q15 */
if(sub(T0, L_SUBFR) <0 ) {
for (i = T0; i < L_SUBFR; i++) {
code[i] = add(code[i], mult(code[i-T0], j));
}
}
/*-------------------------------------------------*
* - Decode pitch and codebook gains. *
*-------------------------------------------------*/
index = *parm++; /* index of energy VQ */
if (sub(CODEC_MODE, 1) == 0) {
Dec_gain_6k(index, code, L_SUBFR, bfi, &gain_pitch, &gain_code, past_qua_en);
}
else if (sub(CODEC_MODE, 2) == 0) {
Dec_gain_8k(index, code, L_SUBFR, bfi, &gain_pitch, &gain_code, past_qua_en);
}
else {
fprintf(stderr, "CODEC mode invalid\n");
exit(-1);
}
/*-------------------------------------------------------------*
* - Update pitch sharpening "sharp" with quantized gain_pitch *
*-------------------------------------------------------------*/
sharp = gain_pitch;
if (sub(sharp, SHARPMAX) > 0) { sharp = SHARPMAX; }
if (sub(sharp, SHARPMIN) < 0) { sharp = SHARPMIN; }
/*-------------------------------------------------------*
* - Find the total excitation. *
* - Find synthesis speech corresponding to exc[]. *
*-------------------------------------------------------*/
if(bfi != 0) /* Bad frame */
{
if (voicing == 0 ) {
g_p = 0;
g_c = gain_code;
} else {
g_p = gain_pitch;
g_c = 0;
}
} else {
g_p = gain_pitch;
g_c = gain_code;
}
for (i = 0; i < L_SUBFR; i++)
{
/* exc[i] = g_p*exc[i] + g_c*code[i]; */
/* exc[i] in Q0 g_p in Q14 */
/* code[i] in Q13 g_code in Q1 */
L_temp = L_mult(exc[i+i_subfr], g_p);
L_temp = L_mac(L_temp, code[i], g_c);
L_temp = L_shl(L_temp, 1);
exc[i+i_subfr] = round(L_temp);
}
/*-------------------------------------------------*
* Updates state machine for phase dispersion in *
* 6.4 kbps mode, if running in 8.0 kbps mode. *
*-------------------------------------------------*/
if (sub(CODEC_MODE, 2) == 0) {
Update64(gain_pitch, gain_code);
}
/* Test whether synthesis yields overflow or not */
Overflow = 0;
Syn_filt(Az, &exc[i_subfr], &synth[i_subfr], L_SUBFR, mem_syn, 0);
/* In case of overflow in the synthesis */
/* -> Scale down vector exc[] and redo synthesis */
if(Overflow != 0)
{
for(i=0; i<PIT_MAX+L_INTERPOL+L_FRAME; i++)
old_exc[i] = shr(old_exc[i], 2);
}
if (sub(CODEC_MODE, 1) == 0) {
Syn_filt64(Az, &exc[i_subfr], &synth[i_subfr], L_SUBFR, mem_syn, 0,
gain_code, gain_pitch, code );
}
else {
Syn_filt(Az, &exc[i_subfr], &synth[i_subfr], L_SUBFR, mem_syn, 0);
}
/* Update of synthesis filter memory */
Copy(&synth[i_subfr+L_SUBFR-M], mem_syn, M);
Az += MP1; /* interpolated LPC parameters for next subframe */
}
/*--------------------------------------------------*
* Update signal for next frame. *
* -> shift to the left by L_FRAME exc[] *
*--------------------------------------------------*/
Copy(&old_exc[L_FRAME], &old_exc[0], PIT_MAX+L_INTERPOL);
return;
}
void ofApp::setup() {
ofSetLogLevel(OF_LOG_NOTICE);
ofLogNotice() << "setup()";
if (ofApp::isSemibreve()) ofLogNotice() << "Going to run Semibreve version";
if (ofApp::isOsx()) ofLogNotice() << "OSX detected";
if (ofApp::isIos()) ofLogNotice() << "iOS detected";
if (ofApp::isAndroid()) ofLogNotice() << "Android detected";
if (ofApp::isPhone()) ofLogNotice() << "Phone detected";
if (ofApp::isTablet()) ofLogNotice() << "Tablet detected";
// if (ofApp::isIphone()) ofLogNotice() << "iPhone detected";
// if (ofApp::isIpad()) ofLogNotice() << "iPad detected";
// if (ofApp::isAndroidPhone()) ofLogNotice() << "Android phone detected";
// if (ofApp::isAndroidTablet()) ofLogNotice() << "Android tablet detected";
#if defined TARGET_OSX
ofLogNotice() << "Running OSX version";
ofSetDataPathRoot("../Resources/data/");
#endif
#if defined TARGET_SEMIBREVE
ofLogNotice() << "Running SEMIBREVE version";
oscReceiver.setup(RECEIVE_PORT);
oscSender.setup(HOST, SEND_PORT);
#endif
#if defined TARGET_OF_IOS
if (ofApp::isTablet()) {
ofSetOrientation(OF_ORIENTATION_90_LEFT);
swiper.setup();
ofAddListener(swiper.swipeRecognized, this, &ofApp::onSwipe);
swiping = false;
} else {
swiper.setup();
ofAddListener(swiper.swipeRecognized, this, &ofApp::onSwipe);
swiping = false;
}
#endif
#ifndef TARGET_OSX
if (isAndroid() || isIos()) {
ofxAccelerometer.setup();
accelCount = 0;
crop = 0;
}
#endif
if (!ofApp::isIos()) {
ofLogNotice() << "Registering for touch events if not ios";
ofRegisterTouchEvents(this);
}
ofSetFrameRate(FRAME_RATE);
ofSetCircleResolution(CIRCLE_RESOLUTION);
if (multitouch) ofHideCursor();
ofApp::language = ofApp::getSystemLanguage();
ofLogNotice() << "Language is " << ofApp::language;
initTranslations();
initModules();
setupModules();
loadModuleSounds();
initImages();
appState = ABOUT;
inactivityState = ACTIVE;
// init global vars
aboutY = 0;
splashAlpha = 255;
arrowDownY = ofGetHeight()/3*2;
arrowDownYBase = arrowDownY;
arrowDownDir = 1;
showSwipeInfo = true;
ofApp::maxParticleY = round(ofGetHeight() * (1-LIMIT_PARTICLE));
uint swipeFontSize;
if (isTablet()) swipeFontSize = 26;
else swipeFontSize = 20;
swipeFont.load(UI_FONT_FACE, swipeFontSize);
}
const char *
VNUM_2bytes(const char *p, uintmax_t *r, uintmax_t rel)
{
double fval;
const char *end;
if (p == NULL || *p == '\0')
return (err_miss_num);
fval = VNUMpfx(p, &end);
if (isnan(fval))
return (err_invalid_num);
if (end == NULL) {
*r = (uintmax_t)fval;
return (NULL);
}
if (end[0] == '%' && end[1] == '\0') {
if (rel == 0)
return (err_abs_req);
fval *= rel / 100.0;
} else {
/* accept a space before the multiplier */
if (end[0] == ' ' && end[1] != '\0')
++end;
switch (end[0]) {
case 'k': case 'K':
fval *= (uintmax_t)1 << 10;
++end;
break;
case 'm': case 'M':
fval *= (uintmax_t)1 << 20;
++end;
break;
case 'g': case 'G':
fval *= (uintmax_t)1 << 30;
++end;
break;
case 't': case 'T':
fval *= (uintmax_t)1 << 40;
++end;
break;
case 'p': case 'P':
fval *= (uintmax_t)1 << 50;
++end;
break;
default:
break;
}
/* [bB] is a generic suffix of no effect */
if (end[0] == 'b' || end[0] == 'B')
end++;
if (end[0] != '\0')
return (err_invalid_suff);
}
*r = (uintmax_t)round(fval);
return (NULL);
}
/*************************************************************************
*
* FUNCTION: Levinson()
*
* PURPOSE: Levinson-Durbin algorithm in double precision. To compute the
* LP filter parameters from the speech autocorrelations.
*
* DESCRIPTION:
* R[i] autocorrelations.
* A[i] filter coefficients.
* K reflection coefficients.
* Alpha prediction gain.
*
* Initialisation:
* A[0] = 1
* K = -R[1]/R[0]
* A[1] = K
* Alpha = R[0] * (1-K**2]
*
* Do for i = 2 to M
*
* S = SUM ( R[j]*A[i-j] ,j=1,i-1 ) + R[i]
*
* K = -S / Alpha
*
* An[j] = A[j] + K*A[i-j] for j=1 to i-1
* where An[i] = new A[i]
* An[i]=K
*
* Alpha=Alpha * (1-K**2)
*
* END
*
*************************************************************************/
int Levinson (
LevinsonState *st,
Word16 Rh[], /* i : Rh[m+1] Vector of autocorrelations (msb) */
Word16 Rl[], /* i : Rl[m+1] Vector of autocorrelations (lsb) */
Word16 A[], /* o : A[m] LPC coefficients (m = 10) */
Word16 rc[] /* o : rc[4] First 4 reflection coefficients */
)
{
Word16 i, j;
Word16 hi, lo;
Word16 Kh, Kl; /* reflexion coefficient; hi and lo */
Word16 alp_h, alp_l, alp_exp; /* Prediction gain; hi lo and exponent */
Word16 Ah[M + 1], Al[M + 1]; /* LPC coef. in double prec. */
Word16 Anh[M + 1], Anl[M + 1];/* LPC coef.for next iteration in double
prec. */
Word32 t0, t1, t2; /* temporary variable */
/* K = A[1] = -R[1] / R[0] */
t1 = L_Comp (Rh[1], Rl[1]);
t2 = L_abs (t1); /* abs R[1] */
t0 = Div_32 (t2, Rh[0], Rl[0]); /* R[1]/R[0] */
if (t1 > 0)
t0 = L_negate (t0); /* -R[1]/R[0] */
L_Extract (t0, &Kh, &Kl); /* K in DPF */
rc[0] = round (t0);
t0 = L_shr (t0, 4); /* A[1] in */
L_Extract (t0, &Ah[1], &Al[1]); /* A[1] in DPF */
/* Alpha = R[0] * (1-K**2) */
t0 = Mpy_32 (Kh, Kl, Kh, Kl); /* K*K */
t0 = L_abs (t0); /* Some case <0 !! */
t0 = L_sub ((Word32) 0x7fffffffL, t0); /* 1 - K*K */
L_Extract (t0, &hi, &lo); /* DPF format */
t0 = Mpy_32 (Rh[0], Rl[0], hi, lo); /* Alpha in */
/* Normalize Alpha */
alp_exp = norm_l (t0);
t0 = L_shl (t0, alp_exp);
L_Extract (t0, &alp_h, &alp_l); /* DPF format */
/*--------------------------------------*
* ITERATIONS I=2 to M *
*--------------------------------------*/
for (i = 2; i <= M; i++)
{
/* t0 = SUM ( R[j]*A[i-j] ,j=1,i-1 ) + R[i] */
t0 = 0;
for (j = 1; j < i; j++)
{
t0 = L_add (t0, Mpy_32 (Rh[j], Rl[j], Ah[i - j], Al[i - j]));
}
t0 = L_shl (t0, 4);
t1 = L_Comp (Rh[i], Rl[i]);
t0 = L_add (t0, t1); /* add R[i] */
/* K = -t0 / Alpha */
t1 = L_abs (t0);
t2 = Div_32 (t1, alp_h, alp_l); /* abs(t0)/Alpha */
if (t0 > 0)
t2 = L_negate (t2); /* K =-t0/Alpha */
t2 = L_shl (t2, alp_exp); /* denormalize; compare to Alpha */
L_Extract (t2, &Kh, &Kl); /* K in DPF */
if (sub (i, 5) < 0)
{
rc[i - 1] = round (t2);
}
/* Test for unstable filter. If unstable keep old A(z) */
if (sub (abs_s (Kh), 32750) > 0)
{
for (j = 0; j <= M; j++)
{
A[j] = st->old_A[j];
}
for (j = 0; j < 4; j++)
{
rc[j] = 0;
}
return 0;
}
/*------------------------------------------*
* Compute new LPC coeff. -> An[i] *
* An[j]= A[j] + K*A[i-j] , j=1 to i-1 *
* An[i]= K *
*------------------------------------------*/
for (j = 1; j < i; j++)
{
t0 = Mpy_32 (Kh, Kl, Ah[i - j], Al[i - j]);
t0 = L_add(t0, L_Comp(Ah[j], Al[j]));
L_Extract (t0, &Anh[j], &Anl[j]);
}
t2 = L_shr (t2, 4);
L_Extract (t2, &Anh[i], &Anl[i]);
/* Alpha = Alpha * (1-K**2) */
t0 = Mpy_32 (Kh, Kl, Kh, Kl); /* K*K */
t0 = L_abs (t0); /* Some case <0 !! */
t0 = L_sub ((Word32) 0x7fffffffL, t0); /* 1 - K*K */
L_Extract (t0, &hi, &lo); /* DPF format */
t0 = Mpy_32 (alp_h, alp_l, hi, lo);
/* Normalize Alpha */
j = norm_l (t0);
t0 = L_shl (t0, j);
L_Extract (t0, &alp_h, &alp_l); /* DPF format */
alp_exp = add (alp_exp, j); /* Add normalization to
alp_exp */
/* A[j] = An[j] */
for (j = 1; j <= i; j++)
{
Ah[j] = Anh[j];
Al[j] = Anl[j];
}
}
A[0] = 4096;
for (i = 1; i <= M; i++)
{
t0 = L_Comp (Ah[i], Al[i]);
st->old_A[i] = A[i] = round (L_shl (t0, 1));
}
return 0;
}
void ossimTileToIplFilter::CopyTileToIplImage(T dummyVariable, ossimRefPtr<ossimImageData> inputTile, IplImage *output, ossimIrect neighborhoodRect)
{
ossimDataObjectStatus status = inputTile->getDataObjectStatus();
uchar *outputData = (uchar *)output->imageData;
int outputStep = output->widthStep/sizeof(uchar);
int outputChannels = output->nChannels;
ossimScalarType inputType = inputTile->getScalarType();
double scFactor;
if (inputType == OSSIM_UINT16)
scFactor = 0.0039; // 255 / 65535
else if (inputType == OSSIM_USHORT11)
scFactor = 0.1246; //255 / 2047
else if (inputType == OSSIM_UINT8)
scFactor = 1;
else
scFactor = 1;
int pixVal;
if (status == OSSIM_PARTIAL)
{
for( int band = 0; band < outputChannels; band++) {
T* inBuf = static_cast<T*>(inputTile->getBuf(band));
for (long y = 0; y < output->height; ++y)
{
for (long x = 0; x < output->width; ++x)
{
pixVal = (int)(*inBuf);
if ((int)round(pixVal * scFactor) > 255)
outputData[y * outputStep + x*outputChannels + band] = 255;
else if ((int)round(pixVal * scFactor) < 0)
outputData[y * outputStep + x*outputChannels + band] = 0;
else
outputData[y * outputStep + x*outputChannels + band] = (uchar)round(pixVal * scFactor);
++inBuf;
}
}
}
}
else
{
for(int band = 0; band < outputChannels; band++) {
T* inBuf = static_cast<T*>(inputTile->getBuf(band));
for (int y = 0; y < output->height; ++y)
{
for (int x = 0; x < output->width; ++x)
{
pixVal = (int)(*inBuf);
if ((int)round(pixVal * scFactor) > 255)
outputData[y * outputStep + x*outputChannels + band] = 255;
else if ((int)round(pixVal * scFactor) < 0)
outputData[y * outputStep + x*outputChannels + band] = 0;
else
outputData[y * outputStep + x*outputChannels + band] = (uchar)round(pixVal * scFactor);
++inBuf;
}
}
}
}
}
bool SpikerComponent::Fire() {
gentity_t *self = entity.oldEnt;
// Check if still resting.
if (restUntil > level.time) {
logger.Verbose("Spiker #%i wanted to fire but wasn't ready.", entity.oldEnt->s.number);
return false;
} else {
logger.Verbose("Spiker #%i is firing!", entity.oldEnt->s.number);
}
// Play shooting animation.
G_SetBuildableAnim(self, BANIM_ATTACK1, false);
GetBuildableComponent().ProtectAnimation(5000);
// TODO: Add a particle effect.
//G_AddEvent(self, EV_ALIEN_SPIKER, DirToByte(self->s.origin2));
// Calculate total perimeter of all spike rows to allow for a more even spike distribution.
// A "row" is a group of missile launch directions with a common base altitude (angle measured
// from the Spiker's horizon to its zenith) which is slightly adjusted for each new missile in
// the row (at most halfway to the base altitude of a neighbouring row).
float totalPerimeter = 0.0f;
for (int row = 0; row < MISSILEROWS; row++) {
float rowAltitude = (((float)row + 0.5f) * M_PI_2) / (float)MISSILEROWS;
float rowPerimeter = 2.0f * M_PI * cos(rowAltitude);
totalPerimeter += rowPerimeter;
}
// TODO: Use new vector library.
vec3_t dir, zenith, rotAxis;
// As rotation axis for setting the altitude, any vector perpendicular to the zenith works.
VectorCopy(self->s.origin2, zenith);
PerpendicularVector(rotAxis, zenith);
// Distribute and launch missiles.
for (int row = 0; row < MISSILEROWS; row++) {
// Set the base altitude and get the perimeter for the current row.
float rowAltitude = (((float)row + 0.5f) * M_PI_2) / (float)MISSILEROWS;
float rowPerimeter = 2.0f * M_PI * cos(rowAltitude);
// Attempt to distribute spikes with equal expected angular distance on all rows.
int spikes = (int)round(((float)MISSILES * rowPerimeter) / totalPerimeter);
// Launch missiles in the current row.
for (int spike = 0; spike < spikes; spike++) {
float spikeAltitude = rowAltitude + (0.5f * crandom() * M_PI_2 / (float)MISSILEROWS);
float spikeAzimuth = 2.0f * M_PI * (((float)spike + 0.5f * crandom()) / (float)spikes);
// Set launch direction altitude.
RotatePointAroundVector(dir, rotAxis, zenith, RAD2DEG(M_PI_2 - spikeAltitude));
// Set launch direction azimuth.
RotatePointAroundVector(dir, zenith, dir, RAD2DEG(spikeAzimuth));
// Trace in the shooting direction and do not shoot spikes that are likely to harm
// friendly entities.
bool fire = SafeToShoot(Vec3::Load(dir));
logger.Debug("Spiker #%d %s: Row %d/%d: Spike %2d/%2d: "
"( Alt %2.0f°, Az %3.0f° → %.2f, %.2f, %.2f )", self->s.number,
fire ? "fires" : "skips", row + 1, MISSILEROWS, spike + 1, spikes,
RAD2DEG(spikeAltitude), RAD2DEG(spikeAzimuth), dir[0], dir[1], dir[2]);
if (!fire) {
continue;
}
G_SpawnMissile(
MIS_SPIKER, self, self->s.origin, dir, nullptr, G_FreeEntity,
level.time + (int)(1000.0f * SPIKE_RANGE / (float)BG_Missile(MIS_SPIKER)->speed));
}
}
restUntil = level.time + COOLDOWN;
RegisterSlowThinker();
return true;
}
