//Finds distance between 2 points
float find_distance( float x_1, float y_1, float x_2, float y_2){
return hypot(x_1 - x_2, y_1 - y_2);
}
/* some rather complex logic here. If the user clicks without modifiers,
* then we start a new list, and use the first object in it as reference.
* If the user clicks using Shift, or draws a rubber-band box, then
* we add objects to the list, but do not specify which one should
* be used as reference.
*/
static void
gimp_align_tool_button_release (GimpTool *tool,
const GimpCoords *coords,
guint32 time,
GdkModifierType state,
GimpButtonReleaseType release_type,
GimpDisplay *display)
{
GimpAlignTool *align_tool = GIMP_ALIGN_TOOL (tool);
GimpAlignOptions *options = GIMP_ALIGN_TOOL_GET_OPTIONS (tool);
GimpDisplayShell *shell = gimp_display_get_shell (display);
GObject *object = NULL;
GimpImage *image = gimp_display_get_image (display);
GdkModifierType extend_mask;
gint i;
extend_mask = gimp_get_extend_selection_mask ();
gimp_draw_tool_pause (GIMP_DRAW_TOOL (tool));
gimp_tool_control_halt (tool->control);
if (release_type == GIMP_BUTTON_RELEASE_CANCEL)
{
align_tool->x2 = align_tool->x1;
align_tool->y2 = align_tool->y1;
gimp_draw_tool_resume (GIMP_DRAW_TOOL (tool));
return;
}
if (! (state & extend_mask)) /* start a new list */
{
gimp_align_tool_clear_selected (align_tool);
align_tool->set_reference = FALSE;
}
/* if mouse has moved less than EPSILON pixels since button press,
* select the nearest thing, otherwise make a rubber-band rectangle
*/
if (hypot (coords->x - align_tool->x1,
coords->y - align_tool->y1) < EPSILON)
{
GimpVectors *vectors;
GimpGuide *guide;
GimpLayer *layer;
gint snap_distance = display->config->snap_distance;
if (gimp_draw_tool_on_vectors (GIMP_DRAW_TOOL (tool), display,
coords, snap_distance, snap_distance,
NULL, NULL, NULL, NULL, NULL,
&vectors))
{
object = G_OBJECT (vectors);
}
else if (gimp_display_shell_get_show_guides (shell) &&
(guide = gimp_image_find_guide (image,
coords->x, coords->y,
FUNSCALEX (shell, snap_distance),
FUNSCALEY (shell, snap_distance))))
{
object = G_OBJECT (guide);
}
else
{
if ((layer = gimp_image_pick_layer_by_bounds (image,
coords->x, coords->y)))
{
object = G_OBJECT (layer);
}
}
if (object)
{
if (! g_list_find (align_tool->selected_objects, object))
{
align_tool->selected_objects =
g_list_append (align_tool->selected_objects, object);
g_signal_connect (object, "removed",
G_CALLBACK (gimp_align_tool_object_removed),
align_tool);
/* if an object has been selected using unmodified click,
* it should be used as the reference
*/
if (! (state & extend_mask))
align_tool->set_reference = TRUE;
}
}
}
else /* FIXME: look for vectors too */
{
gint X0 = MIN (coords->x, align_tool->x1);
gint X1 = MAX (coords->x, align_tool->x1);
gint Y0 = MIN (coords->y, align_tool->y1);
gint Y1 = MAX (coords->y, align_tool->y1);
GList *all_layers;
GList *list;
all_layers = gimp_image_get_layer_list (image);
for (list = all_layers; list; list = g_list_next (list))
{
GimpLayer *layer = list->data;
gint x0, y0, x1, y1;
if (! gimp_item_get_visible (GIMP_ITEM (layer)))
continue;
gimp_item_get_offset (GIMP_ITEM (layer), &x0, &y0);
x1 = x0 + gimp_item_get_width (GIMP_ITEM (layer));
y1 = y0 + gimp_item_get_height (GIMP_ITEM (layer));
if (x0 < X0 || y0 < Y0 || x1 > X1 || y1 > Y1)
continue;
if (g_list_find (align_tool->selected_objects, layer))
continue;
align_tool->selected_objects =
g_list_append (align_tool->selected_objects, layer);
g_signal_connect (layer, "removed",
G_CALLBACK (gimp_align_tool_object_removed),
align_tool);
}
g_list_free (all_layers);
}
for (i = 0; i < ALIGN_OPTIONS_N_BUTTONS; i++)
{
if (options->button[i])
gtk_widget_set_sensitive (options->button[i],
align_tool->selected_objects != NULL);
}
align_tool->x2 = align_tool->x1;
align_tool->y2 = align_tool->y1;
gimp_draw_tool_resume (GIMP_DRAW_TOOL (tool));
}
/**
*
* Function to compute the optical flow in one scale
*
**/
void Dual_TVL1_optic_flow(
float *I0, // source image
float *I1, // target image
float *u1, // x component of the optical flow
float *u2, // y component of the optical flow
const int nx, // image width
const int ny, // image height
const float tau, // time step
const float lambda, // weight parameter for the data term
const float theta, // weight parameter for (u - v)²
const int warps, // number of warpings per scale
const float epsilon, // tolerance for numerical convergence
const bool verbose // enable/disable the verbose mode
)
{
const int size = nx * ny;
const float l_t = lambda * theta;
size_t sf = sizeof(float);
float *I1x = malloc(size*sf);
float *I1y = xmalloc(size*sf);
float *I1w = xmalloc(size*sf);
float *I1wx = xmalloc(size*sf);
float *I1wy = xmalloc(size*sf);
float *rho_c = xmalloc(size*sf);
float *v1 = xmalloc(size*sf);
float *v2 = xmalloc(size*sf);
float *p11 = xmalloc(size*sf);
float *p12 = xmalloc(size*sf);
float *p21 = xmalloc(size*sf);
float *p22 = xmalloc(size*sf);
float *div = xmalloc(size*sf);
float *grad = xmalloc(size*sf);
float *div_p1 = xmalloc(size*sf);
float *div_p2 = xmalloc(size*sf);
float *u1x = xmalloc(size*sf);
float *u1y = xmalloc(size*sf);
float *u2x = xmalloc(size*sf);
float *u2y = xmalloc(size*sf);
centered_gradient(I1, I1x, I1y, nx, ny);
// initialization of p
for (int i = 0; i < size; i++)
{
p11[i] = p12[i] = 0.0;
p21[i] = p22[i] = 0.0;
}
for (int warpings = 0; warpings < warps; warpings++)
{
// compute the warping of the target image and its derivatives
bicubic_interpolation_warp(I1, u1, u2, I1w, nx, ny, true);
bicubic_interpolation_warp(I1x, u1, u2, I1wx, nx, ny, true);
bicubic_interpolation_warp(I1y, u1, u2, I1wy, nx, ny, true);
#pragma omp parallel for
for (int i = 0; i < size; i++)
{
const float Ix2 = I1wx[i] * I1wx[i];
const float Iy2 = I1wy[i] * I1wy[i];
// store the |Grad(I1)|^2
grad[i] = (Ix2 + Iy2);
// compute the constant part of the rho function
rho_c[i] = (I1w[i] - I1wx[i] * u1[i]
- I1wy[i] * u2[i] - I0[i]);
}
int n = 0;
float error = INFINITY;
while (error > epsilon * epsilon && n < MAX_ITERATIONS)
{
n++;
// estimate the values of the variable (v1, v2)
// (thresholding opterator TH)
#pragma omp parallel for
for (int i = 0; i < size; i++)
{
const float rho = rho_c[i]
+ (I1wx[i] * u1[i] + I1wy[i] * u2[i]);
float d1, d2;
if (rho < - l_t * grad[i])
{
d1 = l_t * I1wx[i];
d2 = l_t * I1wy[i];
}
else
{
if (rho > l_t * grad[i])
{
d1 = -l_t * I1wx[i];
d2 = -l_t * I1wy[i];
}
else
{
if (grad[i] < GRAD_IS_ZERO)
d1 = d2 = 0;
else
{
float fi = -rho/grad[i];
d1 = fi * I1wx[i];
d2 = fi * I1wy[i];
}
}
}
v1[i] = u1[i] + d1;
v2[i] = u2[i] + d2;
}
// compute the divergence of the dual variable (p1, p2)
divergence(p11, p12, div_p1, nx ,ny);
divergence(p21, p22, div_p2, nx ,ny);
// estimate the values of the optical flow (u1, u2)
error = 0.0;
#pragma omp parallel for reduction(+:error)
for (int i = 0; i < size; i++)
{
const float u1k = u1[i];
const float u2k = u2[i];
u1[i] = v1[i] + theta * div_p1[i];
u2[i] = v2[i] + theta * div_p2[i];
error += (u1[i] - u1k) * (u1[i] - u1k) +
(u2[i] - u2k) * (u2[i] - u2k);
}
error /= size;
// compute the gradient of the optical flow (Du1, Du2)
forward_gradient(u1, u1x, u1y, nx ,ny);
forward_gradient(u2, u2x, u2y, nx ,ny);
// estimate the values of the dual variable (p1, p2)
#pragma omp parallel for
for (int i = 0; i < size; i++)
{
const float taut = tau / theta;
const float g1 = hypot(u1x[i], u1y[i]);
const float g2 = hypot(u2x[i], u2y[i]);
const float ng1 = 1.0 + taut * g1;
const float ng2 = 1.0 + taut * g2;
p11[i] = (p11[i] + taut * u1x[i]) / ng1;
p12[i] = (p12[i] + taut * u1y[i]) / ng1;
p21[i] = (p21[i] + taut * u2x[i]) / ng2;
p22[i] = (p22[i] + taut * u2y[i]) / ng2;
}
}
if (verbose)
fprintf(stderr, "Warping: %d, "
"Iterations: %d, "
"Error: %f\n", warpings, n, error);
}
// delete allocated memory
free(I1x);
free(I1y);
free(I1w);
free(I1wx);
free(I1wy);
free(rho_c);
free(v1);
free(v2);
free(p11);
free(p12);
free(p21);
free(p22);
free(div);
free(grad);
free(div_p1);
free(div_p2);
free(u1x);
free(u1y);
free(u2x);
free(u2y);
}
static void
square_samples(GwyContainer *data, GwyRunType run)
{
GwyDataField *dfield, *dfields[3];
gdouble xreal, yreal, qx, qy;
gint oldid, newid, xres, yres;
GQuark quark;
g_return_if_fail(run & BASICOPS_RUN_MODES);
gwy_app_data_browser_get_current(GWY_APP_DATA_FIELD, dfields + 0,
GWY_APP_MASK_FIELD, dfields + 1,
GWY_APP_SHOW_FIELD, dfields + 2,
GWY_APP_DATA_FIELD_ID, &oldid,
0);
dfield = dfields[0];
xres = gwy_data_field_get_xres(dfield);
yres = gwy_data_field_get_yres(dfield);
xreal = gwy_data_field_get_xreal(dfield);
yreal = gwy_data_field_get_yreal(dfield);
qx = xres/xreal;
qy = yres/yreal;
if (fabs(log(qx/qy)) > 1.0/hypot(xres, yres)) {
/* Resample */
if (qx < qy)
xres = MAX(GWY_ROUND(xreal*qy), 1);
else
yres = MAX(GWY_ROUND(yreal*qx), 1);
dfields[0] = gwy_data_field_new_resampled(dfields[0], xres, yres,
GWY_INTERPOLATION_BSPLINE);
if (dfields[1]) {
dfields[1]
= gwy_data_field_new_resampled(dfields[1], xres, yres,
GWY_INTERPOLATION_ROUND);
}
if (dfields[2]) {
dfields[2]
= gwy_data_field_new_resampled(dfields[2], xres, yres,
GWY_INTERPOLATION_BSPLINE);
}
}
else {
/* Ratios are equal, just duplicate */
dfields[0] = gwy_data_field_duplicate(dfields[0]);
if (dfields[1])
dfields[1] = gwy_data_field_duplicate(dfields[1]);
if (dfields[2])
dfields[2] = gwy_data_field_duplicate(dfields[2]);
}
newid = gwy_app_data_browser_add_data_field(dfields[0], data, TRUE);
g_object_unref(dfields[0]);
gwy_app_sync_data_items(data, data, oldid, newid, FALSE,
GWY_DATA_ITEM_GRADIENT,
GWY_DATA_ITEM_RANGE,
GWY_DATA_ITEM_MASK_COLOR,
0);
if (dfields[1]) {
quark = gwy_app_get_mask_key_for_id(newid);
gwy_container_set_object(data, quark, dfields[1]);
g_object_unref(dfields[1]);
}
if (dfields[2]) {
quark = gwy_app_get_show_key_for_id(newid);
gwy_container_set_object(data, quark, dfields[2]);
g_object_unref(dfields[2]);
}
gwy_app_channel_log_add_proc(data, oldid, newid);
}
gcc_pure
double Magnitude() const {
return hypot(x, y);
}
/* CKTfour() - perform fourier analysis of an output vector.
*
* Due to the construction of the program which places all the output
* data in the post-processor, the fourier analysis can not be done
* directly. This function allows the post processor to hand back
* vectors of time and data values to have the fourier analysis
* performed on them. */
static int
CKTfour(int ndata, /* number of entries in the Time and
Value arrays */
int numFreq, /* number of harmonics to calculate */
double *thd, /* total harmonic distortion (percent)
to be returned */
double *Time, /* times at which the voltage/current
values were measured*/
double *Value, /* voltage or current vector whose
transform is desired */
double FundFreq, /* the fundamental frequency of the
analysis */
double *Freq, /* the frequency value of the various
harmonics */
double *Mag, /* the Magnitude of the fourier
transform */
double *Phase, /* the Phase of the fourier transform */
double *nMag, /* the normalized magnitude of the
transform: nMag(fund)=1*/
double *nPhase) /* the normalized phase of the
transform: Nphase(fund)=0 */
{
/* Note: we can consider these as a set of arrays. The sizes are:
* Time[ndata], Value[ndata], Freq[numFreq], Mag[numfreq],
* Phase[numfreq], nMag[numfreq], nPhase[numfreq]
*
* The arrays must all be allocated by the caller.
* The Time and Value array must be reasonably distributed over at
* least one full period of the fundamental Frequency for the
* fourier transform to be useful. The function will take the
* last period of the frequency as data for the transform.
*
* We are assuming that the caller has provided exactly one period
* of the fundamental frequency. */
int i;
int j;
double tmp;
NG_IGNORE(Time);
/* clear output/computation arrays */
for (i = 0; i < numFreq; i++) {
Mag[i] = 0;
Phase[i] = 0;
}
for (i = 0; i < ndata; i++)
for (j = 0; j < numFreq; j++) {
Mag[j] += Value[i] * sin(j*2.0*M_PI*i/((double)ndata));
Phase[j] += Value[i] * cos(j*2.0*M_PI*i/((double)ndata));
}
Mag[0] = Phase[0]/ndata;
Phase[0] = nMag[0] = nPhase[0] = Freq[0] = 0;
*thd = 0;
for (i = 1; i < numFreq; i++) {
tmp = Mag[i] * 2.0 / ndata;
Phase[i] *= 2.0 / ndata;
Freq[i] = i * FundFreq;
Mag[i] = hypot(tmp, Phase[i]);
Phase[i] = atan2(Phase[i], tmp) * 180.0/M_PI;
nMag[i] = Mag[i] / Mag[1];
nPhase[i] = Phase[i] - Phase[1];
if (i > 1)
*thd += nMag[i] * nMag[i];
}
*thd = 100*sqrt(*thd);
return (OK);
}
static void auto_features(void)
{
struct player *pl;
struct planet *pln;
u_char course;
int troop_capacity=0;
static int sd_time_last = -1;
int sd_time;
check_observs();
if (!living) return;
if (me->p_flags & PFSELFDEST) {
sd_time = (me->p_selfdest - me->p_updates) / 10;
if (sd_time != sd_time_last) {
sd_time_last = sd_time;
switch (sd_time) {
case 4:
case 5:
new_warning(UNDEF, "You notice everyone on the bridge is staring at you.");
break;
default:
new_warning(UNDEF, "Stand By ... Self Destruct in %d seconds", sd_time);
break;
}
}
} else {
sd_time_last = -1;
}
/* provide a refit countdown 4/6/92 TC */
if (me->p_flags & PFREFITTING) {
static int lastRefitValue = 0; /* for smooth display */
if (lastRefitValue != (rdelay - me->p_updates)/10) {
lastRefitValue = (rdelay - me->p_updates)/10; /* CSE to the rescue? */
switch (lastRefitValue) {
case 3:
case 2:
new_warning(UNDEF, "Engineering: Energizing transporters in %d seconds", lastRefitValue);
break;
case 1:
new_warning(UNDEF, "Engineering: Energize. [ SFX: chimes ]");
break;
case 0:
switch (random()%5) {
case 0:
new_warning(UNDEF,"Wait, you forgot your toothbrush!");
break;
case 1:
new_warning(UNDEF,"Nothing like turning in a used ship for a new one.");
break;
case 2:
new_warning(UNDEF,"First officer: Oh no, not you again... we're doomed!");
break;
case 3:
new_warning(UNDEF,"First officer: Uh, I'd better run diagnostics on the escape pods.");
break;
case 4:
new_warning(UNDEF,"Shipyard controller: This time, *please* be more careful, okay?");
break;
}
break;
}
}
}
/* provide a war declaration countdown 4/6/92 TC */
if (me->p_flags & PFWAR) {
static int lastWarValue = 0;
if (lastWarValue != (delay - me->p_updates)/10) {
lastWarValue = (delay - me->p_updates)/10; /* CSE to the rescue? */
switch (lastWarValue) {
case 9:
new_warning(UNDEF,"Weapons officer: Not again! This is absurd...");
break;
case 8:
break;
case 7:
new_warning(UNDEF,"Weapons officer: ... the whole ship's computer is down?");
break;
case 6:
break;
case 5:
new_warning(UNDEF,"Weapons officer: Just to twiddle a few bits of the ship's memory?");
break;
case 4:
break;
case 3:
new_warning(UNDEF,"Weapons officer: Bah! [ bangs fist on inoperative console ]");
break;
case 2:
break;
case 1:
new_warning(UNDEF,"First Officer: Easy, big guy... it's just one of those mysterious");
break;
case 0:
switch (random()%5) {
case 0:
new_warning(UNDEF,"First Officer: laws of the universe, like 'tires on the ether'.");
break;
case 1:
new_warning(UNDEF,"First Officer: laws of the universe, like 'Klingon bitmaps are ugly'.");
break;
case 2:
new_warning(UNDEF,"First Officer: laws of the universe, like 'all admirals have scummed'.");
break;
case 3:
new_warning(UNDEF,"First Officer: laws of the universe, like 'Mucus Pig exists'.");
break;
case 4:
new_warning(UNDEF,"First Officer: laws of the universe, like 'guests advance 5x faster'.");
break;
}
break;
}
}
}
/* give certain information about bombing or beaming */
if (me->p_flags & PFBOMB) {
if (planets[me->p_planet].pl_armies < 5) {
new_warning(UNDEF, "Weapons Officer: Bombarding %s... Ineffective, %d armies left.",
planets[me->p_planet].pl_name, /* Planet name for stats 10/20/96 [007] */
planets[me->p_planet].pl_armies); /* nifty info feature 2/14/92 TMC */
me->p_flags &= ~PFBOMB;
}
else {
new_warning(UNDEF, "Weapons Officer: Bombarding %s... Sensors read %d armies left.",
planets[me->p_planet].pl_name,
planets[me->p_planet].pl_armies);
}
}
/* Use person observed for kills if we are an observer */
if (Observer && (me->p_flags & PFPLOCK))
pl = &players[me->p_playerl];
else
pl = me; /* Not observer, just use my kills */
troop_capacity = (int)((float)((int)(pl->p_kills*100)/100.0) * (myship->s_type == ASSAULT?3:2));
if (myship->s_type == STARBASE || troop_capacity > myship->s_maxarmies)
troop_capacity = myship->s_maxarmies;
if (me->p_flags & PFBEAMUP) {
/* Messages rewritten for stats 10/20/96 [007] */
char *txt;
if (me->p_flags & PFORBIT) {
if (planets[me->p_planet].pl_armies < 5) {
txt = "Too few armies to beam up";
me->p_flags &= ~PFBEAMUP;
} else if (me->p_armies == troop_capacity) {
txt = "No more room on board for armies";
me->p_flags &= ~PFBEAMUP;
} else {
txt = "Beaming up";
}
new_warning(UNDEF, "%s: (%d/%d) %s has %d armies left",
txt,
me->p_armies,
troop_capacity,
planets[me->p_planet].pl_name,
planets[me->p_planet].pl_armies);
} else if (me->p_flags & PFDOCK) {
struct player *base = bay_owner(me);
if (base->p_armies <= 0) {
txt = "Too few armies to beam up";
me->p_flags &= ~PFBEAMUP;
} else if (me->p_armies >= troop_capacity) {
txt = "No more room on board for armies";
me->p_flags &= ~PFBEAMUP;
} else {
txt = "Beaming up";
}
new_warning(UNDEF, "%s: (%d/%d) Starbase %s has %d armies left",
txt,
me->p_armies,
troop_capacity,
base->p_name,
base->p_armies);
}
}
if (me->p_flags & PFBEAMDOWN) {
/* Messages rewritten for stats 10/20/96 [007] */
char *txt;
if (me->p_flags & PFORBIT) {
if (me->p_armies == 0) {
txt = "No more armies to beam down";
me->p_flags &= ~PFBEAMDOWN;
} else {
txt = "Beaming down";
}
new_warning(UNDEF, "%s: (%d/%d) %s has %d armies left",
txt,
me->p_armies,
troop_capacity,
planets[me->p_planet].pl_name,
planets[me->p_planet].pl_armies);
} else if (me->p_flags & PFDOCK) {
struct player *base = bay_owner(me);
if (me->p_armies <= 0) {
txt = "No more armies to beam down";
me->p_flags &= ~PFBEAMDOWN;
} else if (base->p_armies >= base->p_ship.s_maxarmies) {
txt = "All troop bunkers are full";
me->p_flags &= ~PFBEAMDOWN;
} else {
txt = "Transfering ground units";
}
new_warning(UNDEF, "%s: (%d/%d) Starbase %s has %d armies left",
txt,
me->p_armies,
troop_capacity,
base->p_name,
base->p_armies);
}
}
if (me->p_flags & PFREPAIR) {
if ((me->p_damage == 0) && (me->p_shield == me->p_ship.s_maxshield))
me->p_flags &= ~PFREPAIR;
}
if ((me->p_flags & PFPLOCK) && (!Observer)) { /* set course to player x */
int dist;
pl = &players[me->p_playerl];
if (pl->p_status != PALIVE)
me->p_flags &= ~PFPLOCK;
if (me->p_flags & PFTWARP){
if (pl->p_status != PALIVE){
new_warning(UNDEF, "Starbase is dead; Transwarp aborted!", -1);
me->p_flags &= ~PFTWARP;
me->p_speed = 3;
set_speed(0);
}
if (pl->p_ship.s_type != STARBASE){
new_warning(UNDEF, "Starbase has been turned in; Transwarp aborted!", -1);
me->p_flags &= ~PFTWARP;
me->p_speed = 3;
set_speed(0);
}
if(me->p_etemp > me->p_ship.s_maxegntemp - 10){
new_warning(UNDEF,"Warp drive reaching critical temperature!", -1);
me->p_flags &= ~PFTWARP;
me->p_speed = 3;
set_speed(0);
}
else
if (me->p_fuel < (int) (me->p_ship.s_maxfuel/8)){
new_warning(UNDEF, "Not enough fuel to continue transwarp!", -1);
me->p_flags &= ~PFTWARP;
me->p_speed = 3;
set_speed(0);
}
else if(!(me->p_flags & PFPLOCK)){
new_warning(UNDEF, "Lost player lock!", -1);
me->p_flags &= ~PFTWARP;
me->p_speed = 3;
set_speed(0);
}
}
if (pl->p_ship.s_type == STARBASE) {
dist = hypot((double) (me->p_x - pl->p_x),
(double) (me->p_y - pl->p_y));
if (!(me->p_flags & PFTWARP)) {
if (dist-(DOCKDIST/2) < (11500 * me->p_speed * me->p_speed) /
me->p_ship.s_decint) {
if (me->p_desspeed > 2) {
set_speed(me->p_desspeed-1);
}
}
}
if ((dist < 2*DOCKDIST) && (me->p_flags & PFTWARP)){
p_x_y_join(me, pl);
me->p_flags &= ~(PFPLOCK);
me->p_flags &= ~(PFTWARP);
#ifdef SB_CALVINWARP
if (!(pl->p_flags & PFDOCKOK) ||
((pl->p_flags & PFPRESS) &&
(pl->p_tractor == me->p_no))) ;
else {
#endif
me->p_speed=2;
orbit();
#ifdef SB_CALVINWARP
}
#endif
} else {
if ((dist < DOCKDIST) && (me->p_speed <= 2)) {
me->p_flags &= ~PFPLOCK;
orbit();
}
}
}
if (me->p_flags & PFPLOCK) {
course = newcourse(pl->p_x, pl->p_y);
if (me->p_flags & PFTWARP)
me->p_dir = course;
else
set_course(course);
}
}
if ((me->p_flags & PFPLLOCK) && (!Observer) ) { /* set course to planet */
int dist;
pln = &planets[me->p_planet];
dist = hypot((double) (me->p_x - pln->pl_x),
(double) (me->p_y - pln->pl_y));
/* This is magic. It should be based on defines, but it slows
the ship down to warp two an appropriate distance from the
planet for orbit */
if (dist-(ORBDIST/2) < (11500 * me->p_speed * me->p_speed) /
me->p_ship.s_decint) {
if (me->p_desspeed > 2) {
set_speed(2);
}
}
if ((dist < ENTORBDIST) && (me->p_speed <= 2)) {
me->p_flags &= ~PFPLLOCK;
orbit();
}
else {
int ax, ay, ad, missing;
extern int nowobble;
/* calculate course to planet from current coordinates */
course = newcourse(pln->pl_x, pln->pl_y);
/* avoid superfluous midflight wobble */
if (nowobble) {
/* test case; at 6 o'clock on earth, lock on altair, warp 8 */
/* calculate arrival point at current course */
ax = (double) (me->p_x + Cos[me->p_desdir] * dist);
ay = (double) (me->p_y + Sin[me->p_desdir] * dist);
ad = hypot((double) (ax - pln->pl_x),
(double) (ay - pln->pl_y));
/* change to the corrected course if the expected error
exceeds the remaining distance divided by the nowobble
factor (25 works well) */
missing = (ad > dist / nowobble);
if (missing)
set_course(course);
} else {
/* classical behaviour */
if ( (ABS(course-me->p_desdir) > 2) || (dist < ENTORBDIST*10) )
set_course(course);
}
}
}
/* Send ship cap if necessary */
sndShipCap();
#ifdef STURGEON
/* Check if eligible for free upgrade */
if (sturgeon) {
if (me->p_free_upgrade) {
me->p_upgrades += baseupgradecost[me->p_free_upgrade] + me->p_upgradelist[me->p_free_upgrade]*adderupgradecost[me->p_free_upgrade];
me->p_upgradelist[me->p_free_upgrade]++;
sturgeon_apply_upgrade(me->p_free_upgrade, me, 1);
me->p_free_upgrade = 0;
}
}
#endif
}
void Camera::update(LocalPlayer* player, f32 frametime, f32 busytime,
f32 tool_reload_ratio, ClientEnvironment &c_env)
{
// Get player position
// Smooth the movement when walking up stairs
v3f old_player_position = m_playernode->getPosition();
v3f player_position = player->getPosition();
if (player->isAttached && player->parent)
player_position = player->parent->getPosition();
//if(player->touching_ground && player_position.Y > old_player_position.Y)
if(player->touching_ground &&
player_position.Y > old_player_position.Y)
{
f32 oldy = old_player_position.Y;
f32 newy = player_position.Y;
f32 t = exp(-23*frametime);
player_position.Y = oldy * t + newy * (1-t);
}
// Set player node transformation
m_playernode->setPosition(player_position);
m_playernode->setRotation(v3f(0, -1 * player->getYaw(), 0));
m_playernode->updateAbsolutePosition();
// Get camera tilt timer (hurt animation)
float cameratilt = fabs(fabs(player->hurt_tilt_timer-0.75)-0.75);
// Fall bobbing animation
float fall_bobbing = 0;
if(player->camera_impact >= 1 && m_camera_mode < CAMERA_MODE_THIRD)
{
if(m_view_bobbing_fall == -1) // Effect took place and has finished
player->camera_impact = m_view_bobbing_fall = 0;
else if(m_view_bobbing_fall == 0) // Initialize effect
m_view_bobbing_fall = 1;
// Convert 0 -> 1 to 0 -> 1 -> 0
fall_bobbing = m_view_bobbing_fall < 0.5 ? m_view_bobbing_fall * 2 : -(m_view_bobbing_fall - 0.5) * 2 + 1;
// Smoothen and invert the above
fall_bobbing = sin(fall_bobbing * 0.5 * M_PI) * -1;
// Amplify according to the intensity of the impact
fall_bobbing *= (1 - rangelim(50 / player->camera_impact, 0, 1)) * 5;
fall_bobbing *= m_cache_fall_bobbing_amount;
}
// Calculate players eye offset for different camera modes
v3f PlayerEyeOffset = player->getEyeOffset();
if (m_camera_mode == CAMERA_MODE_FIRST)
PlayerEyeOffset += player->eye_offset_first;
else
PlayerEyeOffset += player->eye_offset_third;
// Set head node transformation
m_headnode->setPosition(PlayerEyeOffset+v3f(0,cameratilt*-player->hurt_tilt_strength+fall_bobbing,0));
m_headnode->setRotation(v3f(player->getPitch(), 0, cameratilt*player->hurt_tilt_strength));
m_headnode->updateAbsolutePosition();
// Compute relative camera position and target
v3f rel_cam_pos = v3f(0,0,0);
v3f rel_cam_target = v3f(0,0,1);
v3f rel_cam_up = v3f(0,1,0);
if (m_view_bobbing_anim != 0 && m_camera_mode < CAMERA_MODE_THIRD)
{
f32 bobfrac = my_modf(m_view_bobbing_anim * 2);
f32 bobdir = (m_view_bobbing_anim < 0.5) ? 1.0 : -1.0;
#if 1
f32 bobknob = 1.2;
f32 bobtmp = sin(pow(bobfrac, bobknob) * M_PI);
//f32 bobtmp2 = cos(pow(bobfrac, bobknob) * M_PI);
v3f bobvec = v3f(
0.3 * bobdir * sin(bobfrac * M_PI),
-0.28 * bobtmp * bobtmp,
0.);
//rel_cam_pos += 0.2 * bobvec;
//rel_cam_target += 0.03 * bobvec;
//rel_cam_up.rotateXYBy(0.02 * bobdir * bobtmp * M_PI);
float f = 1.0;
f *= m_cache_view_bobbing_amount;
rel_cam_pos += bobvec * f;
//rel_cam_target += 0.995 * bobvec * f;
rel_cam_target += bobvec * f;
rel_cam_target.Z -= 0.005 * bobvec.Z * f;
//rel_cam_target.X -= 0.005 * bobvec.X * f;
//rel_cam_target.Y -= 0.005 * bobvec.Y * f;
rel_cam_up.rotateXYBy(-0.03 * bobdir * bobtmp * M_PI * f);
#else
f32 angle_deg = 1 * bobdir * sin(bobfrac * M_PI);
f32 angle_rad = angle_deg * M_PI / 180;
f32 r = 0.05;
v3f off = v3f(
r * sin(angle_rad),
r * (cos(angle_rad) - 1),
0);
rel_cam_pos += off;
//rel_cam_target += off;
rel_cam_up.rotateXYBy(angle_deg);
#endif
}
// Compute absolute camera position and target
m_headnode->getAbsoluteTransformation().transformVect(m_camera_position, rel_cam_pos);
m_headnode->getAbsoluteTransformation().rotateVect(m_camera_direction, rel_cam_target - rel_cam_pos);
v3f abs_cam_up;
m_headnode->getAbsoluteTransformation().rotateVect(abs_cam_up, rel_cam_up);
// Seperate camera position for calculation
v3f my_cp = m_camera_position;
// Reposition the camera for third person view
if (m_camera_mode > CAMERA_MODE_FIRST)
{
if (m_camera_mode == CAMERA_MODE_THIRD_FRONT)
m_camera_direction *= -1;
my_cp.Y += 2;
// Calculate new position
bool abort = false;
for (int i = BS; i <= BS*2.75; i++)
{
my_cp.X = m_camera_position.X + m_camera_direction.X*-i;
my_cp.Z = m_camera_position.Z + m_camera_direction.Z*-i;
if (i > 12)
my_cp.Y = m_camera_position.Y + (m_camera_direction.Y*-i);
// Prevent camera positioned inside nodes
INodeDefManager *nodemgr = m_gamedef->ndef();
MapNode n = c_env.getClientMap().getNodeNoEx(floatToInt(my_cp, BS));
const ContentFeatures& features = nodemgr->get(n);
if(features.walkable)
{
my_cp.X += m_camera_direction.X*-1*-BS/2;
my_cp.Z += m_camera_direction.Z*-1*-BS/2;
my_cp.Y += m_camera_direction.Y*-1*-BS/2;
abort = true;
break;
}
}
// If node blocks camera position don't move y to heigh
if (abort && my_cp.Y > player_position.Y+BS*2)
my_cp.Y = player_position.Y+BS*2;
}
// Update offset if too far away from the center of the map
m_camera_offset.X += CAMERA_OFFSET_STEP*
(((s16)(my_cp.X/BS) - m_camera_offset.X)/CAMERA_OFFSET_STEP);
m_camera_offset.Y += CAMERA_OFFSET_STEP*
(((s16)(my_cp.Y/BS) - m_camera_offset.Y)/CAMERA_OFFSET_STEP);
m_camera_offset.Z += CAMERA_OFFSET_STEP*
(((s16)(my_cp.Z/BS) - m_camera_offset.Z)/CAMERA_OFFSET_STEP);
// Set camera node transformation
m_cameranode->setPosition(my_cp-intToFloat(m_camera_offset, BS));
m_cameranode->setUpVector(abs_cam_up);
// *100.0 helps in large map coordinates
m_cameranode->setTarget(my_cp-intToFloat(m_camera_offset, BS) + 100 * m_camera_direction);
// update the camera position in front-view mode to render blocks behind player
if (m_camera_mode == CAMERA_MODE_THIRD_FRONT)
m_camera_position = my_cp;
// Get FOV setting
f32 fov_degrees = m_cache_fov;
fov_degrees = MYMAX(fov_degrees, 10.0);
fov_degrees = MYMIN(fov_degrees, 170.0);
// FOV and aspect ratio
m_aspect = (f32) porting::getWindowSize().X / (f32) porting::getWindowSize().Y;
m_fov_y = fov_degrees * M_PI / 180.0;
// Increase vertical FOV on lower aspect ratios (<16:10)
m_fov_y *= MYMAX(1.0, MYMIN(1.4, sqrt(16./10. / m_aspect)));
m_fov_x = 2 * atan(m_aspect * tan(0.5 * m_fov_y));
m_cameranode->setAspectRatio(m_aspect);
m_cameranode->setFOV(m_fov_y);
// Position the wielded item
//v3f wield_position = v3f(45, -35, 65);
v3f wield_position = v3f(55, -35, 65);
//v3f wield_rotation = v3f(-100, 120, -100);
v3f wield_rotation = v3f(-100, 120, -100);
wield_position.Y += fabs(m_wield_change_timer)*320 - 40;
if(m_digging_anim < 0.05 || m_digging_anim > 0.5)
{
f32 frac = 1.0;
if(m_digging_anim > 0.5)
frac = 2.0 * (m_digging_anim - 0.5);
// This value starts from 1 and settles to 0
f32 ratiothing = pow((1.0f - tool_reload_ratio), 0.5f);
//f32 ratiothing2 = pow(ratiothing, 0.5f);
f32 ratiothing2 = (easeCurve(ratiothing*0.5))*2.0;
wield_position.Y -= frac * 25.0 * pow(ratiothing2, 1.7f);
//wield_position.Z += frac * 5.0 * ratiothing2;
wield_position.X -= frac * 35.0 * pow(ratiothing2, 1.1f);
wield_rotation.Y += frac * 70.0 * pow(ratiothing2, 1.4f);
//wield_rotation.X -= frac * 15.0 * pow(ratiothing2, 1.4f);
//wield_rotation.Z += frac * 15.0 * pow(ratiothing2, 1.0f);
}
if (m_digging_button != -1)
{
f32 digfrac = m_digging_anim;
wield_position.X -= 50 * sin(pow(digfrac, 0.8f) * M_PI);
wield_position.Y += 24 * sin(digfrac * 1.8 * M_PI);
wield_position.Z += 25 * 0.5;
// Euler angles are PURE EVIL, so why not use quaternions?
core::quaternion quat_begin(wield_rotation * core::DEGTORAD);
core::quaternion quat_end(v3f(80, 30, 100) * core::DEGTORAD);
core::quaternion quat_slerp;
quat_slerp.slerp(quat_begin, quat_end, sin(digfrac * M_PI));
quat_slerp.toEuler(wield_rotation);
wield_rotation *= core::RADTODEG;
} else {
f32 bobfrac = my_modf(m_view_bobbing_anim);
wield_position.X -= sin(bobfrac*M_PI*2.0) * 3.0;
wield_position.Y += sin(my_modf(bobfrac*2.0)*M_PI) * 3.0;
}
m_wieldnode->setPosition(wield_position);
m_wieldnode->setRotation(wield_rotation);
m_wieldnode->setColor(player->light_color);
// Render distance feedback loop
updateViewingRange(frametime, busytime);
// If the player is walking, swimming, or climbing,
// view bobbing is enabled and free_move is off,
// start (or continue) the view bobbing animation.
v3f speed = player->getSpeed();
const bool movement_XZ = hypot(speed.X, speed.Z) > BS;
const bool movement_Y = fabs(speed.Y) > BS;
const bool walking = movement_XZ && player->touching_ground;
const bool swimming = (movement_XZ || player->swimming_vertical) && player->in_liquid;
const bool climbing = movement_Y && player->is_climbing;
if ((walking || swimming || climbing) &&
m_cache_view_bobbing &&
(!g_settings->getBool("free_move") || !m_gamedef->checkLocalPrivilege("fly")))
{
// Start animation
m_view_bobbing_state = 1;
m_view_bobbing_speed = MYMIN(speed.getLength(), 40);
}
else if (m_view_bobbing_state == 1)
{
// Stop animation
m_view_bobbing_state = 2;
m_view_bobbing_speed = 60;
}
}
static TPoint computePupil (
int num,
TPoint mouse,
const TRectangle *screen)
{
double cx, cy;
double dist;
double angle;
double dx, dy;
double cosa, sina;
TPoint ret;
cx = EYE_X(num); dx = mouse.x - cx;
cy = EYE_Y(num); dy = mouse.y - cy;
if (dx == 0 && dy == 0);
else {
angle = atan2 ((double) dy, (double) dx);
cosa = cos (angle);
sina = sin (angle);
dist = BALL_DIST;
if (screen)
{
/* use distance mapping */
double x0, y0, x1, y1;
double a[4];
x0 = screen->x - cx;
y0 = screen->y - cy;
x1 = x0 + screen->width;
y1 = y0 + screen->height;
a[0] = atan2(y0, x0);
a[1] = atan2(y1, x0);
a[2] = atan2(y1, x1);
a[3] = atan2(y0, x1);
if (AngleBetween(angle, a[0], a[1]))
{
/* left */
dist *= dx / x0;
}
else if (AngleBetween(angle, a[1], a[2]))
{
/* bottom */
dist *= dy / y1;
}
else if (AngleBetween(angle, a[2], a[3]))
{
/* right */
dist *= dx / x1;
}
else if (AngleBetween(angle, a[3], a[0]))
{
/* top */
dist *= dy / y0;
}
if (dist > BALL_DIST)
dist = BALL_DIST;
}
if (dist > hypot ((double) dx, (double) dy)) {
cx += dx;
cy += dy;
} else {
cx += dist * cosa;
cy += dist * sina;
}
}
ret.x = cx;
ret.y = cy;
return ret;
}
/*
* All the hard work is contained in this function.
* x and y are assumed positive or zero, and less than RECIP_EPSILON.
* Upon return:
* rx = Re(casinh(z)) = -Im(cacos(y + I*x)).
* B_is_usable is set to 1 if the value of B is usable.
* If B_is_usable is set to 0, sqrt_A2my2 = sqrt(A*A - y*y), and new_y = y.
* If returning sqrt_A2my2 has potential to result in an underflow, it is
* rescaled, and new_y is similarly rescaled.
*/
static inline void
do_hard_work(double x, double y, double *rx, int *B_is_usable, double *B,
double *sqrt_A2my2, double *new_y)
{
double R, S, A; /* A, B, R, and S are as in Hull et al. */
double Am1, Amy; /* A-1, A-y. */
R = hypot(x, y + 1); /* |z+I| */
S = hypot(x, y - 1); /* |z-I| */
/* A = (|z+I| + |z-I|) / 2 */
A = (R + S) / 2;
/*
* Mathematically A >= 1. There is a small chance that this will not
* be so because of rounding errors. So we will make certain it is
* so.
*/
if (A < 1)
A = 1;
if (A < A_crossover) {
/*
* Am1 = fp + fm, where fp = f(x, 1+y), and fm = f(x, 1-y).
* rx = log1p(Am1 + sqrt(Am1*(A+1)))
*/
if (y == 1 && x < DBL_EPSILON * DBL_EPSILON / 128) {
/*
* fp is of order x^2, and fm = x/2.
* A = 1 (inexactly).
*/
*rx = sqrt(x);
} else if (x >= DBL_EPSILON * fabs(y - 1)) {
/*
* Underflow will not occur because
* x >= DBL_EPSILON^2/128 >= FOUR_SQRT_MIN
*/
Am1 = f(x, 1 + y, R) + f(x, 1 - y, S);
*rx = log1p(Am1 + sqrt(Am1 * (A + 1)));
} else if (y < 1) {
/*
* fp = x*x/(1+y)/4, fm = x*x/(1-y)/4, and
* A = 1 (inexactly).
*/
*rx = x / sqrt((1 - y) * (1 + y));
} else { /* if (y > 1) */
/*
* A-1 = y-1 (inexactly).
*/
*rx = log1p((y - 1) + sqrt((y - 1) * (y + 1)));
}
} else {
*rx = log(A + sqrt(A * A - 1));
}
*new_y = y;
if (y < FOUR_SQRT_MIN) {
/*
* Avoid a possible underflow caused by y/A. For casinh this
* would be legitimate, but will be picked up by invoking atan2
* later on. For cacos this would not be legitimate.
*/
*B_is_usable = 0;
*sqrt_A2my2 = A * (2 / DBL_EPSILON);
*new_y = y * (2 / DBL_EPSILON);
return;
}
/* B = (|z+I| - |z-I|) / 2 = y/A */
*B = y / A;
*B_is_usable = 1;
if (*B > B_crossover) {
*B_is_usable = 0;
/*
* Amy = fp + fm, where fp = f(x, y+1), and fm = f(x, y-1).
* sqrt_A2my2 = sqrt(Amy*(A+y))
*/
if (y == 1 && x < DBL_EPSILON / 128) {
/*
* fp is of order x^2, and fm = x/2.
* A = 1 (inexactly).
*/
*sqrt_A2my2 = sqrt(x) * sqrt((A + y) / 2);
} else if (x >= DBL_EPSILON * fabs(y - 1)) {
/*
* Underflow will not occur because
* x >= DBL_EPSILON/128 >= FOUR_SQRT_MIN
* and
* x >= DBL_EPSILON^2 >= FOUR_SQRT_MIN
*/
Amy = f(x, y + 1, R) + f(x, y - 1, S);
*sqrt_A2my2 = sqrt(Amy * (A + y));
} else if (y > 1) {
/*
* fp = x*x/(y+1)/4, fm = x*x/(y-1)/4, and
* A = y (inexactly).
*
* y < RECIP_EPSILON. So the following
* scaling should avoid any underflow problems.
*/
*sqrt_A2my2 = x * (4 / DBL_EPSILON / DBL_EPSILON) * y /
sqrt((y + 1) * (y - 1));
*new_y = y * (4 / DBL_EPSILON / DBL_EPSILON);
} else { /* if (y < 1) */
/*
* fm = 1-y >= DBL_EPSILON, fp is of order x^2, and
* A = 1 (inexactly).
*/
*sqrt_A2my2 = sqrt((1 - y) * (1 + y));
}
}
}
t_int *leaker_perform(t_int *w)
{
int i,j,odd,even;
float a1,a2,b1,b2;
t_leaker *x = (t_leaker *) (w[1]);
t_float *in1 = (t_float *)(w[2]);
t_float *in2 = (t_float *)(w[3]);
t_float *in3 = (t_float *)(w[4]);
t_float *out = (t_float *)(w[5]);
t_int n = w[6];
float fade_value = x->fade_value;
float *input1 = x->input1;
float *input2 = x->input2;
int inCount = x->inCount;
int R = x->R;
int N = x->N;
int N2 = x->N2;
int D = x->D;
int Nw = x->Nw;
float *Wanal = x->Wanal;
float *Wsyn = x->Wsyn;
float *output = x->output;
float *buffer1 = x->buffer1;
float *buffer2 = x->buffer2;
float *channel1 = x->channel1;
float *channel2 = x->channel2;
int *sieve = x->sieve;
int *bitshuffle = x->bitshuffle;
float *trigland = x->trigland;
float mult = x->mult;
/* dereference struncture */
if( x->mute) {
while(n--){
*out++ = 0.;
}
return (w+7);
}
if( x->bypass ) {
while(n--){
*out++ = *in1++;
}
return (w+7);
}
#if MSP
if(x->fade_connected)
fade_value = *in3++ * (float) N2;
#endif
#if PD
fade_value = *in3++ * (float) N2;
#endif
inCount += D;
for ( j = 0 ; j < Nw - D ; j++ ){
input1[j] = input1[j+D];
input2[j] = input2[j+D];
}
for ( j = Nw - D; j < Nw; j++ ) {
input1[j] = *in1++;
input2[j] = *in2++;
}
fold(input1, Wanal, Nw, buffer1, N, inCount);
fold(input2, Wanal, Nw, buffer2, N, inCount);
rdft(N, 1, buffer1, bitshuffle, trigland);
rdft(N, 1, buffer2, bitshuffle, trigland);
for ( i = 0; i <= N2; i++ ) {
odd = ( even = i<<1 ) + 1;
if( fade_value <= 0 || fade_value < sieve[i] ){
a1 = ( i == N2 ? *(buffer1+1) : *(buffer1+even) );
b1 = ( i == 0 || i == N2 ? 0. : *(buffer1+odd) );
*(channel1+even) = hypot( a1, b1 ) ;
*(channel1+odd) = -atan2( b1, a1 );
*(buffer1+even) = *(channel1+even) * cos(*(channel1+odd));
if ( i != N2 ){
*(buffer1+odd) = -(*(channel1+even)) * sin(*(channel1+odd));
}
} else {
a2 = ( i == N2 ? *(buffer2+1) : *(buffer2+even) );
b2 = ( i == 0 || i == N2 ? 0. : *(buffer2+odd) );
*(channel1+even) = hypot( a2, b2 ) ;
*(channel1+odd) = -atan2( b2, a2 );
*(buffer1+even) = *(channel1+even) * cos(*(channel1+odd) );
if ( i != N2 ){
*(buffer1+odd) = -(*(channel1+even)) * sin( *(channel1+odd) );
}
}
}
rdft( N, -1, buffer1, bitshuffle, trigland );
overlapadd( buffer1, N, Wsyn, output, Nw, inCount);
for ( j = 0; j < D; j++ )
*out++ = output[j] * mult;
for ( j = 0; j < Nw - D; j++ )
output[j] = output[j+D];
for ( j = Nw - D; j < Nw; j++ )
output[j] = 0.;
x->inCount = inCount % Nw;
return (w+7);
}
PathPoint TkPathConfigureArrow(PathPoint pf, PathPoint pl, ArrowDescr *arrowDescr,
Tk_PathStyle *lineStyle, int updateFirstPoint)
{
if (arrowDescr->arrowEnabled) {
PathPoint p0;
double lineWidth = lineStyle->strokeWidth;
double shapeLength = arrowDescr->arrowLength;
double shapeWidth = arrowDescr->arrowWidth;
double shapeFill = arrowDescr->arrowFillRatio;
double dx, dy, length, sinTheta, cosTheta;
double backup; /* Distance to backup end points so the line
* ends in the middle of the arrowhead. */
double minsShapeFill;
PathPoint *poly = arrowDescr->arrowPointsPtr;
int capStyle = lineStyle->capStyle; /* CapButt, CapProjecting, or CapRound. */
if (!poly) {
Tcl_Panic("Internal error: PathPoint list is NULL pointer\n");
}
if (shapeWidth < lineWidth) {
shapeWidth = lineWidth;
}
minsShapeFill = lineWidth*shapeLength/shapeWidth;
if (shapeFill > 0.0 && fabs(shapeLength*shapeFill) < fabs(minsShapeFill))
shapeFill = 1.1*minsShapeFill / shapeLength;
backup = 0.0;
if (lineWidth > 1.0) {
backup = (capStyle == CapProjecting) ? 0.5 * lineWidth : 0.0;
if (shapeFill > 0.0 && shapeLength != 0.0) {
backup += 0.5 * lineWidth * shapeLength / shapeWidth;
}
}
dx = pf.x - pl.x;
dy = pf.y - pl.y;
length = hypot(dx, dy);
if (length == 0) {
sinTheta = cosTheta = 0.0;
} else {
sinTheta = dy/length;
cosTheta = dx/length;
}
p0.x = pf.x - shapeLength * cosTheta;
p0.y = pf.y - shapeLength * sinTheta;
if (shapeFill > 0.0 && shapeLength != 0.0) {
poly[0].x = pf.x - shapeLength * shapeFill * cosTheta;
poly[0].y = pf.y - shapeLength * shapeFill * sinTheta;
poly[4] = poly[0];
} else {
poly[0].x = poly[0].y = poly[4].x = poly[4].y = NaN;
}
poly[1].x = p0.x - shapeWidth * sinTheta;
poly[1].y = p0.y + shapeWidth * cosTheta;
poly[2].x = pf.x;
poly[2].y = pf.y;
poly[3].x = p0.x + shapeWidth * sinTheta;
poly[3].y = p0.y - shapeWidth * cosTheta;
/*
* Polygon done. Now move the first point towards the second so that
* the corners at the end of the line are inside the arrowhead.
*/
poly[LINE_PT_IN_ARROW] = poly[ORIG_PT_IN_ARROW];
if (updateFirstPoint) {
poly[LINE_PT_IN_ARROW].x -= backup*cosTheta;
poly[LINE_PT_IN_ARROW].y -= backup*sinTheta;
}
return poly[LINE_PT_IN_ARROW];
}
return pf;
}
int
main (int argc, char **argv)
{
struct experiment_params params;
double dx, dy;
double speed, angle;
gsl_vector *solution;
int c;
double compute_start, delta;
while ((c = getopt (argc, argv, "v")) != EOF) {
switch (c) {
case 'v':
vflag = 1;
break;
default:
usage ();
}
}
memset (¶ms, 0, sizeof params);
params.observed_hit[0] = 0;
params.observed_hit[1] = 0;
params.observed_hit[2] = 1;
params.observed_bounce[0] = 25;
params.observed_bounce[1] = 0;
params.observed_bounce[2] = 0;
params.observed_secs = 1.359;
dx = params.observed_bounce[0] - params.observed_hit[0];
dy = params.observed_bounce[1] - params.observed_hit[1];
params.observed_dist = hypot (dy, dx);
params.simulator_dimen = 4;
params.odesys.function = sim_func;
params.odesys.dimension = params.simulator_dimen;
params.odesys.params = ¶ms;
params.stepper = gsl_odeiv_step_alloc (gsl_odeiv_step_rk8pd,
params.simulator_dimen);
params.controller = gsl_odeiv_control_y_new (1e-6, 0.0);
params.evolver = gsl_odeiv_evolve_alloc (params.simulator_dimen);
params.minimizer_dimen = 2;
params.starting_point = gsl_vector_alloc (params.minimizer_dimen);
params.minimizer_step_sizes = gsl_vector_alloc (params.minimizer_dimen);
params.minimizer = gsl_multimin_fminimizer_alloc
(gsl_multimin_fminimizer_nmsimplex2, params.minimizer_dimen);
gsl_vector_set_all (params.starting_point, 0);
gsl_vector_set_all (params.minimizer_step_sizes, 1.0);
if (0) {
graph_error_func (¶ms);
}
compute_start = get_secs ();
solve_by_simulation (¶ms);
delta = get_secs () - compute_start;
solution = gsl_multimin_fminimizer_x (params.minimizer);
speed = gsl_vector_get (solution, 0);
angle = gsl_vector_get (solution, 1);
printf ("speed = %8.3f angle = %8.3f; compute time %.3fms\n",
speed * METERS_PER_SEC_TO_MPH,
RTOD (angle),
delta * 1000);
return (0);
}
/* This function produces a robust stack of an ensemble of multiwavelet
transformed signals stored in the input array z. The algorithm used here
is novel, or perhaps more appropriately called experimental. It was
written to replace the principal component method used in the Bear and
Pavlis paper for computing phased array stacks. That method repeatly
showed instability problems that hard to track. Entirely possible
this was only a buggy program, but the very concept of the pc method
seemed inherently unstable to me. Furthermore, it clearly would not
work right in an extension to correlation of data from clusters of
events (source arrays) because the size of signals in that situation
can vary by orders of magnitude.
This algorithm uses a different approach that is like M-estimators
but with the loss function that does not require a statistical model.
I don't know the proper term for this approach, but I'm sure there
is one in the mathematical literature. i.e. I'm fairly sure is a
reinvention of a concept used elsewhere. Haven't found it yet is all.
Key elements of the algorithm are:
1. We form the solution as a weighted stack of an ensemble of complex
valued traces passed through the 3D z array of complex numbers.
2. The initial solution is a median stack.
3. The parent loss function used to compute the weights has not
been derived, but the solution is based on weights computed as follows:
| 1/(residual amp) if residual amp > noise level
w_channel = |
| 1/noise otherwise
This is something like weighting each element by coherence, but it
is normalized differently and made more aggressive by the reciprocal
relationship. Because the traces are not normalized prior to the
stack this weighting is effectively an approximate weighting by signal
to noise ratio if the underlying signals to be stacked are perfectly coherent.
However, when there is a large misfit due to any reason the 1/residual
form will dominate and a signal gets downweighted quickly.
4. The MWstack object returns a set of complex amplitudes coefficients.
The modulus of these numbers are the relative amplitude numbers for
each channel and the phase can be used to estimate a time shift as
described in the Bear and Pavlis papers. These factors are computed
as a complex dot product between the final stack and each of the parent,
multiwavelet transformed signals. Nonparametric statistics can be applied
to these estimates to estimate uncertainties in the relative amplitudes
and timing.
5. The algorith allows one to pass in channel weights. This can be used
to produce variable array apertures with frequency to compensate for
coherence loss with the spatial separation of receivers. This vector
should be all ones if one does not wish to use this feature.
Arguments:
z - 3D C array of multiwavelet transform data to be stacked.
z[i][j][k] is assumed to be the kth time sample from
channel j of wavelet number i.
nwavelet - number of multiwavelets in transform
nchan - number of channels to be stacked
nt - number of time samples to stack
NOTE: z is thus assumed of size z[nwavelet][nchan][nt]
t0 - relative time of sample 0 of input matrices.
dt - dample interval
chanweight - base channel weight. Always used to scale
each channel in the stack, but ignored for statistical
estimates.
timeweight - vector of length nt of time window weights
to be applied (input) in computing stack
noise - nwavelet by nchan C matrix of estimate of single sample
noise level for each wavelet and each channel.
Author: Gary L. Pavlis
Written: December 2001
*/
MWstack *MWrobuststack(complex ***z,
int nwavelets,
int nchan,
int nt,
double t0,
double dt,
double *chanweight,
double *timeweight,
double **noise)
{
MWstack *s;
int ts,te; /* index of first and last nonzero element of timeweight */
int nt_used;
int i,j,k,l;
/* These are matrix work spaces. All are created as 1d vectors with
virtual indexing ala fortran. Sizes are as noted*/
complex *zwork, *residuals; /* per wavelet workspace: nwavelet X nt_used*/
double **amps; /*scalar amplitude factors nwavelets x nchan */
complex *window_stacks; /* accumulation space for stack with time weights applied
size is nt_used by nwavelets (note transpose from others) */
complex *wstack_last; /* window_stacks from previous iteration */
/* vector work spaces */
double *resnorms; /* Used to accumulate residual norms across all wavelets */
complex *stack; /* Used to accumulate full stack without windowing -- copied to
MWstack object */
double *reswt; /* working vector of robust weights derived from residuals */
complex cscale;
double sum_column_weights,sum_chan_weights; /* required normalizations */
double weight;
complex cwork;
double cmag;
int iteration;
double dzmod,ctest;
complex *A;
/* We scan for the first and last number in timeweight that are nonzero.
We need to use a reduced workspace to build stack to avoid biasing statistics
with 0 weight samples */
for(ts=0;ts<nt;++ts) if(timeweight[ts]!= 0.0) break;
if(ts>=nt)
{
elog_complain(0,"Invalid time weight function--all zeros\n");
return(NULL);
}
/* we don't need to test from the reverse direction, although we could
end up with as few as 1 nonzero weight */
for(te=nt-1;te>=0;--te) if(timeweight[te]!=0.0) break;
nt_used = te-ts+1;
for(i=ts,sum_column_weights=0.0;i<=te;++i)
sum_column_weights += fabs(timeweight[i]);
s = create_MWstack(nwavelets,nchan,nt);
s->tstart = t0;
s->dt = dt;
s->tend = t0 + dt*((double)(nt-1));
for(i=0;i<nt;++i)s->timeweight[i]=timeweight[i];
/* work spaces */
allot(complex *,zwork,nchan*nt_used);
allot(complex *,residuals,nchan*nt_used);
allot(double **,amps,nwavelets);
for(i=0;i<nwavelets;++i) allot(double *,amps[i],nchan);
allot(double *,resnorms,nchan);
allot(complex *,window_stacks,nchan*nt_used);
allot(complex *,wstack_last,nchan*nt_used);
allot(complex *,stack,nt);
allot(double *,reswt,nchan);
/* Use a median stack as a starting point. We don't apply channel
weights here as it will bias the results */
for(i=0;i<nwavelets;++i)
{
MWcopy_one_wavelet_matrix(z,i,zwork,nchan,ts,nt_used);
MWapply_time_window(zwork,nchan,nchan,nt_used,timeweight+ts);
MWmedian_stack_complex(zwork,nchan,nchan,nt_used,window_stacks+i*nt_used);
/* cdotc of a complex vector with itself gives the L2 norm squared.
We use this as a measure of raw signal strength. It only needs
to be computed once. Note sqrt and normalization by window weights*/
for(j=0;j<nchan;++j)
{
cwork = cdotc(nt_used,zwork+j,nchan,zwork+j,nchan);
/* cwork will only have a nonzero real part that is the sume
of the moduli of all the components of the zwork vector */
amps[i][j]=sqrt((double)cwork.r);
amps[i][j] /= sum_column_weights;
}
/* We normalize the stacks to unit L2 norm because otherwise computing
relative amplitudes gets problematic. */
normalize_stack(window_stacks+i*nt_used,nt_used);
}
iteration = 0;
do
{
/* We can do this because we packed this matrix into continguous block.
It saves the last stack value for comparison at the bottom of the loop. */
ccopy(nt_used*nwavelets,window_stacks,1,wstack_last,1);
/* We first have to accumulate residuals for all channels across all
wavelets for the current stack. */
for(j=0;j<nchan;++j)
{
resnorms[j]=0.0;
reswt[j]=0.0;
}
for(i=0;i<nwavelets;++i)
{
MWcopy_one_wavelet_matrix(z,i,zwork,nchan,ts,nt_used);
MWapply_time_window(zwork,nchan,nchan,nt_used,timeweight+ts);
for(j=0;j<nchan;++j)
{
ccopy(nt_used,zwork+j,nchan,residuals+j,nchan);
if(chanweight[j]!=1.0)
{
cscale.r = chanweight[j];
cscale.i = 0.0;
#ifdef SUNPERF6
cscal(nt_used,cscale,residuals+j,nchan);
#else
cscal(nt_used,&cscale,residuals+j,nchan);
#endif
}
/* This complex pointer stuff happens because of the use of
implicit fortran type matrices in contiguous blocks. The
cdotc call computes the dot product between the current
stack and data. caxpy call computes the residual derived by
computing difference between data and this scaled version of
the stack. */
s->amp[i][j] = cdotc(nt_used,zwork+j,nchan,window_stacks+i*nt_used,1);
cwork.r = -(s->amp[i][j].r);
cwork.i = -(s->amp[i][j].i);
cmag=hypot((double)cwork.r,(double)cwork.i);
#ifdef SUNPERF6
caxpy(nt_used,cwork,window_stacks+i*nt_used,1,
residuals+j,nchan);
#else
caxpy(nt_used,&cwork,window_stacks+i*nt_used,1,
residuals+j,nchan);
#endif
if(chanweight[j]>0.0)
{
cwork = cdotc(nt_used,residuals+j,nchan,
residuals+j,nchan);
resnorms[i] += sqrt((double)(cwork.r));
}
}
}
for(i=0;i<nwavelets;++i)
{
resnorms[i] /= sum_column_weights;
/* When chanweight is 0 the associated row is 0.0 so computing residuals
is not only pointless, but could cause inf errors */
for(j=0;j<nchan;++j)
{
if(chanweight[j]==0.0)
reswt[j] = 0.0;
else
{
if(resnorms[i]>noise[i][j])
reswt[j] += amps[i][j]/resnorms[i];
else
reswt[j] += amps[i][j]/noise[i][j];
}
reswt[j] /= ((double)nwavelets);
}
}
/* we now form weighted stacks */
cwork.i = 0.0;
for(i=0;i<nwavelets;++i)
{
for(k=0;k<nt;++k)
{
s->z[i][k].r = 0.0;
s->z[i][k].i = 0.0;
}
for(j=0,sum_chan_weights=0.0;j<nchan;++j)
{
weight = reswt[j]*chanweight[j];
s->weights[j]=weight;
sum_chan_weights += weight;
cwork.r = weight;
#ifdef SUNPERF6
caxpy(nt,cwork,z[i][j],1,s->z[i],1);
#else
caxpy(nt,&cwork,z[i][j],1,s->z[i],1);
#endif
}
cwork.r = 1.0/sum_chan_weights;
#ifdef SUNPERF6
cscal(nt,cwork,s->z[i],1);
#else
cscal(nt,&cwork,s->z[i],1);
#endif
/* That is the full window stack, when we loop back we
use the windowed version so we have to form it. I
use explicit indexing here to make this clearer rather
than use the the BLAS for something this simple to compute.
The indexing is ugly enough this way.*/
for(k=ts,l=0;k<=te;++k,++l)
{
window_stacks[i*nt_used + l].r = (s->z[i][k].r)*timeweight[k];
window_stacks[i*nt_used + l].i = (s->z[i][k].i)*timeweight[k];
}
normalize_stack(window_stacks+i*nt_used,nt_used);
}
/* convergence is defined by ratio of correction vector to norm of stack
across all wavelets. The stacks are normalized in each pass to unit norm
so this becomes simply the norm of the correction vector. This
crude loop works because we assume the window_stack matrices are in a
packed storage arrangement and can be dealt with as one long vector */
for(l=0,ctest=0.0;l<nt_used*nwavelets;++l)
{
dzmod = hypot((double)(window_stacks[l].r - wstack_last[l].r),
(double)(window_stacks[l].i - wstack_last[l].i));
ctest += dzmod*dzmod;
}
ctest /= ((double)((cwork.r)*(cwork.r)));
++iteration;
} while ( (ctest>CONVERGENCE_RATIO) && (iteration<=MAXIT));
/* We perhaps should recompute the amp factors before exiting, but if the
algorithm converges properly this should not be an issue. */
if(iteration>MAXIT)
{
elog_complain(0,"MWrobuststack: iterative loop did not converge\n");
s->stack_is_valid=0;
}
else
s->stack_is_valid=1;
free(zwork);
free(residuals);
for(i=0;i<nwavelets;++i)free(amps[i]);
free(amps);
free(resnorms);
free(window_stacks);
free(wstack_last);
free(stack);
free(reswt);
return(s);
}
long double
hypotl (long double x, long double y)
{
return hypot (x, y);
}
MiniDrive::MiniDrive()
{
//initialize vector
for (int i = 0; i < 3; i++)
{
Wheel temp;
wheels.push_back(temp);
}
wheels[1].pos_x = WHEEL_1_POS_X;
wheels[1].pos_y = WHEEL_1_POS_Y;
wheels[1].cent_val = WHEEL_1_CENT_VAL;
wheels[1].cent_ang = WHEEL_1_CENTER;
wheels[1].servo_range = WHEEL_1_RANGE;
wheels[2].pos_x = WHEEL_2_POS_X;
wheels[2].pos_y = WHEEL_2_POS_Y;
wheels[2].cent_val = WHEEL_2_CENT_VAL;
wheels[1].cent_ang = WHEEL_1_CENTER;
wheels[2].servo_range = WHEEL_2_RANGE;
wheels[3].pos_x = WHEEL_3_POS_X;
wheels[3].pos_y = WHEEL_3_POS_Y;
wheels[3].cent_val = WHEEL_3_CENT_VAL;
wheels[1].cent_ang = WHEEL_1_CENTER;
wheels[3].servo_range = WHEEL_3_RANGE;
//pre calculate som parameters for rotation
for(int i = 0; i < wheels.size(); i++)
{
float rot_ang;
//get the angle of the wheel when it is tangent to the center of the robot
rot_ang = atan2(wheels[i].pos_y, wheels[i].pos_x) + (M_PI/2);
//calcutlate the x and y percents to make faster
wheels[i].x_rot_per = cos(rot_ang);
wheels[i].y_rot_per = sin(rot_ang);
//calculate radius from the center of the robot
wheels[i].diameter = hypot(wheels[i].pos_x, wheels[i].pos_y) * 2;
}
//create servo driver class
miniservo servo_cmd;
//calculate the whire wrap states
for(int i = 0; i < wheels.size(); i++)
{
servo_cmd.setPod(i,1,wheels[i].cent_val);
servo_cmd.setPod(i,0,127);
//calculate velocities as percentage of max speed
wheels[i].forward = 1;
wheels[i].max_fwd_yaw = wheels[i].cent_ang + (wheels[i].servo_range/2);
wheels[i].max_fwd_yaw = wheels[i].cent_ang - (wheels[i].servo_range/2);
//set for reverse
wheels[i].max_rev_yaw = wheels[i].cent_ang + (wheels[i].servo_range/2);
wheels[i].max_rev_yaw = wheels[i].cent_ang - (wheels[i].servo_range/2);
}
}
// The arc is approximated by generating a huge number of tiny, linear segments. The length of each
// segment is configured in settings.mm_per_arc_segment.
void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8_t axis_1,
uint8_t axis_linear, float feed_rate, float radius, uint8_t isclockwise, uint8_t extruder)
{
// int acceleration_manager_was_enabled = plan_is_acceleration_manager_enabled();
// plan_set_acceleration_manager_enabled(false); // disable acceleration management for the duration of the arc
float center_axis0 = position[axis_0] + offset[axis_0];
float center_axis1 = position[axis_1] + offset[axis_1];
float linear_travel = target[axis_linear] - position[axis_linear];
float extruder_travel = target[E_AXIS] - position[E_AXIS];
float r_axis0 = -offset[axis_0]; // Radius vector from center to current location
float r_axis1 = -offset[axis_1];
float rt_axis0 = target[axis_0] - center_axis0;
float rt_axis1 = target[axis_1] - center_axis1;
// CCW angle between position and target from circle center. Only one atan2() trig computation required.
float angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
if (angular_travel < 0) { angular_travel += 2*M_PI; }
if (isclockwise) { angular_travel -= 2*M_PI; }
float millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel));
if (millimeters_of_travel < 0.001) { return; }
uint16_t segments = floor(millimeters_of_travel/MM_PER_ARC_SEGMENT);
if(segments == 0) segments = 1;
/*
// Multiply inverse feed_rate to compensate for the fact that this movement is approximated
// by a number of discrete segments. The inverse feed_rate should be correct for the sum of
// all segments.
if (invert_feed_rate) { feed_rate *= segments; }
*/
float theta_per_segment = angular_travel/segments;
float linear_per_segment = linear_travel/segments;
float extruder_per_segment = extruder_travel/segments;
/* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
r_T = [cos(phi) -sin(phi);
sin(phi) cos(phi] * r ;
For arc generation, the center of the circle is the axis of rotation and the radius vector is
defined from the circle center to the initial position. Each line segment is formed by successive
vector rotations. This requires only two cos() and sin() computations to form the rotation
matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
all double numbers are single precision on the Arduino. (True double precision will not have
round off issues for CNC applications.) Single precision error can accumulate to be greater than
tool precision in some cases. Therefore, arc path correction is implemented.
Small angle approximation may be used to reduce computation overhead further. This approximation
holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
issue for CNC machines with the single precision Arduino calculations.
This approximation also allows mc_arc to immediately insert a line segment into the planner
without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
This is important when there are successive arc motions.
*/
// Vector rotation matrix values
float cos_T = 1-0.5*theta_per_segment*theta_per_segment; // Small angle approximation
float sin_T = theta_per_segment;
float arc_target[4];
float sin_Ti;
float cos_Ti;
float r_axisi;
uint16_t i;
int8_t count = 0;
// Initialize the linear axis
arc_target[axis_linear] = position[axis_linear];
// Initialize the extruder axis
arc_target[E_AXIS] = position[E_AXIS];
for (i = 1; i<segments; i++) { // Increment (segments-1)
if (count < N_ARC_CORRECTION) {
// Apply vector rotation matrix
r_axisi = r_axis0*sin_T + r_axis1*cos_T;
r_axis0 = r_axis0*cos_T - r_axis1*sin_T;
r_axis1 = r_axisi;
count++;
} else {
// Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
// Compute exact location by applying transformation matrix from initial radius vector(=-offset).
cos_Ti = cos(i*theta_per_segment);
sin_Ti = sin(i*theta_per_segment);
r_axis0 = -offset[axis_0]*cos_Ti + offset[axis_1]*sin_Ti;
r_axis1 = -offset[axis_0]*sin_Ti - offset[axis_1]*cos_Ti;
count = 0;
}
// Update arc_target location
arc_target[axis_0] = center_axis0 + r_axis0;
arc_target[axis_1] = center_axis1 + r_axis1;
arc_target[axis_linear] += linear_per_segment;
arc_target[E_AXIS] += extruder_per_segment;
clamp_to_software_endstops(arc_target);
plan_buffer_line(arc_target[X_AXIS], arc_target[Y_AXIS], arc_target[Z_AXIS], arc_target[E_AXIS], feed_rate, extruder);
}
// Ensure last segment arrives at target location.
plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], feed_rate, extruder);
// plan_set_acceleration_manager_enabled(acceleration_manager_was_enabled);
}
void MiniDrive::SetVel(float x_in, float y_in, float yaw_in)
{
float fastest_wheel_speed = 0;
float speed_scaler;
//TODO most of these loops can be combined
//calculate the x and y movement vectors in m/s
for(int i = 0; i < wheels.size(); i++)
{
//calculate velocities as percentage of max speed
wheels[i].move_x = x_in;
wheels[i].move_y = y_in;
wheels[i].move_speed = hypot(x_in, y_in);
}
//calculate the x and y rotation vectors in m/s
for(int i = 0; i < wheels.size(); i++)
{
//calculate the speed the wheel has to move at for the desired rotation
wheels[i].rot_speed = (wheels[i].diameter * M_PI) * (yaw_in/(2*M_PI));
//calculate the x and y componet vectors of movement
wheels[i].rot_x = wheels[i].rot_speed * wheels[i].x_rot_per;
wheels[i].rot_y = wheels[i].rot_speed * wheels[i].y_rot_per;
}
//add componets to get the total componts
for(int i = 0; i < wheels.size(); i++)
{
//calculate the sume of the vector componets
wheels[i].sum_x = wheels[i].move_x + wheels[i].rot_x;
wheels[i].sum_y = wheels[i].move_y + wheels[i].rot_y;
//calculate the speeds
wheels[i].sum_speed = hypot(wheels[i].sum_x, wheels[i].sum_y);
//get fastest wheel speed
if (wheels[i].sum_speed > fastest_wheel_speed)
{
fastest_wheel_speed = wheels[i].sum_speed;
}
}
//get scaler to bring wheels to limits
if(fastest_wheel_speed > MAX_WHEEL_SPEED)
{
speed_scaler = MAX_WHEEL_SPEED/fastest_wheel_speed;
//scale wheels to limits
for(int i = 0; i < wheels.size(); i++)
{
//scale the values
wheels[i].sum_x = wheels[i].sum_x * speed_scaler;
wheels[i].sum_y = wheels[i].sum_y * speed_scaler;
}
}
//find the velocity commands
for(int i = 0; i < wheels.size(); i++)
{
//find the yaw angle for each wheel
wheels[i].yaw_angle = atan2(wheels[i].sum_y, wheels[i].sum_x);
//figure out what wire wraping state we are in
if(wheels[i].forward)
{
//wheel wrap state forward
float delta_yaw;
delta_yaw = wheels[i].yaw_angle - prev_yaw_abs;
prev_yaw_abs += delta_yaw;
}
else
{
//wheel wrap state reverce
}
//find the speed for each wheel
wheels[i].cmd_vel = CMD_MAX * (hypot(wheels[i].sum_x, wheels[i].sum_y) / MAX_WHEEL_SPEED);
}
//find rotation commands accounting for motor limitations
for(int i = 0; i < wheels.size(); i++)
{
//TODO need to set two halves of rotation states up in initialize then
//depending on the state move the motor to the corect position to match
//the yaw that I need to calculate in one of the previous steps then check
//to see if it would be faster to switch states and run the motor backward
//bellow I also need to take these commands and pipe them into the serial driver
//its 2:31 and thats alot of work so im going to bed sorry Daniel
}
}
// vecLength - Length of a vector.
double vecLength(geomVec v)
{
return hypot(v[0], v[1]);
}
void potential_double(int *ndim,double *pos,double *acc,double *pot,double *time)
{
double apar, qpar, spar, ppar, lpar, rpar, rcyl;
double i1,i2,i3;
// 20070312 bwillett added ppar - the plummer denominator: r + rc = sqrt(x^2+y^2+z^2) + rc
// 20070312 bwillett added lpar - the logarithmic argument: R^2 + (z/q)^2 + d^2
// 20070427 bwillett added rpar - the spherical radius: r + rc - rc = ppar - plu_rc
// 20070501 bwillett added apar - a + qpar
// 20070507 bwillett took out pow statements
// 20070507 bwillett used hypot from math.h
rcyl = hypot(pos[X],pos[Y]);
ppar = sqrt ((pos[X]*pos[X])+(pos[Y]*pos[Y])+(pos[Z]*pos[Z])) + plu_rc;
rpar = ppar - plu_rc;
lpar = (rcyl*rcyl) + ((pos[Z]/q)*(pos[Z]/q)) + (d*d);
// 20070312 bwillett added plummer sphere potential and logarithmic potential
//*pot = (-(miya_mass)/spar) + (-(plu_mass)/ppar) + (vhalo*vhalo*log(lpar));
// 20070312 bwillett rest left unchanged
// 20070427 bwillett changed the acceleration field
// This is only valid for 3 dimensions, and is in (x,y,z)
// Recall F_mu = -grad_mu U
// So a_mu = -grad_mu Phi
// I did these derivatives in Mathematica, and will try to keep it consistent with the conventions written above
acc[X] = - ( ( (2.0*vhalo*vhalo*pos[X])/(lpar) ) + ( (plu_mass*pos[X])/(rpar*ppar*ppar) ) );
acc[Y] = - ( ( (2.0*vhalo*vhalo*pos[Y])/(lpar) ) + ( (plu_mass*pos[Y])/(rpar*ppar*ppar) ) );
acc[Z] = - ( ( (2.0*vhalo*vhalo*pos[Z])/(lpar) ) + ( (plu_mass*pos[Z])/(rpar*ppar*ppar) ) );
// willeb 20080911 - do the integrals required for the ring potential
// X-integral
double x,y,z;
x = pos[X];
y = pos[Y];
z = pos[Z];
double Mdisk = disk_mass;
double R = disk_radius;
double phi = 0, dphi = 0;
double r = 0, dr = 0;
double zp = 0, dzp = 0;
double N = 50;
double i,j,k;
double resultx = 0, resulty = 0, resultz = 0;
dphi = (2*PI)/N;
dr = (R)/N;
dzp = (h)/N;
// X-integral
for(phi=0;phi<=2*PI;phi+=(2*PI)/N) {
for(r=0;r<=R;r+=R/N) {
for(zp=-h/2;zp<h/2;zp+=h/N) {
resultx += funcx(x,y,z,zp,r,phi,h)*r*dr*dphi*dzp;
}
}
}
resultx *= -(Mdisk)/(PI*R*R*h);
// Y-integral
for(phi=0;phi<=2*PI;phi+=(2*PI)/N) {
for(r=0;r<=R;r+=R/N) {
for(zp=-h/2;zp<h/2;zp+=h/N) {
resulty += funcy(x,y,z,zp,r,phi,h)*r*dr*dphi*dzp;
}
}
}
resulty *= -(Mdisk)/(PI*R*R*h);
// Z-integral
for(phi=0;phi<=2*PI;phi+=(2*PI)/N) {
for(r=0;r<=R;r+=R/N) {
for(zp=-h/2;zp<h/2;zp+=h/N) {
resultz += funcz(x,y,z,zp,r,phi,h)*r*dr*dphi*dzp;
}
}
}
resultz *= -(Mdisk)/(PI*R*R*h);
//printf("%f %f %f %f %f %f\n", x, y, z, resultx, resulty, resultz);
acc[X] += resultx;
acc[Y] += resulty;
acc[Z] += resultz;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "data_player");
//ULogger::setType(ULogger::kTypeConsole);
//ULogger::setLevel(ULogger::kDebug);
//ULogger::setEventLevel(ULogger::kWarning);
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
std::string frameId = "base_link";
std::string odomFrameId = "odom";
std::string cameraFrameId = "camera_optical_link";
std::string scanFrameId = "base_laser_link";
double rate = -1.0f;
std::string databasePath = "";
bool publishTf = true;
int startId = 0;
pnh.param("frame_id", frameId, frameId);
pnh.param("odom_frame_id", odomFrameId, odomFrameId);
pnh.param("camera_frame_id", cameraFrameId, cameraFrameId);
pnh.param("scan_frame_id", scanFrameId, scanFrameId);
pnh.param("rate", rate, rate); // Set -1 to use database stamps
pnh.param("database", databasePath, databasePath);
pnh.param("publish_tf", publishTf, publishTf);
pnh.param("start_id", startId, startId);
// based on URG-04LX
double scanHeight, scanAngleMin, scanAngleMax, scanAngleIncrement, scanTime, scanRangeMin, scanRangeMax;
pnh.param<double>("scan_height", scanHeight, 0.3);
pnh.param<double>("scan_angle_min", scanAngleMin, -M_PI / 2.0);
pnh.param<double>("scan_angle_max", scanAngleMax, M_PI / 2.0);
pnh.param<double>("scan_angle_increment", scanAngleIncrement, M_PI / 360.0);
pnh.param<double>("scan_time", scanTime, 1.0 / 10.0);
pnh.param<double>("scan_range_min", scanRangeMin, 0.02);
pnh.param<double>("scan_range_max", scanRangeMax, 6.0);
ROS_INFO("frame_id = %s", frameId.c_str());
ROS_INFO("odom_frame_id = %s", odomFrameId.c_str());
ROS_INFO("camera_frame_id = %s", cameraFrameId.c_str());
ROS_INFO("scan_frame_id = %s", scanFrameId.c_str());
ROS_INFO("rate = %f", rate);
ROS_INFO("publish_tf = %s", publishTf?"true":"false");
ROS_INFO("start_id = %d", startId);
if(databasePath.empty())
{
ROS_ERROR("Parameter \"database\" must be set (path to a RTAB-Map database).");
return -1;
}
databasePath = uReplaceChar(databasePath, '~', UDirectory::homeDir());
if(databasePath.size() && databasePath.at(0) != '/')
{
databasePath = UDirectory::currentDir(true) + databasePath;
}
ROS_INFO("database = %s", databasePath.c_str());
rtabmap::DBReader reader(databasePath, rate, false, false, false, startId);
if(!reader.init())
{
ROS_ERROR("Cannot open database \"%s\".", databasePath.c_str());
return -1;
}
ros::ServiceServer pauseSrv = pnh.advertiseService("pause", pauseCallback);
ros::ServiceServer resumeSrv = pnh.advertiseService("resume", resumeCallback);
image_transport::ImageTransport it(nh);
image_transport::Publisher imagePub;
image_transport::Publisher rgbPub;
image_transport::Publisher depthPub;
image_transport::Publisher leftPub;
image_transport::Publisher rightPub;
ros::Publisher rgbCamInfoPub;
ros::Publisher depthCamInfoPub;
ros::Publisher leftCamInfoPub;
ros::Publisher rightCamInfoPub;
ros::Publisher odometryPub;
ros::Publisher scanPub;
tf2_ros::TransformBroadcaster tfBroadcaster;
UTimer timer;
rtabmap::CameraInfo info;
rtabmap::SensorData data = reader.takeImage(&info);
rtabmap::OdometryEvent odom(data, info.odomPose, info.odomCovariance);
double acquisitionTime = timer.ticks();
while(ros::ok() && odom.data().id())
{
ROS_INFO("Reading sensor data %d...", odom.data().id());
ros::Time time = ros::Time::now();
sensor_msgs::CameraInfo camInfoA; //rgb or left
sensor_msgs::CameraInfo camInfoB; //depth or right
camInfoA.K.assign(0);
camInfoA.K[0] = camInfoA.K[4] = camInfoA.K[8] = 1;
camInfoA.R.assign(0);
camInfoA.R[0] = camInfoA.R[4] = camInfoA.R[8] = 1;
camInfoA.P.assign(0);
camInfoA.P[10] = 1;
camInfoA.header.frame_id = cameraFrameId;
camInfoA.header.stamp = time;
camInfoB = camInfoA;
int type = -1;
if(!odom.data().depthRaw().empty() && (odom.data().depthRaw().type() == CV_32FC1 || odom.data().depthRaw().type() == CV_16UC1))
{
if(odom.data().cameraModels().size() > 1)
{
ROS_WARN("Multi-cameras detected in database but this node cannot send multi-images yet...");
}
else
{
//depth
if(odom.data().cameraModels().size())
{
camInfoA.D.resize(5,0);
camInfoA.P[0] = odom.data().cameraModels()[0].fx();
camInfoA.K[0] = odom.data().cameraModels()[0].fx();
camInfoA.P[5] = odom.data().cameraModels()[0].fy();
camInfoA.K[4] = odom.data().cameraModels()[0].fy();
camInfoA.P[2] = odom.data().cameraModels()[0].cx();
camInfoA.K[2] = odom.data().cameraModels()[0].cx();
camInfoA.P[6] = odom.data().cameraModels()[0].cy();
camInfoA.K[5] = odom.data().cameraModels()[0].cy();
camInfoB = camInfoA;
}
type=0;
if(rgbPub.getTopic().empty()) rgbPub = it.advertise("rgb/image", 1);
if(depthPub.getTopic().empty()) depthPub = it.advertise("depth_registered/image", 1);
if(rgbCamInfoPub.getTopic().empty()) rgbCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("rgb/camera_info", 1);
if(depthCamInfoPub.getTopic().empty()) depthCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("depth_registered/camera_info", 1);
}
}
else if(!odom.data().rightRaw().empty() && odom.data().rightRaw().type() == CV_8U)
{
//stereo
if(odom.data().stereoCameraModel().isValidForProjection())
{
camInfoA.D.resize(8,0);
camInfoA.P[0] = odom.data().stereoCameraModel().left().fx();
camInfoA.K[0] = odom.data().stereoCameraModel().left().fx();
camInfoA.P[5] = odom.data().stereoCameraModel().left().fy();
camInfoA.K[4] = odom.data().stereoCameraModel().left().fy();
camInfoA.P[2] = odom.data().stereoCameraModel().left().cx();
camInfoA.K[2] = odom.data().stereoCameraModel().left().cx();
camInfoA.P[6] = odom.data().stereoCameraModel().left().cy();
camInfoA.K[5] = odom.data().stereoCameraModel().left().cy();
camInfoB = camInfoA;
camInfoB.P[3] = odom.data().stereoCameraModel().right().Tx(); // Right_Tx = -baseline*fx
}
type=1;
if(leftPub.getTopic().empty()) leftPub = it.advertise("left/image", 1);
if(rightPub.getTopic().empty()) rightPub = it.advertise("right/image", 1);
if(leftCamInfoPub.getTopic().empty()) leftCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("left/camera_info", 1);
if(rightCamInfoPub.getTopic().empty()) rightCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("right/camera_info", 1);
}
else
{
if(imagePub.getTopic().empty()) imagePub = it.advertise("image", 1);
}
camInfoA.height = odom.data().imageRaw().rows;
camInfoA.width = odom.data().imageRaw().cols;
camInfoB.height = odom.data().depthOrRightRaw().rows;
camInfoB.width = odom.data().depthOrRightRaw().cols;
if(!odom.data().laserScanRaw().empty())
{
if(scanPub.getTopic().empty()) scanPub = nh.advertise<sensor_msgs::LaserScan>("scan", 1);
}
// publish transforms first
if(publishTf)
{
rtabmap::Transform localTransform;
if(odom.data().cameraModels().size() == 1)
{
localTransform = odom.data().cameraModels()[0].localTransform();
}
else if(odom.data().stereoCameraModel().isValidForProjection())
{
localTransform = odom.data().stereoCameraModel().left().localTransform();
}
if(!localTransform.isNull())
{
geometry_msgs::TransformStamped baseToCamera;
baseToCamera.child_frame_id = cameraFrameId;
baseToCamera.header.frame_id = frameId;
baseToCamera.header.stamp = time;
rtabmap_ros::transformToGeometryMsg(localTransform, baseToCamera.transform);
tfBroadcaster.sendTransform(baseToCamera);
}
if(!odom.pose().isNull())
{
geometry_msgs::TransformStamped odomToBase;
odomToBase.child_frame_id = frameId;
odomToBase.header.frame_id = odomFrameId;
odomToBase.header.stamp = time;
rtabmap_ros::transformToGeometryMsg(odom.pose(), odomToBase.transform);
tfBroadcaster.sendTransform(odomToBase);
}
if(!scanPub.getTopic().empty())
{
geometry_msgs::TransformStamped baseToLaserScan;
baseToLaserScan.child_frame_id = scanFrameId;
baseToLaserScan.header.frame_id = frameId;
baseToLaserScan.header.stamp = time;
rtabmap_ros::transformToGeometryMsg(rtabmap::Transform(0,0,scanHeight,0,0,0), baseToLaserScan.transform);
tfBroadcaster.sendTransform(baseToLaserScan);
}
}
if(!odom.pose().isNull())
{
if(odometryPub.getTopic().empty()) odometryPub = nh.advertise<nav_msgs::Odometry>("odom", 1);
if(odometryPub.getNumSubscribers())
{
nav_msgs::Odometry odomMsg;
odomMsg.child_frame_id = frameId;
odomMsg.header.frame_id = odomFrameId;
odomMsg.header.stamp = time;
rtabmap_ros::transformToPoseMsg(odom.pose(), odomMsg.pose.pose);
UASSERT(odomMsg.pose.covariance.size() == 36 &&
odom.covariance().total() == 36 &&
odom.covariance().type() == CV_64FC1);
memcpy(odomMsg.pose.covariance.begin(), odom.covariance().data, 36*sizeof(double));
odometryPub.publish(odomMsg);
}
}
if(type >= 0)
{
if(rgbCamInfoPub.getNumSubscribers() && type == 0)
{
rgbCamInfoPub.publish(camInfoA);
}
if(leftCamInfoPub.getNumSubscribers() && type == 1)
{
leftCamInfoPub.publish(camInfoA);
}
if(depthCamInfoPub.getNumSubscribers() && type == 0)
{
depthCamInfoPub.publish(camInfoB);
}
if(rightCamInfoPub.getNumSubscribers() && type == 1)
{
rightCamInfoPub.publish(camInfoB);
}
}
if(imagePub.getNumSubscribers() || rgbPub.getNumSubscribers() || leftPub.getNumSubscribers())
{
cv_bridge::CvImage img;
if(odom.data().imageRaw().channels() == 1)
{
img.encoding = sensor_msgs::image_encodings::MONO8;
}
else
{
img.encoding = sensor_msgs::image_encodings::BGR8;
}
img.image = odom.data().imageRaw();
sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
imageRosMsg->header.frame_id = cameraFrameId;
imageRosMsg->header.stamp = time;
if(imagePub.getNumSubscribers())
{
imagePub.publish(imageRosMsg);
}
if(rgbPub.getNumSubscribers() && type == 0)
{
rgbPub.publish(imageRosMsg);
}
if(leftPub.getNumSubscribers() && type == 1)
{
leftPub.publish(imageRosMsg);
leftCamInfoPub.publish(camInfoA);
}
}
if(depthPub.getNumSubscribers() && !odom.data().depthRaw().empty() && type==0)
{
cv_bridge::CvImage img;
if(odom.data().depthRaw().type() == CV_32FC1)
{
img.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
}
else
{
img.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
}
img.image = odom.data().depthRaw();
sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
imageRosMsg->header.frame_id = cameraFrameId;
imageRosMsg->header.stamp = time;
depthPub.publish(imageRosMsg);
depthCamInfoPub.publish(camInfoB);
}
if(rightPub.getNumSubscribers() && !odom.data().rightRaw().empty() && type==1)
{
cv_bridge::CvImage img;
img.encoding = sensor_msgs::image_encodings::MONO8;
img.image = odom.data().rightRaw();
sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
imageRosMsg->header.frame_id = cameraFrameId;
imageRosMsg->header.stamp = time;
rightPub.publish(imageRosMsg);
rightCamInfoPub.publish(camInfoB);
}
if(scanPub.getNumSubscribers() && !odom.data().laserScanRaw().empty())
{
//inspired from pointcloud_to_laserscan package
sensor_msgs::LaserScan msg;
msg.header.frame_id = scanFrameId;
msg.header.stamp = time;
msg.angle_min = scanAngleMin;
msg.angle_max = scanAngleMax;
msg.angle_increment = scanAngleIncrement;
msg.time_increment = 0.0;
msg.scan_time = scanTime;
msg.range_min = scanRangeMin;
msg.range_max = scanRangeMax;
uint32_t rangesSize = std::ceil((msg.angle_max - msg.angle_min) / msg.angle_increment);
msg.ranges.assign(rangesSize, 0.0);
const cv::Mat & scan = odom.data().laserScanRaw();
UASSERT(scan.type() == CV_32FC2 || scan.type() == CV_32FC3);
UASSERT(scan.rows == 1);
for (int i=0; i<scan.cols; ++i)
{
cv::Vec2f pos = scan.at<cv::Vec2f>(i);
double range = hypot(pos[0], pos[1]);
if (range >= scanRangeMin && range <=scanRangeMax)
{
double angle = atan2(pos[1], pos[0]);
if (angle >= msg.angle_min && angle <= msg.angle_max)
{
int index = (angle - msg.angle_min) / msg.angle_increment;
if (index>=0 && index<rangesSize && (range < msg.ranges[index] || msg.ranges[index]==0))
{
msg.ranges[index] = range;
}
}
}
}
scanPub.publish(msg);
}
if(odom.data().userDataRaw().type() == CV_8SC1 &&
odom.data().userDataRaw().cols >= 7 && // including null str ending
odom.data().userDataRaw().rows == 1 &&
memcmp(odom.data().userDataRaw().data, "GOAL:", 5) == 0)
{
//GOAL format detected, remove it from the user data and send it as goal event
std::string goalStr = (const char *)odom.data().userDataRaw().data;
if(!goalStr.empty())
{
std::list<std::string> strs = uSplit(goalStr, ':');
if(strs.size() == 2)
{
int goalId = atoi(strs.rbegin()->c_str());
if(goalId > 0)
{
ROS_WARN("Goal %d detected, calling rtabmap's set_goal service!", goalId);
rtabmap_ros::SetGoal setGoalSrv;
setGoalSrv.request.node_id = goalId;
setGoalSrv.request.node_label = "";
if(!ros::service::call("set_goal", setGoalSrv))
{
ROS_ERROR("Can't call \"set_goal\" service");
}
}
}
}
}
ros::spinOnce();
while(ros::ok() && paused)
{
uSleep(100);
ros::spinOnce();
}
timer.restart();
info = rtabmap::CameraInfo();
data = reader.takeImage(&info);
odom = rtabmap::OdometryEvent(data, info.odomPose, info.odomCovariance);
acquisitionTime = timer.ticks();
}
return 0;
}
void
draw_settings(BWAnal *aa) {
int xx= d_set_xx;
int yy= d_set_yy;
int sx= d_set_sx;
int sy= d_set_sy;
int fsx= s_font ? 8 : 6;
int cyy;
int set;
char ch;
char buf[10];
clear_rect(xx, yy, sx, sy, colour[0]);
// Display logo or current setting value
if (c_set < 0) {
int sy2= 2 * disp_font;
int x, y;
for (y= 0; y<sy2; y++)
for (x= 0; x<sx; x++) {
double fx= (0.5 + x - sx/2) / sy2;
double fy= (0.5 + y - sy2/2) / sy2;
double mag= hypot(fx, fy);
double pha= atan2(fx, fy) / 2 / M_PI;
plot_hue(xx+x, yy+y, 1, 0.8 - 0.5 / (1 + mag*1), pha - mag * 0.8 + 0.088);
}
drawtext(disp_font, xx, yy + disp_font/2, "\x94" "BWView");
} else {
int pre;
int mox= sx - (s_font ? 8 : 6) * 2; // Maximum offset-X
buf[0]= '\x98';
buf[1]= set_codes[c_set];
buf[2]= pre= s_preset[c_set];
buf[3]= 0;
if (!isdigit(pre)) {
double fp= set_get(aa, c_set);
for (pre= 1; pre < 10; pre++)
if (fp >= set_preset_values[c_set][pre] &&
fp <= set_preset_values[c_set][(pre+1)%10])
break;
} else if (pre == '0')
pre= 10;
else
pre= (pre-'0');
drawtext(disp_font, xx + mox * (pre-1) / 9, yy, buf);
buf[0]= '\x8C';
set_format(aa, c_set, buf+1);
drawtext(disp_font, xx, yy + disp_font, buf);
}
// Draw main display of settings
for (set= 0; ch= set_codes[set]; set++) {
cyy= yy + (set/2 + 3) * disp_font;
if (set == c_set)
drawtext(disp_font, xx + (set&1)*fsx*3, cyy, "\x91 ");
buf[0]= ch;
buf[1]= s_preset[set];
buf[2]= 0;
drawtext(disp_font, xx + (set&1)*fsx*3 + fsx/2, cyy, buf);
}
update(xx, yy, sx, sy);
}
int LappedUtils::circle_circle_intersection(double x0, double y0, double r0,
double x1, double y1, double r1,
double *xi, double *yi,
double *xi_prime, double *yi_prime)
{
double a, dx, dy, d, h, rx, ry;
double x2, y2;
/* dx and dy are the vertical and horizontal distances between
* the circle centers.
*/
dx = x1 - x0;
dy = y1 - y0;
/* Determine the straight-line distance between the centers. */
//d = sqrt((dy*dy) + (dx*dx));
d = hypot(dx,dy); // Suggested by Keith Briggs
/* Check for solvability. */
if (d > (r0 + r1))
{
/* no solution. circles do not intersect. */
return 0;
}
if (d < fabs(r0 - r1))
{
/* no solution. one circle is contained in the other */
return 0;
}
/* 'point 2' is the point where the line through the circle
* intersection points crosses the line between the circle
* centers.
*/
/* Determine the distance from point 0 to point 2. */
a = ((r0*r0) - (r1*r1) + (d*d)) / (2.0 * d) ;
/* Determine the coordinates of point 2. */
x2 = x0 + (dx * a/d);
y2 = y0 + (dy * a/d);
/* Determine the distance from point 2 to either of the
* intersection points.
*/
h = sqrt((r0*r0) - (a*a));
/* Now determine the offsets of the intersection points from
* point 2.
*/
rx = -dy * (h/d);
ry = dx * (h/d);
/* Determine the absolute intersection points. */
*xi = x2 + rx;
*xi_prime = x2 - rx;
*yi = y2 + ry;
*yi_prime = y2 - ry;
return 1;
}
double
cabs(double complex z)
{
return hypot(__real__ z, __imag__ z);
}
void VectorHistogram::AddVector(float x, float y) {
bins_[NormAngle(x, y) * num_bins_] += hypot(x, y);
++num_vectors_;
}
// Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase
// characters and signed floating point values (no whitespace). Comments and block delete
// characters have been removed. All units and positions are converted and exported to grbl's
// internal functions in terms of (mm, mm/min) and absolute machine coordinates, respectively.
uint8_t gc_execute_line(char *line)
{
// If in alarm state, don't process. Immediately return with error.
// NOTE: Might not be right place for this, but also prevents $N storing during alarm.
if (sys.state == STATE_ALARM)
return(STATUS_ALARM_LOCK);
uint8_t char_counter = 0;
char letter;
float value;
int int_value;
uint16_t modal_group_words = 0; // Bitflag variable to track and check modal group words in block
uint8_t axis_words = 0; // Bitflag to track which XYZ(ABC) parameters exist in block
float inverse_feed_rate = -1; // negative inverse_feed_rate means no inverse_feed_rate specified
uint8_t absolute_override = false; // true(1) = absolute motion for this block only {G53}
uint8_t non_modal_action = NON_MODAL_NONE; // Tracks the actions of modal group 0 (non-modal)
/// 8c0
float target[N_AXIS], offset[N_AXIS];
clear_vector(target); // XYZ(ABC) axes parameters.
clear_vector(offset); // IJK Arc offsets are incremental. Value of zero indicates no change.
gc.status_code = STATUS_OK;
/* Pass 1: Commands and set all modes. Check for modal group violations.
NOTE: Modal group numbers are defined in Table 4 of NIST RS274-NGC v3, pg.20 */
uint8_t group_number = MODAL_GROUP_NONE;
while(next_statement(&letter, &value, line, &char_counter)) {
int_value = trunc(value);
switch(letter) {
case 'G':
// Set modal group values
switch(int_value) {
case 4: case 10: case 28: case 30: case 53: case 92: group_number = MODAL_GROUP_0; break;
case 0: case 1: case 2: case 3: case 80: group_number = MODAL_GROUP_1; break;
case 17: case 18: case 19: group_number = MODAL_GROUP_2; break;
case 90: case 91: group_number = MODAL_GROUP_3; break;
case 93: case 94: group_number = MODAL_GROUP_5; break;
case 20: case 21: group_number = MODAL_GROUP_6; break;
case 54: case 55: case 56: case 57: case 58: case 59: group_number = MODAL_GROUP_12; break;
}
// Set 'G' commands
switch(int_value) {
case 0: gc.motion_mode = MOTION_MODE_SEEK; break;
case 1: gc.motion_mode = MOTION_MODE_LINEAR; break;
case 2: gc.motion_mode = MOTION_MODE_CW_ARC; break;
case 3: gc.motion_mode = MOTION_MODE_CCW_ARC; break;
case 4: non_modal_action = NON_MODAL_DWELL; break;
case 10: non_modal_action = NON_MODAL_SET_COORDINATE_DATA; break;
case 17: select_plane(X_AXIS, Y_AXIS, Z_AXIS); break;
case 18: select_plane(Z_AXIS, X_AXIS, Y_AXIS); break;
case 19: select_plane(Y_AXIS, Z_AXIS, X_AXIS); break;
case 20: gc.inches_mode = true; break;
case 21: gc.inches_mode = false; break;
case 28: case 30:
int_value = trunc(10*value); // Multiply by 10 to pick up Gxx.1
switch(int_value) {
case 280: non_modal_action = NON_MODAL_GO_HOME_0; break;
case 281: non_modal_action = NON_MODAL_SET_HOME_0; break;
case 300: non_modal_action = NON_MODAL_GO_HOME_1; break;
case 301: non_modal_action = NON_MODAL_SET_HOME_1; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
case 53: absolute_override = true; break;
case 54: case 55: case 56: case 57: case 58: case 59:
gc.coord_select = int_value-54;
break;
case 80: gc.motion_mode = MOTION_MODE_CANCEL; break;
case 90: gc.absolute_mode = true; break;
case 91: gc.absolute_mode = false; break;
case 92:
int_value = trunc(10*value); // Multiply by 10 to pick up G92.1
switch(int_value) {
case 920: non_modal_action = NON_MODAL_SET_COORDINATE_OFFSET; break;
case 921: non_modal_action = NON_MODAL_RESET_COORDINATE_OFFSET; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
case 93: gc.inverse_feed_rate_mode = true; break;
case 94: gc.inverse_feed_rate_mode = false; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
case 'M':
// Set modal group values
switch(int_value) {
case 0: case 1: case 2: case 30: group_number = MODAL_GROUP_4; break;
case 3: case 4: case 5: group_number = MODAL_GROUP_7; break;
}
// Set 'M' commands
switch(int_value) {
case 0: gc.program_flow = PROGRAM_FLOW_PAUSED; break; // Program pause
case 1: break; // Optional stop not supported. Ignore.
case 2: case 30: gc.program_flow = PROGRAM_FLOW_COMPLETED; break; // Program end and reset
case 3: gc.spindle_direction = 1; break;
case 4: gc.spindle_direction = -1; break;
case 5: gc.spindle_direction = 0; break;
#ifdef ENABLE_M7
case 7: gc.coolant_mode = COOLANT_MIST_ENABLE; break;
#endif
case 8: gc.coolant_mode = COOLANT_FLOOD_ENABLE; break;
case 9: gc.coolant_mode = COOLANT_DISABLE; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
}
// Check for modal group multiple command violations in the current block
if (group_number) {
if ( bit_istrue(modal_group_words,bit(group_number)) ) {
FAIL(STATUS_MODAL_GROUP_VIOLATION);
} else {
bit_true(modal_group_words,bit(group_number));
}
group_number = MODAL_GROUP_NONE; // Reset for next command.
}
}
// If there were any errors parsing this line, we will return right away with the bad news
if (gc.status_code)
return gc.status_code;
/* Pass 2: Parameters. All units converted according to current block commands. Position
parameters are converted and flagged to indicate a change. These can have multiple connotations
for different commands. Each will be converted to their proper value upon execution. */
float p = 0, r = 0;
uint8_t l = 0;
char_counter = 0;
while(next_statement(&letter, &value, line, &char_counter)) {
switch(letter) {
case 'G': case 'M': case 'N': break; // Ignore command statements and line numbers
case 'F':
if (value <= 0)
FAIL(STATUS_INVALID_STATEMENT); // Must be greater than zero
if (gc.inverse_feed_rate_mode) {
inverse_feed_rate = to_millimeters(value); // seconds per motion for this motion only
}
else {
gc.feed_rate = to_millimeters(value); // millimeters per minute
}
break;
case 'I': case 'J': case 'K':
offset[letter-'I'] = to_millimeters(value);
break;
case 'L': l =
trunc(value);
break;
case 'P':
p = value;
break;
case 'R':
r = to_millimeters(value);
break;
case 'S':
if (value < 0)
FAIL(STATUS_INVALID_STATEMENT); // Cannot be negative
// TBD: Spindle speed not supported due to PWM issues, but may come back once resolved.
// gc.spindle_speed = value;
break;
case 'T':
if (value < 0)
FAIL(STATUS_INVALID_STATEMENT); // Cannot be negative
gc.tool = trunc(value);
break;
case 'X':
target[X_AXIS] = to_millimeters(value);
bit_true(axis_words,bit(X_AXIS));
break;
case 'Y':
target[Y_AXIS] = to_millimeters(value);
bit_true(axis_words,bit(Y_AXIS));
break;
case 'Z':
target[Z_AXIS] = to_millimeters(value);
bit_true(axis_words,bit(Z_AXIS));
break;
/// 8c1
#if (AXIS_T_TYPE == LINEAR)
/// axis U, V, W choice
#if (AXIS_T == AXIS_U)
case 'U':
#endif
#if (AXIS_T == AXIS_V)
case 'V' :
#endif
#if (AXIS_T == AXIS_W)
case 'W' :
#endif
#if ((AXIS_T == AXIS_U) || (AXIS_T == AXIS_V) || (AXIS_T == AXIS_W))
target[T_AXIS] = to_millimeters(value);
bit_true(axis_words,bit(T_AXIS));
break;
#else
#error
#endif
#elif (AXIS_T_TYPE == ROTARY)
/// axis A, B, C choice
#if (AXIS_T == AXIS_A)
case 'A':
#elif (AXIS_T == AXIS_B)
case 'B':
#elif (AXIS_T == AXIS_C)
case 'C':
#endif
#if ((AXIS_T == AXIS_A) || (AXIS_T == AXIS_B) || (AXIS_T == AXIS_C))
target[T_AXIS] = to_degrees(value);
bit_true(axis_words,bit(T_AXIS));
#else
#error
#endif
break;
#else
#error
#endif
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
}
// If there were any errors parsing this line, we will return right away with the bad news
if (gc.status_code)
return gc.status_code;
/* Execute Commands: Perform by order of execution defined in NIST RS274-NGC.v3, Table 8, pg.41.
NOTE: Independent non-motion/settings parameters are set out of this order for code efficiency
and simplicity purposes, but this should not affect proper g-code execution. */
// ([F]: Set feed and seek rates.)
// TODO: Seek rates can change depending on the direction and maximum speeds of each axes. When
// max axis speed is installed, the calculation can be performed here, or maybe in the planner.
if (sys.state != STATE_CHECK_MODE) {
// ([M6]: Tool change should be executed here.)
// [M3,M4,M5]: Update spindle state
spindle_run(gc.spindle_direction);
// [*M7,M8,M9]: Update coolant state
coolant_run(gc.coolant_mode);
}
// [G54,G55,...,G59]: Coordinate system selection
if ( bit_istrue(modal_group_words,bit(MODAL_GROUP_12)) ) { // Check if called in block
float coord_data[N_AXIS];
if (!(settings_read_coord_data(gc.coord_select,coord_data)))
return(STATUS_SETTING_READ_FAIL);
memcpy(gc.coord_system,coord_data,sizeof(coord_data));
}
// [G4,G10,G28,G30,G92,G92.1]: Perform dwell, set coordinate system data, homing, or set axis offsets.
// NOTE: These commands are in the same modal group, hence are mutually exclusive. G53 is in this
// modal group and do not effect these actions.
switch (non_modal_action) {
case NON_MODAL_DWELL:
if (p < 0) { // Time cannot be negative.
FAIL(STATUS_INVALID_STATEMENT);
}
else {
// Ignore dwell in check gcode modes
if (sys.state != STATE_CHECK_MODE)
mc_dwell(p);
}
break;
case NON_MODAL_SET_COORDINATE_DATA:
int_value = trunc(p); // Convert p value to int.
if ((l != 2 && l != 20) || (int_value < 0 || int_value > N_COORDINATE_SYSTEM)) { // L2 and L20. P1=G54, P2=G55, ...
FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
else
if (!axis_words && l==2) {// No axis words.
FAIL(STATUS_INVALID_STATEMENT);
}
else {
if (int_value > 0)
int_value--; // Adjust P1-P6 index to EEPROM coordinate data indexing.
else
int_value = gc.coord_select; // Index P0 as the active coordinate system
float coord_data[N_AXIS];
if (!settings_read_coord_data(int_value,coord_data))
return(STATUS_SETTING_READ_FAIL);
uint8_t i;
// Update axes defined only in block. Always in machine coordinates. Can change non-active system.
for (i=0; i<N_AXIS; i++) { // Axes indices are consistent, so loop may be used.
if (bit_istrue(axis_words,bit(i)) ) {
if (l == 20) {
coord_data[i] = gc.position[i]-target[i]; // L20: Update axis current position to target
}
else {
coord_data[i] = target[i]; // L2: Update coordinate system axis
}
}
}
settings_write_coord_data(int_value,coord_data);
// Update system coordinate system if currently active.
if (gc.coord_select == int_value)
memcpy(gc.coord_system,coord_data,sizeof(coord_data));
}
axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags.
break;
case NON_MODAL_GO_HOME_0: case NON_MODAL_GO_HOME_1:
// Move to intermediate position before going home. Obeys current coordinate system and offsets
// and absolute and incremental modes.
if (axis_words) {
// Apply absolute mode coordinate offsets or incremental mode offsets.
uint8_t i;
for (i=0; i<N_AXIS; i++) { // Axes indices are consistent, so loop may be used.
if ( bit_istrue(axis_words,bit(i)) ) {
if (gc.absolute_mode) {
target[i] += gc.coord_system[i] + gc.coord_offset[i];
}
else {
target[i] += gc.position[i];
}
}
else {
target[i] = gc.position[i];
}
}
/// 8c1 : line
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[T_AXIS], settings.default_seek_rate, false, C_LINE);
}
// Retreive G28/30 go-home position data (in machine coordinates) from EEPROM
float coord_data[N_AXIS];
if (non_modal_action == NON_MODAL_GO_HOME_1) {
if (!settings_read_coord_data(SETTING_INDEX_G30 ,coord_data))
return STATUS_SETTING_READ_FAIL;
}
else {
if (!settings_read_coord_data(SETTING_INDEX_G28 ,coord_data))
return(STATUS_SETTING_READ_FAIL);
}
///8c1 : line
mc_line(coord_data[X_AXIS], coord_data[Y_AXIS], coord_data[Z_AXIS], coord_data[T_AXIS], settings.default_seek_rate, false, C_LINE);
memcpy(gc.position, coord_data, sizeof(coord_data)); // gc.position[] = coord_data[];
axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags.
break;
case NON_MODAL_SET_HOME_0: case NON_MODAL_SET_HOME_1:
if (non_modal_action == NON_MODAL_SET_HOME_1) {
settings_write_coord_data(SETTING_INDEX_G30,gc.position);
}
else {
settings_write_coord_data(SETTING_INDEX_G28,gc.position);
}
break;
case NON_MODAL_SET_COORDINATE_OFFSET:
if (!axis_words) { // No axis words
FAIL(STATUS_INVALID_STATEMENT);
}
else {
// Update axes defined only in block. Offsets current system to defined value. Does not update when
// active coordinate system is selected, but is still active unless G92.1 disables it.
uint8_t i;
/// 8c0 : 2 -> N_AXIS
for (i=0; i<N_AXIS; i++) { // Axes indices are consistent, so loop may be used.
if (bit_istrue(axis_words,bit(i)) ) {
gc.coord_offset[i] = gc.position[i]-gc.coord_system[i]-target[i];
}
}
}
axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags.
break;
case NON_MODAL_RESET_COORDINATE_OFFSET:
clear_vector(gc.coord_offset); // Disable G92 offsets by zeroing offset vector.
break;
}
// [G0,G1,G2,G3,G80]: Perform motion modes.
// NOTE: Commands G10,G28,G30,G92 lock out and prevent axis words from use in motion modes.
// Enter motion modes only if there are axis words or a motion mode command word in the block.
if ( bit_istrue(modal_group_words,bit(MODAL_GROUP_1)) || axis_words ) {
// G1,G2,G3 require F word in inverse time mode.
if ( gc.inverse_feed_rate_mode ) {
if (inverse_feed_rate < 0 && gc.motion_mode != MOTION_MODE_CANCEL)
FAIL(STATUS_INVALID_STATEMENT);
}
// Absolute override G53 only valid with G0 and G1 active.
if ( absolute_override && !(gc.motion_mode == MOTION_MODE_SEEK || gc.motion_mode == MOTION_MODE_LINEAR))
FAIL(STATUS_INVALID_STATEMENT);
// Report any errors.
if (gc.status_code)
return(gc.status_code);
// Convert all target position data to machine coordinates for executing motion. Apply
// absolute mode coordinate offsets or incremental mode offsets.
// NOTE: Tool offsets may be appended to these conversions when/if this feature is added.
uint8_t i;
/// 8c0
for (i=0; i< N_AXIS; i++) { // Axes indices are consistent, so loop may be used to save flash space.
if ( bit_istrue(axis_words,bit(i)) ) {
if (!absolute_override) { // Do not update target in absolute override mode
if (gc.absolute_mode) {
target[i] += gc.coord_system[i] + gc.coord_offset[i]; // Absolute mode
}
else {
target[i] += gc.position[i]; // Incremental mode
}
}
}
else {
target[i] = gc.position[i]; // No axis word in block. Keep same axis position.
}
}
switch (gc.motion_mode) {
case MOTION_MODE_CANCEL:
if (axis_words)
FAIL(STATUS_INVALID_STATEMENT); // No axis words allowed while active.
break;
case MOTION_MODE_SEEK:
if (!axis_words) {
FAIL(STATUS_INVALID_STATEMENT);
}
else
/// 8c1 :line
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[T_AXIS],
settings.default_seek_rate, false, C_LINE);
break;
case MOTION_MODE_LINEAR:
// TODO: Inverse time requires F-word with each statement. Need to do a check. Also need
// to check for initial F-word upon startup. Maybe just set to zero upon initialization
// and after an inverse time move and then check for non-zero feed rate each time. This
// should be efficient and effective.
if (!axis_words) {
FAIL(STATUS_INVALID_STATEMENT);
}
else {
/// 8c1 :line
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[T_AXIS],
(gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode, C_LINE);
}
break;
case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC:
// Check if at least one of the axes of the selected plane has been specified. If in center
// format arc mode, also check for at least one of the IJK axes of the selected plane was sent.
if ( !( bit_false(axis_words,bit(gc.plane_axis_2)) ) ||
( !r && !offset[gc.plane_axis_0] && !offset[gc.plane_axis_1] ) ) {
FAIL(STATUS_INVALID_STATEMENT);
} else {
if (r != 0) { // Arc Radius Mode
/*
We need to calculate the center of the circle that has the designated radius and passes
through both the current position and the target position. This method calculates the following
set of equations where [x,y] is the vector from current to target position, d == magnitude of
that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to
the center of the travel vector. A vector perpendicular to the travel vector [-y,x] is scaled to the
length of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] to form the new point
[i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the center of our arc.
d^2 == x^2 + y^2
h^2 == r^2 - (d/2)^2
i == x/2 - y/d*h
j == y/2 + x/d*h
O <- [i,j]
- |
r - |
- |
- | h
- |
[0,0] -> C -----------------+--------------- T <- [x,y]
| <------ d/2 ---->|
C - Current position
T - Target position
O - center of circle that pass through both C and T
d - distance from C to T
r - designated radius
h - distance from center of CT to O
Expanding the equations:
d -> sqrt(x^2 + y^2)
h -> sqrt(4 * r^2 - x^2 - y^2)/2
i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
Which can be written:
i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
Which we for size and speed reasons optimize to:
h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2)
i = (x - (y * h_x2_div_d))/2
j = (y + (x * h_x2_div_d))/2
*/
// Calculate the change in position along each selected axis
float x = target[gc.plane_axis_0]-gc.position[gc.plane_axis_0];
float y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1];
clear_vector(offset);
// First, use h_x2_div_d to compute 4*h^2 to check if it is negative or r is smaller
// than d. If so, the sqrt of a negative number is complex and error out.
float h_x2_div_d = 4 * r*r - x*x - y*y;
if (h_x2_div_d < 0) { FAIL(STATUS_ARC_RADIUS_ERROR); return(gc.status_code); }
// Finish computing h_x2_div_d.
h_x2_div_d = -sqrt(h_x2_div_d)/hypot(x,y); // == -(h * 2 / d)
// Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below)
if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; }
/* The counter clockwise circle lies to the left of the target direction. When offset is positive,
the left hand circle will be generated - when it is negative the right hand circle is generated.
T <-- Target position
^
Clockwise circles with this center | Clockwise circles with this center will have
will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing!
\ | /
center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative
|
|
C <-- Current position */
// Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!),
// even though it is advised against ever generating such circles in a single line of g-code. By
// inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of
// travel and thus we get the unadvisably long arcs as prescribed.
if (r < 0) {
h_x2_div_d = -h_x2_div_d;
r = -r; // Finished with r. Set to positive for mc_arc
}
// Complete the operation by calculating the actual center of the arc
offset[gc.plane_axis_0] = 0.5*(x-(y*h_x2_div_d));
offset[gc.plane_axis_1] = 0.5*(y+(x*h_x2_div_d));
} else { // Arc Center Format Offset Mode
r = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]); // Compute arc radius for mc_arc
}
// Set clockwise/counter-clockwise sign for mc_arc computations
uint8_t isclockwise = false;
if (gc.motion_mode == MOTION_MODE_CW_ARC) { isclockwise = true; }
// Trace the arc
mc_arc(gc.position, target, offset, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2,
(gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode,
r, isclockwise);
}
break;
}
// Report any errors.
if (gc.status_code) { return(gc.status_code); }
// As far as the parser is concerned, the position is now == target. In reality the
// motion control system might still be processing the action and the real tool position
// in any intermediate location.
memcpy(gc.position, target, sizeof(target)); // gc.position[] = target[];
}
// M0,M1,M2,M30: Perform non-running program flow actions. During a program pause, the buffer may
// refill and can only be resumed by the cycle start run-time command.
if (gc.program_flow) {
plan_synchronize(); // Finish all remaining buffered motions. Program paused when complete.
sys.auto_start = false; // Disable auto cycle start. Forces pause until cycle start issued.
// If complete, reset to reload defaults (G92.2,G54,G17,G90,G94,M48,G40,M5,M9). Otherwise,
// re-enable program flow after pause complete, where cycle start will resume the program.
if (gc.program_flow == PROGRAM_FLOW_COMPLETED) { mc_reset(); }
else { gc.program_flow = PROGRAM_FLOW_RUNNING; }
}
return(gc.status_code);
}
int
gsl_sf_legendre_H3d_e(const int ell, const double lambda, const double eta,
gsl_sf_result * result)
{
const double abs_lam = fabs(lambda);
const double lsq = abs_lam*abs_lam;
const double xi = abs_lam * eta;
const double cosh_eta = cosh(eta);
/* CHECK_POINTER(result) */
if(eta < 0.0) {
DOMAIN_ERROR(result);
}
else if(eta > GSL_LOG_DBL_MAX) {
/* cosh(eta) is too big. */
OVERFLOW_ERROR(result);
}
else if(ell == 0) {
return gsl_sf_legendre_H3d_0_e(lambda, eta, result);
}
else if(ell == 1) {
return gsl_sf_legendre_H3d_1_e(lambda, eta, result);
}
else if(eta == 0.0) {
result->val = 0.0;
result->err = 0.0;
return GSL_SUCCESS;
}
else if(xi < 1.0) {
return legendre_H3d_series(ell, lambda, eta, result);
}
else if((ell*ell+lsq)/sqrt(1.0+lsq)/(cosh_eta*cosh_eta) < 5.0*GSL_ROOT3_DBL_EPSILON) {
/* Large argument.
*/
gsl_sf_result P;
double lm;
int stat_P = gsl_sf_conicalP_large_x_e(-ell-0.5, lambda, cosh_eta, &P, &lm);
if(P.val == 0.0) {
result->val = 0.0;
result->err = 0.0;
return stat_P;
}
else {
double lnN;
gsl_sf_result lnsh;
double ln_abslam;
double lnpre_val, lnpre_err;
int stat_e;
gsl_sf_lnsinh_e(eta, &lnsh);
legendre_H3d_lnnorm(ell, lambda, &lnN);
ln_abslam = log(abs_lam);
lnpre_val = 0.5*(M_LNPI + lnN - M_LN2 - lnsh.val) - ln_abslam;
lnpre_err = lnsh.err;
lnpre_err += 2.0 * GSL_DBL_EPSILON * (0.5*(M_LNPI + M_LN2 + fabs(lnN)) + fabs(ln_abslam));
lnpre_err += 2.0 * GSL_DBL_EPSILON * fabs(lnpre_val);
stat_e = gsl_sf_exp_mult_err_e(lnpre_val + lm, lnpre_err, P.val, P.err, result);
return GSL_ERROR_SELECT_2(stat_e, stat_P);
}
}
else if(abs_lam > 1000.0*ell*ell) {
/* Large degree.
*/
gsl_sf_result P;
double lm;
int stat_P = gsl_sf_conicalP_xgt1_neg_mu_largetau_e(ell+0.5,
lambda,
cosh_eta, eta,
&P, &lm);
if(P.val == 0.0) {
result->val = 0.0;
result->err = 0.0;
return stat_P;
}
else {
double lnN;
gsl_sf_result lnsh;
double ln_abslam;
double lnpre_val, lnpre_err;
int stat_e;
gsl_sf_lnsinh_e(eta, &lnsh);
legendre_H3d_lnnorm(ell, lambda, &lnN);
ln_abslam = log(abs_lam);
lnpre_val = 0.5*(M_LNPI + lnN - M_LN2 - lnsh.val) - ln_abslam;
lnpre_err = lnsh.err;
lnpre_err += GSL_DBL_EPSILON * (0.5*(M_LNPI + M_LN2 + fabs(lnN)) + fabs(ln_abslam));
lnpre_err += 2.0 * GSL_DBL_EPSILON * fabs(lnpre_val);
stat_e = gsl_sf_exp_mult_err_e(lnpre_val + lm, lnpre_err, P.val, P.err, result);
return GSL_ERROR_SELECT_2(stat_e, stat_P);
}
}
else {
/* Backward recurrence.
*/
const double coth_eta = 1.0/tanh(eta);
const double coth_err_mult = fabs(eta) + 1.0;
gsl_sf_result rH;
int stat_CF1 = legendre_H3d_CF1_ser(ell, lambda, coth_eta, &rH);
double Hlm1;
double Hl = GSL_SQRT_DBL_MIN;
double Hlp1 = rH.val * Hl;
int lp;
for(lp=ell; lp>0; lp--) {
double root_term_0 = hypot(lambda,lp);
double root_term_1 = hypot(lambda,lp+1.0);
Hlm1 = ((2.0*lp + 1.0)*coth_eta*Hl - root_term_1 * Hlp1)/root_term_0;
Hlp1 = Hl;
Hl = Hlm1;
}
if(fabs(Hl) > fabs(Hlp1)) {
gsl_sf_result H0;
int stat_H0 = gsl_sf_legendre_H3d_0_e(lambda, eta, &H0);
result->val = GSL_SQRT_DBL_MIN/Hl * H0.val;
result->err = GSL_SQRT_DBL_MIN/fabs(Hl) * H0.err;
result->err += fabs(rH.err/rH.val) * (ell+1.0) * coth_err_mult * fabs(result->val);
result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
return GSL_ERROR_SELECT_2(stat_H0, stat_CF1);
}
else {
gsl_sf_result H1;
int stat_H1 = gsl_sf_legendre_H3d_1_e(lambda, eta, &H1);
result->val = GSL_SQRT_DBL_MIN/Hlp1 * H1.val;
result->err = GSL_SQRT_DBL_MIN/fabs(Hlp1) * H1.err;
result->err += fabs(rH.err/rH.val) * (ell+1.0) * coth_err_mult * fabs(result->val);
result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
return GSL_ERROR_SELECT_2(stat_H1, stat_CF1);
}
}
}
double G_vector_norm_maxval(vec_struct * vc, int vflag)
{
doublereal xval, *startpt, *curpt;
double cellval;
int ncells, incr;
if (!vc->is_init)
G_fatal_error(_("Matrix is not initialised"));
if (vc->type == ROWVEC_) {
ncells = (integer) vc->cols;
incr = (integer) vc->ldim;
if (vc->v_indx < 0)
startpt = vc->vals;
else
startpt = vc->vals + vc->v_indx;
}
else {
ncells = (integer) vc->rows;
incr = 1;
if (vc->v_indx < 0)
startpt = vc->vals;
else
startpt = vc->vals + vc->v_indx * vc->ldim;
}
xval = *startpt;
curpt = startpt;
while (ncells > 0) {
if (curpt != startpt) {
switch (vflag) {
case MAX_POS:
{
if (*curpt > xval)
xval = *curpt;
break;
}
case MAX_NEG:
{
if (*curpt < xval)
xval = *curpt;
break;
}
case MAX_ABS:
{
cellval = (double)(*curpt);
if (hypot(cellval, cellval) > (double)xval)
xval = *curpt;
}
} /* switch */
} /* if(curpt != startpt) */
curpt += incr;
ncells--;
}
return (double)xval;
}
static void eml_qrsolve(const emxArray_real_T *A, emxArray_real_T *B, double Y[2],
double *rankR)
{
emxArray_real_T *b_A;
int m;
int mn;
int i6;
int itemp;
int b_m;
int b_mn;
double tau_data[2];
signed char jpvt[2];
double work[2];
int i;
int k;
double vn1[2];
double vn2[2];
int pvt;
double smax;
int i_i;
int mmi;
double temp2;
int ix;
int iy;
double atmp;
int lastv;
int lastc;
boolean_T exitg2;
int32_T exitg1;
double t;
unsigned int b_i;
emxInit_real_T(&b_A, 2);
m = A->size[0] - 2;
mn = (int)fmin(A->size[0], 2.0) - 1;
i6 = b_A->size[0] * b_A->size[1];
b_A->size[0] = A->size[0];
b_A->size[1] = 2;
emxEnsureCapacity((emxArray__common *)b_A, i6, (int)sizeof(double));
itemp = A->size[0] * A->size[1];
for (i6 = 0; i6 < itemp; i6++) {
b_A->data[i6] = A->data[i6];
}
b_m = b_A->size[0];
if (b_A->size[0] <= 2) {
b_mn = b_A->size[0];
} else {
b_mn = 2;
}
for (i6 = 0; i6 < 2; i6++) {
jpvt[i6] = (signed char)(1 + i6);
}
if (b_A->size[0] == 0) {
} else {
for (i = 0; i < 2; i++) {
work[i] = 0.0;
}
k = 1;
for (pvt = 0; pvt < 2; pvt++) {
smax = eml_xnrm2(b_m, b_A, k);
vn2[pvt] = smax;
k += b_m;
vn1[pvt] = smax;
}
for (i = 0; i + 1 <= b_mn; i++) {
i_i = i + i * b_m;
mmi = (b_m - i) - 1;
itemp = 0;
if (2 - i > 1) {
smax = fabs(vn1[i]);
k = 2;
while (k <= 2 - i) {
temp2 = fabs(vn1[1]);
if (temp2 > smax) {
itemp = 1;
smax = temp2;
}
k = 3;
}
}
pvt = i + itemp;
if (pvt + 1 != i + 1) {
ix = b_m * pvt;
iy = b_m * i;
for (k = 1; k <= b_m; k++) {
smax = b_A->data[ix];
b_A->data[ix] = b_A->data[iy];
b_A->data[iy] = smax;
ix++;
iy++;
}
itemp = jpvt[pvt];
jpvt[pvt] = jpvt[i];
jpvt[i] = (signed char)itemp;
vn1[pvt] = vn1[i];
vn2[pvt] = vn2[i];
}
if (i + 1 < b_m) {
atmp = b_A->data[i_i];
temp2 = 0.0;
if (1 + mmi <= 0) {
} else {
smax = b_eml_xnrm2(mmi, b_A, i_i + 2);
if (smax != 0.0) {
smax = hypot(b_A->data[i_i], smax);
if (b_A->data[i_i] >= 0.0) {
smax = -smax;
}
if (fabs(smax) < 1.0020841800044864E-292) {
pvt = 0;
do {
pvt++;
eml_xscal(mmi, 9.9792015476736E+291, b_A, i_i + 2);
smax *= 9.9792015476736E+291;
atmp *= 9.9792015476736E+291;
} while (!(fabs(smax) >= 1.0020841800044864E-292));
smax = b_eml_xnrm2(mmi, b_A, i_i + 2);
smax = hypot(atmp, smax);
if (atmp >= 0.0) {
smax = -smax;
}
temp2 = (smax - atmp) / smax;
eml_xscal(mmi, 1.0 / (atmp - smax), b_A, i_i + 2);
for (k = 1; k <= pvt; k++) {
smax *= 1.0020841800044864E-292;
}
atmp = smax;
} else {
temp2 = (smax - b_A->data[i_i]) / smax;
eml_xscal(mmi, 1.0 / (b_A->data[i_i] - smax), b_A, i_i + 2);
atmp = smax;
}
}
}
tau_data[i] = temp2;
} else {
atmp = b_A->data[i_i];
tau_data[i] = eml_matlab_zlarfg();
}
b_A->data[i_i] = atmp;
if (i + 1 < 2) {
atmp = b_A->data[i_i];
b_A->data[i_i] = 1.0;
if (tau_data[0] != 0.0) {
lastv = mmi + 1;
itemp = i_i + mmi;
while ((lastv > 0) && (b_A->data[itemp] == 0.0)) {
lastv--;
itemp--;
}
lastc = 1;
exitg2 = false;
while ((!exitg2) && (lastc > 0)) {
itemp = b_m + 1;
do {
exitg1 = 0;
if (itemp <= b_m + lastv) {
if (b_A->data[itemp - 1] != 0.0) {
exitg1 = 1;
} else {
itemp++;
}
} else {
lastc = 0;
exitg1 = 2;
}
} while (exitg1 == 0);
if (exitg1 == 1) {
exitg2 = true;
}
}
} else {
lastv = 0;
lastc = 0;
}
if (lastv > 0) {
if (lastc == 0) {
} else {
work[0] = 0.0;
iy = 0;
pvt = 1 + b_m;
while ((b_m > 0) && (pvt <= b_m + 1)) {
ix = i_i;
smax = 0.0;
i6 = (pvt + lastv) - 1;
for (itemp = pvt; itemp <= i6; itemp++) {
smax += b_A->data[itemp - 1] * b_A->data[ix];
ix++;
}
work[iy] += smax;
iy++;
pvt += b_m;
}
}
if (-tau_data[0] == 0.0) {
} else {
k = b_m;
iy = 0;
pvt = 1;
while (pvt <= lastc) {
if (work[iy] != 0.0) {
smax = work[iy] * -tau_data[0];
ix = i_i;
i6 = lastv + k;
for (itemp = k; itemp + 1 <= i6; itemp++) {
b_A->data[itemp] += b_A->data[ix] * smax;
ix++;
}
}
iy++;
k += b_m;
pvt = 2;
}
}
}
b_A->data[i_i] = atmp;
}
pvt = i + 2;
while (pvt < 3) {
itemp = (i + b_m) + 1;
if (vn1[1] != 0.0) {
smax = fabs(b_A->data[i + b_A->size[0]]) / vn1[1];
smax = 1.0 - smax * smax;
if (smax < 0.0) {
smax = 0.0;
}
temp2 = vn1[1] / vn2[1];
temp2 = smax * (temp2 * temp2);
if (temp2 <= 1.4901161193847656E-8) {
if (i + 1 < b_m) {
smax = 0.0;
if (mmi < 1) {
} else if (mmi == 1) {
smax = fabs(b_A->data[itemp]);
} else {
temp2 = 2.2250738585072014E-308;
pvt = itemp + mmi;
while (itemp + 1 <= pvt) {
atmp = fabs(b_A->data[itemp]);
if (atmp > temp2) {
t = temp2 / atmp;
smax = 1.0 + smax * t * t;
temp2 = atmp;
} else {
t = atmp / temp2;
smax += t * t;
}
itemp++;
}
smax = temp2 * sqrt(smax);
}
vn1[1] = smax;
vn2[1] = smax;
} else {
vn1[1] = 0.0;
vn2[1] = 0.0;
}
} else {
vn1[1] *= sqrt(smax);
}
}
pvt = 3;
}
}
}
*rankR = 0.0;
if (mn + 1 > 0) {
smax = fmax(A->size[0], 2.0) * fabs(b_A->data[0]) * 2.2204460492503131E-16;
k = 0;
while ((k <= mn) && (!(fabs(b_A->data[k + b_A->size[0] * k]) <= smax))) {
(*rankR)++;
k++;
}
}
for (i = 0; i < 2; i++) {
Y[i] = 0.0;
}
for (pvt = 0; pvt <= mn; pvt++) {
if (tau_data[pvt] != 0.0) {
smax = B->data[pvt];
i6 = m - pvt;
for (i = 0; i <= i6; i++) {
b_i = ((unsigned int)pvt + i) + 2U;
smax += b_A->data[((int)b_i + b_A->size[0] * pvt) - 1] * B->data[(int)
b_i - 1];
}
smax *= tau_data[pvt];
if (smax != 0.0) {
B->data[pvt] -= smax;
i6 = m - pvt;
for (i = 0; i <= i6; i++) {
b_i = ((unsigned int)pvt + i) + 2U;
B->data[(int)b_i - 1] -= b_A->data[((int)b_i + b_A->size[0] * pvt) - 1]
* smax;
}
}
}
}
for (i = 0; i <= mn; i++) {
Y[jpvt[i] - 1] = B->data[i];
}
for (pvt = 0; pvt <= mn; pvt++) {
itemp = mn - pvt;
Y[jpvt[itemp] - 1] /= b_A->data[itemp + b_A->size[0] * itemp];
i = 0;
while (i <= itemp - 1) {
Y[jpvt[0] - 1] -= Y[jpvt[itemp] - 1] * b_A->data[b_A->size[0] * itemp];
i = 1;
}
}
emxFree_real_T(&b_A);
}
/*!
\brief Calculate line distance.
Sets (if not null):
- px, py - point on line,
- dist - distance to line,
- spdist - distance to point on line from segment beginning,
- sldist - distance to point on line form line beginning along line
\param points pointer to line_pnts structure
\param ux,uy,uz point coordinates
\param with_z flag if to use z coordinate (3D calculation)
\param[out] px,py,pz point on line
\param[out] dist distance to line,
\param[out] spdist distance of point from segment beginning
\param[out] lpdist distance of point from line
\return nearest segment (first is 1)
*/
int Vect_line_distance(const struct line_pnts *points,
double ux, double uy, double uz,
int with_z,
double *px, double *py, double *pz,
double *dist, double *spdist, double *lpdist)
{
int i;
double distance;
double new_dist;
double tpx, tpy, tpz, tdist, tspdist, tlpdist = 0;
double dx, dy, dz;
int n_points;
int segment;
n_points = points->n_points;
if (n_points == 1) {
distance =
dig_distance2_point_to_line(ux, uy, uz, points->x[0],
points->y[0], points->z[0],
points->x[0], points->y[0],
points->z[0], with_z, NULL, NULL,
NULL, NULL, NULL);
tpx = points->x[0];
tpy = points->y[0];
tpz = points->z[0];
tdist = sqrt(distance);
tspdist = 0;
tlpdist = 0;
segment = 0;
}
else {
distance =
dig_distance2_point_to_line(ux, uy, uz, points->x[0],
points->y[0], points->z[0],
points->x[1], points->y[1],
points->z[1], with_z, NULL, NULL,
NULL, NULL, NULL);
segment = 1;
for (i = 1; i < n_points - 1; i++) {
new_dist = dig_distance2_point_to_line(ux, uy, uz,
points->x[i], points->y[i],
points->z[i],
points->x[i + 1],
points->y[i + 1],
points->z[i + 1], with_z,
NULL, NULL, NULL, NULL,
NULL);
if (new_dist < distance) {
distance = new_dist;
segment = i + 1;
}
}
/* we have segment and now we can recalculate other values (speed) */
new_dist = dig_distance2_point_to_line(ux, uy, uz,
points->x[segment - 1],
points->y[segment - 1],
points->z[segment - 1],
points->x[segment],
points->y[segment],
points->z[segment], with_z,
&tpx, &tpy, &tpz, &tspdist,
NULL);
/* calculate distance from beginning of line */
if (lpdist) {
tlpdist = 0;
for (i = 0; i < segment - 1; i++) {
dx = points->x[i + 1] - points->x[i];
dy = points->y[i + 1] - points->y[i];
if (with_z)
dz = points->z[i + 1] - points->z[i];
else
dz = 0;
tlpdist += hypot(hypot(dx, dy), dz);
}
tlpdist += tspdist;
}
tdist = sqrt(distance);
}
if (px)
*px = tpx;
if (py)
*py = tpy;
if (pz && with_z)
*pz = tpz;
if (dist)
*dist = tdist;
if (spdist)
*spdist = tspdist;
if (lpdist)
*lpdist = tlpdist;
return (segment);
}
