double lgammafn(NMATH_STATE *state, double x)
{
double ans, y, sinpiy;
/* For IEEE double precision DBL_EPSILON = 2^-52 = 2.220446049250313e-16 :
xmax = DBL_MAX / log(DBL_MAX) = 2^1024 / (1024 * log(2)) = 2^1014 / log(2)
dxrel = sqrt(DBL_EPSILON) = 2^-26 = 5^26 * 1e-26 (is *exact* below !)
*/
#define xmax 2.5327372760800758e+305
#define dxrel 1.490116119384765696e-8
// R_signgam = 1;
#ifdef IEEE_754
if(ISNAN(x)) return x;
#endif
if (x < 0 && fmod(floor(-x), 2.) == 0)
// R_signgam = -1;
if (x <= 0 && x == trunc(x)) { /* Negative integer argument */
printf("lgammafn: range error");
return INFINITY;/* +Inf, since lgamma(x) = log|gamma(x)| */
}
y = fabs(x);
if (y <= 10)
return log(fabs(gammafn(state,x)));
/*
ELSE y = |x| > 10 ---------------------- */
if (y > xmax) {
printf("lgammafn: range error");
return INFINITY;
}
if (x > 0) { /* i.e. y = x > 10 */
#ifdef IEEE_754
if(x > 1e17)
return(x*(log(x) - 1.));
else if(x > 4934720.)
return(M_LN_SQRT_2PI + (x - 0.5) * log(x) - x);
else
#endif
return M_LN_SQRT_2PI + (x - 0.5) * log(x) - x + lgammacor(x);
}
/* else: x < -10; y = -x */
sinpiy = fabs(sin(M_PI * y));
if (sinpiy == 0) { /* Negative integer argument ===
Now UNNECESSARY: caught above */
printf(" ** should NEVER happen! *** [lgamma.c: Neg.int, y=%g]\n",y);
return NAN;
}
ans = M_LN_SQRT_PId2 + (x - 0.5) * log(y) - x - log(sinpiy) - lgammacor(y);
if(fabs((x - trunc(x - 0.5)) * ans / x) < dxrel) {
/* The answer is less than half precision because
* the argument is too near a negative integer. */
printf("lgammafn: precision error");
}
return ans;
}
void Dodge::onLeave()
{
State::onLeave();
unsigned int shieldVal = static_cast<unsigned int>(trunc(_entity->_shieldMaxValue/5));
_entity->_shieldValue = _entity->_shieldValue < shieldVal ? 0 : _entity->_shieldValue - shieldVal;
}
float pgeMathTrunc(float x)
{
return trunc(x);
}
int64_t float_to_nsec(double t){
return trunc(t * NANO_SCALE_FLOAT);
}
void output(vsx_module_param_abs* param)
{
VSX_UNUSED(param);
if (text_in->updates)
{
if (process_lines())
text_in->updates = 0;
}
if (text_alpha->get() <= 0)
return;
if (!ftfont)
{
user_message = "module||error loading font "+cur_font;
return;
}
if (text_in->get() == "_")
return;
float obj_size = size->get();
gl_state->matrix_mode (VSX_GL_MODELVIEW_MATRIX );
gl_state->matrix_push();
gl_state->matrix_rotate_f( (float)angle->get()*360, rotation_axis->get(0), rotation_axis->get(1), rotation_axis->get(2) );
if (obj_size < 0)
obj_size = 0;
gl_state->matrix_scale_f( obj_size*0.8*0.01, obj_size*0.01, obj_size*0.01 );
int l_align = align->get();
float l_leading = leading->get();
float ypos = 0;
if (cur_render_type == 0)
glEnable(GL_TEXTURE_2D);
glColor4f(red->get(),green->get(),blue->get(),text_alpha->get());
for (unsigned long i = 0; i < lines.size(); ++i)
{
float ll = limit_line->get();
if (ll != -1.0f)
{
if (trunc(ll) != i) continue;
}
gl_state->matrix_push();
if (l_align == 0)
{
gl_state->matrix_translate_f( 0, ypos, 0 );
} else
if (l_align == 1)
{
gl_state->matrix_translate_f( -lines[i].size_x*0.5f,ypos,0 );
}
if (l_align == 2)
{
gl_state->matrix_translate_f( -lines[i].size_x,ypos,0 );
}
if (cur_render_type == 1)
{
if (outline_alpha->get() > 0.0f && ftfont2) {
float pre_linew;
pre_linew = gl_state->line_width_get();
gl_state->line_width_set( outline_thickness->get() );
glColor4f(outline_color->get(0),outline_color->get(1),outline_color->get(2),outline_alpha->get()*outline_color->get(3));
ftfont2->Render(lines[i].string.c_str());
gl_state->line_width_set( pre_linew );
}
glColor4f(red->get(),green->get(),blue->get(),text_alpha->get());
}
ftfont->Render(lines[i].string.c_str());
gl_state->matrix_pop();
ypos += l_leading;
}
if (cur_render_type == 0)
glDisable(GL_TEXTURE_2D);
gl_state->matrix_pop();
render_result->set(1);
loading_done = true;
}
static int rpn_eval(const token* t)
{
double o1, o2;
switch (t->type) {
case MATHOP_NUMBER:
return push_number(t->value);
case MATHOP_ADD:
return pop_number(&o2) || pop_number(&o1) || push_number(o1 + o2);
case MATHOP_SUB:
return pop_number(&o2) || pop_number(&o1) || push_number(o1 - o2);
case MATHOP_MUL:
return pop_number(&o2) || pop_number(&o1) || push_number(o1 * o2);
case MATHOP_DIV:
return pop_number(&o2) || pop_number(&o1) || push_number(o1 / o2);
case MATHOP_NEG:
return pop_number(&o1) || push_number(-o1);
case MATHOP_DROP:
return pop_number(&o1);
case MATHOP_DUP:
if(pop_number(&o1)) return -1;
return push_number(o1) || push_number(o1);
case MATHOP_SWAP:
return pop_number(&o2) || pop_number(&o1) || push_number(o2) || push_number(o1);
case MATHOP_MOD:
return pop_number(&o2) || pop_number(&o1) || push_number(fmod(o1,o2));
case MATHOP_POW:
return pop_number(&o2) || pop_number(&o1) || push_number(pow(o1,o2));
case MATHOP_EXP:
return pop_number(&o1) || push_number(exp(o1));
case MATHOP_LOG10:
return pop_number(&o1) || push_number(log10(o1));
case MATHOP_LN:
return pop_number(&o1) || push_number(log(o1));
case MATHOP_ABS:
return pop_number(&o1) || push_number(fabs(o1));
case MATHOP_SQRT:
return pop_number(&o1) || push_number(sqrt(o1));
case MATHOP_CBRT:
return pop_number(&o1) || push_number(cbrt(o1));
case MATHOP_FLOOR:
return pop_number(&o1) || push_number(floor(o1));
case MATHOP_CEIL:
return pop_number(&o1) || push_number(ceil(o1));
case MATHOP_ROUND:
return pop_number(&o1) || push_number(round(o1));
case MATHOP_NEARBYINT:
return pop_number(&o1) || push_number(nearbyint(o1));
case MATHOP_TRUNC:
return pop_number(&o1) || push_number(trunc(o1));
case MATHOP_E:
return push_number(M_E);
case MATHOP_PI:
return push_number(M_PI);
default:
LM_WARN("Invalid RPN token type\n");
return -1;
}
}
static uint8_t _parse_gcode_block(char *buf)
{
uint8_t i=0; // persistent index into Gcode block buffer (buf)
char letter; // parsed letter, eg.g. G or X or Y
double value; // value parsed from letter (e.g. 2 for G2)
uint8_t status = TG_OK;
// set initial state for new move
memset(&gp, 0, sizeof(gp)); // clear all parser values
memset(&gf, 0, sizeof(gf)); // clear all next-state flags
memset(&gn, 0, sizeof(gn)); // clear all next-state values
gn.motion_mode = cm_get_motion_mode(); // get motion mode from previous block
// extract commands and parameters
while((status = _get_next_statement(&letter, &value, buf, &i)) == TG_OK) {
switch(letter) {
case 'G':
switch((uint8_t)value) {
case 0: SET_MODAL (MODAL_GROUP_G1, motion_mode, MOTION_MODE_STRAIGHT_TRAVERSE);
case 1: SET_MODAL (MODAL_GROUP_G1, motion_mode, MOTION_MODE_STRAIGHT_FEED);
case 2: SET_MODAL (MODAL_GROUP_G1, motion_mode, MOTION_MODE_CW_ARC);
case 3: SET_MODAL (MODAL_GROUP_G1, motion_mode, MOTION_MODE_CCW_ARC);
case 4: SET_NON_MODAL (next_action, NEXT_ACTION_DWELL);
case 10: SET_MODAL (MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_COORD_DATA);
case 17: SET_MODAL (MODAL_GROUP_G2, select_plane, CANON_PLANE_XY);
case 18: SET_MODAL (MODAL_GROUP_G2, select_plane, CANON_PLANE_XZ);
case 19: SET_MODAL (MODAL_GROUP_G2, select_plane, CANON_PLANE_YZ);
case 20: SET_MODAL (MODAL_GROUP_G6, units_mode, INCHES);
case 21: SET_MODAL (MODAL_GROUP_G6, units_mode, MILLIMETERS);
case 28: {
switch (_point(value)) {
case 0: SET_MODAL (MODAL_GROUP_G0, next_action, NEXT_ACTION_GO_HOME);
case 1: SET_MODAL (MODAL_GROUP_G0, next_action, NEXT_ACTION_SEARCH_HOME);
default: status = TG_UNRECOGNIZED_COMMAND;
}
break;
}
case 40: break; // ignore cancel cutter radius compensation
case 49: break; // ignore cancel tool length offset comp.
case 53: SET_NON_MODAL (absolute_override, true);
case 54: SET_MODAL (MODAL_GROUP_G12, coord_system, G54);
case 55: SET_MODAL (MODAL_GROUP_G12, coord_system, G55);
case 56: SET_MODAL (MODAL_GROUP_G12, coord_system, G56);
case 57: SET_MODAL (MODAL_GROUP_G12, coord_system, G57);
case 58: SET_MODAL (MODAL_GROUP_G12, coord_system, G58);
case 59: SET_MODAL (MODAL_GROUP_G12, coord_system, G59);
case 61: {
switch (_point(value)) {
case 0: SET_MODAL (MODAL_GROUP_G13, path_control, PATH_EXACT_PATH);
case 1: SET_MODAL (MODAL_GROUP_G13, path_control, PATH_EXACT_STOP);
default: status = TG_UNRECOGNIZED_COMMAND;
}
break;
}
case 64: SET_MODAL (MODAL_GROUP_G13,path_control, PATH_CONTINUOUS);
case 80: SET_MODAL (MODAL_GROUP_G1, motion_mode, MOTION_MODE_CANCEL_MOTION_MODE);
case 90: SET_MODAL (MODAL_GROUP_G3, distance_mode, ABSOLUTE_MODE);
case 91: SET_MODAL (MODAL_GROUP_G3, distance_mode, INCREMENTAL_MODE);
case 92: {
switch (_point(value)) {
case 0: SET_MODAL (MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_ORIGIN_OFFSETS);
case 1: SET_NON_MODAL (next_action, NEXT_ACTION_RESET_ORIGIN_OFFSETS);
case 2: SET_NON_MODAL (next_action, NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS);
case 3: SET_NON_MODAL (next_action, NEXT_ACTION_RESUME_ORIGIN_OFFSETS);
default: status = TG_UNRECOGNIZED_COMMAND;
}
break;
}
case 93: SET_MODAL (MODAL_GROUP_G5, inverse_feed_rate_mode, true);
case 94: SET_MODAL (MODAL_GROUP_G5, inverse_feed_rate_mode, false);
default: status = TG_UNRECOGNIZED_COMMAND;
}
break;
case 'M':
switch((uint8_t)value) {
case 0: case 1:
SET_MODAL (MODAL_GROUP_M4, program_flow, PROGRAM_STOP);
case 2: case 30: case 60:
SET_MODAL (MODAL_GROUP_M4, program_flow, PROGRAM_END);
case 3: SET_MODAL (MODAL_GROUP_M7, spindle_mode, SPINDLE_CW);
case 4: SET_MODAL (MODAL_GROUP_M7, spindle_mode, SPINDLE_CCW);
case 5: SET_MODAL (MODAL_GROUP_M7, spindle_mode, SPINDLE_OFF);
case 6: SET_NON_MODAL (change_tool, true);
case 7: SET_MODAL (MODAL_GROUP_M8, mist_coolant, true);
case 8: SET_MODAL (MODAL_GROUP_M8, flood_coolant, true);
case 9: SET_MODAL (MODAL_GROUP_M8, flood_coolant, false);
case 48: SET_MODAL (MODAL_GROUP_M9, feed_override_enable, true);
case 49: SET_MODAL (MODAL_GROUP_M9, feed_override_enable, false);
default: status = TG_UNRECOGNIZED_COMMAND;
}
break;
case 'T': SET_NON_MODAL (tool, (uint8_t)trunc(value));
case 'F': SET_NON_MODAL (feed_rate, value);
case 'P': SET_NON_MODAL (dwell_time, value); // also used as G10 coord system select
case 'S': SET_NON_MODAL (spindle_speed, value);
case 'X': SET_NON_MODAL (target[X], value);
case 'Y': SET_NON_MODAL (target[Y], value);
case 'Z': SET_NON_MODAL (target[Z], value);
case 'A': SET_NON_MODAL (target[A], value);
case 'B': SET_NON_MODAL (target[B], value);
case 'C': SET_NON_MODAL (target[C], value);
// case 'U': SET_NON_MODAL (target[U], value); // reserved
// case 'V': SET_NON_MODAL (target[V], value); // reserved
// case 'W': SET_NON_MODAL (target[W], value); // reserved
case 'I': SET_NON_MODAL (arc_offset[0], value);
case 'J': SET_NON_MODAL (arc_offset[1], value);
case 'K': SET_NON_MODAL (arc_offset[2], value);
case 'R': SET_NON_MODAL (arc_radius, value);
case 'N': SET_NON_MODAL (linenum,(uint32_t)value);// line number
case 'L': break; // not used for anything
default: status = TG_UNRECOGNIZED_COMMAND;
}
if(status != TG_OK) break;
}
if ((status != TG_OK) && (status != TG_COMPLETE)) return (status);
ritorno(_check_gcode_block()); // perform Gcode error checking
return (_execute_gcode_block()); // if successful execute the block
}
// This procedure assumes that poly is monic and that babyStep contains
// at least k+delta powers, where delta = deg(poly) mod k
static long
recursivePolyEval(const ZZX& poly, long k, DynamicPtxtPowers& babyStep,
DynamicPtxtPowers& giantStep, long mod,
long& recursiveDepth)
{
if (deg(poly)<=babyStep.size()) { // Edge condition, use simple eval
long ret = simplePolyEval(poly, babyStep, mod);
recursiveDepth = babyStep.getDepth(deg(poly));
return ret;
}
if (verbose) cerr << "recursivePolyEval("<<poly<<")\n";
long delta = deg(poly) % k; // deg(poly) mod k
long n = divc(deg(poly),k); // ceil( deg(poly)/k )
long t = 1L<<(NextPowerOfTwo(n)); // t >= n, so t*k >= deg(poly)
// Special case for deg(poly) = k * 2^e +delta
if (n==t)
return degPowerOfTwo(poly, k, babyStep, giantStep, mod, recursiveDepth);
// When deg(poly) = k*(2^e -1) we use the Paterson-Stockmeyer recursion
if (n == t-1 && delta==0)
return PatersonStockmeyer(poly, k, t/2, delta,
babyStep, giantStep, mod, recursiveDepth);
t = t/2;
// In any other case we have kt < deg(poly) < k(2t-1). We then set
// u = deg(poly) - k*(t-1) and poly = q*X^u + r with deg(r)<u
// and recurse on poly = (q-1)*X^u + (X^u+r)
long u = deg(poly) - k*(t-1);
ZZX r = trunc(poly, u); // degree <= u-1
ZZX q = RightShift(poly, u); // degree == k*(t-1)
q -= 1;
SetCoeff(r, u); // degree == u
long ret, tmp;
long subDepth1=0, subDepth2=0;
if (verbose)
cerr << " {deg(poly)="<<deg(poly)<<"<k*(2t-1)="<<k*(2*t-1)
<< "} recursing on "<<r<<" + X^"<<u<<"*"<<q<<endl;
ret = PatersonStockmeyer(q, k, t/2, 0,
babyStep, giantStep, mod, subDepth1);
if (verbose) {
cerr << " PatersonStockmeyer("<<q<<") returns "<<ret<<", depth="<<subDepth1<<endl;
if (ret != polyEvalMod(q,babyStep[0], mod)) {
cerr << " @@1st recursive call failed, q="<<q
<< ", ret="<<ret<<"!=" << polyEvalMod(q,babyStep[0], mod)<<endl;
exit(0);
}
}
tmp = giantStep.getPower(u/k);
subDepth2 = giantStep.getDepth(u/k);
if (delta!=0) { // if u is not divisible by k then compute it
if (verbose)
cerr <<" multiplying by X^"<<u
<<"=giantStep.getPower("<<(u/k)<<")*babyStep.getPower("<<delta<<")="
<< giantStep.getPower(u/k)<<"*"<<babyStep.getPower(delta)
<< "="<<tmp<<endl;
tmp = MulMod(tmp, babyStep.getPower(delta), mod);
nMults++;
subDepth2++;
}
ret = MulMod(ret, tmp, mod);
nMults ++;
subDepth1 = max(subDepth1, subDepth2)+1;
if (verbose) cerr << " after mult by X^{k*"<<u<<"+"<<delta<<"}, depth="<< subDepth1<<endl;
tmp = recursivePolyEval(r, k, babyStep, giantStep, mod, subDepth2);
if (verbose)
cerr << " recursivePolyEval("<<r<<") returns "<<tmp<<", depth="<<subDepth2<<endl;
if (tmp != polyEvalMod(r,babyStep[0], mod)) {
cerr << " @@2nd recursive call failed, r="<<r
<< ", ret="<<tmp<<"!=" << polyEvalMod(r,babyStep[0], mod)<<endl;
exit(0);
}
recursiveDepth = max(subDepth1, subDepth2);
return AddMod(ret, tmp, mod);
}
static CYTHON_INLINE struct ZZX* ZZX_truncate(struct ZZX* x, long m)
{
ZZX* t = new ZZX();
trunc(*t, *x, m);
return t;
}
int main(){
int chave = 0;
FILE *arq, *arqo, *arqw;
arq = fopen("entrada-transposicao.txt", "r");
arqo = fopen("texto-original.txt", "r");
arqw = fopen("saida-transposicao.txt", "w");
if(arq == NULL) printf("Não abriu");
char dados[10000], dados1[10000];
fgets(dados, 10000, arq);
fgets(dados1, 10000, arqo);
int i=0, t, j, ic;
for(i=0; i<strlen(dados1); i++){
if(dados1[i]==10) dados1[i] = '\0';
}
if(dados == NULL) printf("Não leu fgets");
for(t=1; t<256; t++){
char *chavx = (char *)malloc(sizeof(char)*strlen(dados));
int tamanho_codificado = strlen(dados);
int ldecod = trunc(tamanho_codificado / t); //quantidade de linhas
int cdecod = trunc(tamanho_codificado / ldecod);
int mdecod[cdecod][ldecod];
ic = 0;
int cc=0, cl=0;
int max_linha = strlen(dados) / t; //t vai ser a chave agora
int inc = 0;
for(i=0; i<cdecod; i++){
for(j=0; j<ldecod; j++){
if(dados[inc]!=10){
mdecod[i][j] = dados[inc++];
}
}
}
inc = 0;
for(i=0; i<ldecod; i++){
for(j=0; j<cdecod; j++){
chavx[inc++] = mdecod[j][i];
}
}
int ehc = 0;
for(i=strlen(chavx)-1; i>0; i--){
if(chavx[i]!=' ') break;
if(chavx[i]==' ') chavx[i] = '\0';
}
if(strcmp(chavx, dados1)==0){
printf("---------------\nChave descoberta: %d\n--------------------\n", t);
break;
}else{
//printf("chavx =>> [%s]==[%s]\n", chavx, dados1);
}
}
/*int i = 0, j = 0, tamanho_linhas = 0, icol = 0, ilinha = 0;
tamanho_linhas = trunc(strlen(dados) / chave) + 1;
int matrix[tamanho_linhas][chave];
for(i=0; i<tamanho_linhas; i++){
for(j=0; j<chave; j++){
matrix[i][j] = ' '; //"Limpar" a matriz
}
}
for(i=0; i<strlen(dados); i++){
int k = (int)dados[i];
if(k!=10) matrix[ilinha][icol] = k; //ignorar a quebra de linha
//int gerado = codificar(k, chave);
//int decodificado = decodificar(gerado, chave);
//printf(">%c - %c\n", gerado, decodificado);
icol++;
if(icol >= chave){
icol = 0;
ilinha++;
}
}
printf("--------------%dx%d----------------\n", tamanho_linhas, chave);
for(i=0; i<tamanho_linhas; i++){
for(j=0; j<chave; j++){
printf("%c", matrix[i][j]);
}
printf("\n");
}
printf("------------------------------\n");
printf("Texto codificado: \n");
char codificado[tamanho_linhas*chave];
int ic = 0;
for(i=0; i<chave; i++){
for(j=0; j<tamanho_linhas; j++){
printf("%c", matrix[j][i]);
codificado[ic++] = matrix[j][i];
}
}
printf("\n");
printf("=================================\nTexto decodificado:\n");
int tamanho_codificado = strlen(codificado);
int ldecod = tamanho_codificado / chave; //quantidade de linhas
int cdecod = tamanho_codificado / ldecod;
int mdecod[cdecod][ldecod];
ic = 0;
int cc=0, cl=0;
for(i=0; i<strlen(codificado); i++){
mdecod[cc][cl] = codificado[i];
cc++;
if(cc>=chave){
cc=0;
cl++;
}
}
for(i=0; i<ldecod; i++){
for(j=0; j<cdecod; j++){
printf("%c", matrix[i][j]);
}
}
printf("\n========================================\n"); */
return 0;
}
// This procedure assumes that poly is monic, deg(poly)=k*(2t-1)+delta
// with t=2^e, and that babyStep contains k+delta powers
static long
PatersonStockmeyer(const ZZX& poly, long k, long t, long delta,
DynamicPtxtPowers& babyStep, DynamicPtxtPowers& giantStep,
long mod, long& recursiveDepth)
{
if (verbose) cerr << "PatersonStockmeyer("<<poly<<"), k="<<k<<", t="<<t<<endl;
if (deg(poly)<=babyStep.size()) { // Edge condition, use simple eval
long ret = simplePolyEval(poly, babyStep, mod);
recursiveDepth = babyStep.getDepth(deg(poly));
return ret;
}
long subDepth1=0, subDepth2=0;
long ret, tmp;
ZZX r = trunc(poly, k*t); // degree <= k*2^e-1
ZZX q = RightShift(poly, k*t); // degree == k(2^e-1) +delta
if (verbose) cerr << " r ="<<r<< ", q="<<q;
const ZZ& coef = coeff(r,deg(q));
SetCoeff(r, deg(q), coef-1); // r' = r - X^{deg(q)}
if (verbose) cerr << ", r'="<<r;
ZZX c,s;
DivRem(c,s,r,q); // r' = c*q + s
// deg(s)<deg(q), and if c!= 0 then deg(c)<k-delta
if (verbose) cerr << ", c="<<c<< ", s ="<<s<<endl;
assert(deg(s)<deg(q));
assert(IsZero(c) || deg(c)<k-delta);
SetCoeff(s,deg(q)); // s' = s + X^{deg(q)}, deg(s)==deg(q)
// reduce the coefficients modulo mod
for (long i=0; i<=deg(c); i++) rem(c[i],c[i], to_ZZ(mod));
c.normalize();
for (long i=0; i<=deg(s); i++) rem(s[i],s[i], to_ZZ(mod));
s.normalize();
if (verbose) cerr << " {t==n+1} recursing on "<<s<<" + (X^"<<t*k<<"+"<<c<<")*"<<q<<endl;
// Evaluate recursively poly = (c+X^{kt})*q + s'
tmp = simplePolyEval(c, babyStep, mod);
tmp = AddMod(tmp, giantStep.getPower(t), mod);
subDepth1 = max(babyStep.getDepth(deg(c)), giantStep.getDepth(t));
ret = PatersonStockmeyer(q, k, t/2, delta,
babyStep, giantStep, mod, subDepth2);
if (verbose) {
cerr << " PatersonStockmeyer("<<q<<") returns "<<ret
<< ", depth="<<subDepth2<<endl;
if (ret != polyEvalMod(q,babyStep[0], mod)) {
cerr << " **1st recursive call failed, q="<<q<<endl;
exit(0);
}
}
ret = MulMod(ret, tmp, mod);
nMults++;
subDepth1 = max(subDepth1, subDepth2)+1;
tmp = PatersonStockmeyer(s, k, t/2, delta,
babyStep, giantStep, mod, subDepth2);
if (verbose) {
cerr << " PatersonStockmeyer("<<s<<") returns "<<tmp
<< ", depth="<<subDepth2<<endl;
if (tmp != polyEvalMod(s,babyStep[0], mod)) {
cerr << " **2nd recursive call failed, s="<<s<<endl;
exit(0);
}
}
ret = AddMod(ret,tmp,mod);
recursiveDepth = max(subDepth1, subDepth2);
return ret;
}
int main() {
/* Host/device data structures */
cl_device_id device;
cl_context context;
cl_command_queue queue;
cl_program program;
cl_kernel kernel_init, kernel_stage_0, kernel_stage_n, kernel_merge,
kernel_merge_last;
cl_int i, err, check, direction;
/* Data and buffers */
float data[NUM_FLOATS];
cl_mem data_buffer;
cl_uint stage, high_stage, num_stages;
size_t local_size, global_size;
/* Initialize data */
srand(time(NULL));
for(i=0; i<NUM_FLOATS; i++) {
data[i] = rand();
}
/* Create a device and context */
device = create_device();
context = clCreateContext(NULL, 1, &device, NULL, NULL, &err);
if(err < 0) {
perror("Couldn't create a context");
exit(1);
}
/* Build the program */
program = build_program(context, device, PROGRAM_FILE);
/* Create kernels */
kernel_init = clCreateKernel(program, BSORT_INIT, &err);
if(err < 0) {
perror("Couldn't create the initial kernel");
exit(1);
};
kernel_stage_0 = clCreateKernel(program, BSORT_STAGE_0, &err);
if(err < 0) {
perror("Couldn't create the stage_0 kernel");
exit(1);
};
kernel_stage_n = clCreateKernel(program, BSORT_STAGE_N, &err);
if(err < 0) {
perror("Couldn't create the stage_n kernel");
exit(1);
};
kernel_merge = clCreateKernel(program, BSORT_MERGE, &err);
if(err < 0) {
perror("Couldn't create the merge kernel");
exit(1);
};
kernel_merge_last = clCreateKernel(program, BSORT_MERGE_LAST, &err);
if(err < 0) {
perror("Couldn't create the merge_last kernel");
exit(1);
};
/* Determine maximum work-group size */
err = clGetKernelWorkGroupInfo(kernel_init, device, CL_KERNEL_WORK_GROUP_SIZE,
sizeof(local_size), &local_size, NULL);
if(err < 0) {
perror("Couldn't find the maximum work-group size");
exit(1);
};
local_size = (int)pow(2, trunc(log2(local_size)));
/* Create buffer */
data_buffer = clCreateBuffer(context, CL_MEM_READ_WRITE |
CL_MEM_COPY_HOST_PTR, sizeof(data), data, &err);
if(err < 0) {
perror("Couldn't create a buffer");
exit(1);
};
/* Create kernel argument */
err = clSetKernelArg(kernel_init, 0, sizeof(cl_mem), &data_buffer);
err |= clSetKernelArg(kernel_stage_0, 0, sizeof(cl_mem), &data_buffer);
err |= clSetKernelArg(kernel_stage_n, 0, sizeof(cl_mem), &data_buffer);
err |= clSetKernelArg(kernel_merge, 0, sizeof(cl_mem), &data_buffer);
err |= clSetKernelArg(kernel_merge_last, 0, sizeof(cl_mem), &data_buffer);
if(err < 0) {
printf("Couldn't set a kernel argument");
exit(1);
};
/* Create kernel argument */
err = clSetKernelArg(kernel_init, 1, 8*local_size*sizeof(float), NULL);
err |= clSetKernelArg(kernel_stage_0, 1, 8*local_size*sizeof(float), NULL);
err |= clSetKernelArg(kernel_stage_n, 1, 8*local_size*sizeof(float), NULL);
err |= clSetKernelArg(kernel_merge, 1, 8*local_size*sizeof(float), NULL);
err |= clSetKernelArg(kernel_merge_last, 1, 8*local_size*sizeof(float), NULL);
if(err < 0) {
printf("Couldn't set a kernel argument");
exit(1);
};
/* Create a command queue */
queue = clCreateCommandQueue(context, device, 0, &err);
if(err < 0) {
perror("Couldn't create a command queue");
exit(1);
};
/* Enqueue initial sorting kernel */
global_size = NUM_FLOATS/8;
if(global_size < local_size) {
local_size = global_size;
}
err = clEnqueueNDRangeKernel(queue, kernel_init, 1, NULL, &global_size,
&local_size, 0, NULL, NULL);
if(err < 0) {
perror("Couldn't enqueue the kernel");
exit(1);
}
/* Execute further stages */
num_stages = global_size/local_size;
for(high_stage = 2; high_stage < num_stages; high_stage <<= 1) {
err = clSetKernelArg(kernel_stage_0, 2, sizeof(int), &high_stage);
err |= clSetKernelArg(kernel_stage_n, 3, sizeof(int), &high_stage);
if(err < 0) {
printf("Couldn't set a kernel argument");
exit(1);
};
for(stage = high_stage; stage > 1; stage >>= 1) {
err = clSetKernelArg(kernel_stage_n, 2, sizeof(int), &stage);
if(err < 0) {
printf("Couldn't set a kernel argument");
exit(1);
};
err = clEnqueueNDRangeKernel(queue, kernel_stage_n, 1, NULL,
&global_size, &local_size, 0, NULL, NULL);
if(err < 0) {
perror("Couldn't enqueue the kernel");
exit(1);
}
}
err = clEnqueueNDRangeKernel(queue, kernel_stage_0, 1, NULL,
&global_size, &local_size, 0, NULL, NULL);
if(err < 0) {
perror("Couldn't enqueue the kernel");
exit(1);
}
}
/* Set the sort direction */
direction = DIRECTION;
err = clSetKernelArg(kernel_merge, 3, sizeof(int), &direction);
err |= clSetKernelArg(kernel_merge_last, 2, sizeof(int), &direction);
if(err < 0) {
printf("Couldn't set a kernel argument");
exit(1);
};
/* Perform the bitonic merge */
for(stage = num_stages; stage > 1; stage >>= 1) {
err = clSetKernelArg(kernel_merge, 2, sizeof(int), &stage);
if(err < 0) {
printf("Couldn't set a kernel argument");
exit(1);
};
err = clEnqueueNDRangeKernel(queue, kernel_merge, 1, NULL,
&global_size, &local_size, 0, NULL, NULL);
if(err < 0) {
perror("Couldn't enqueue the kernel");
exit(1);
}
}
err = clEnqueueNDRangeKernel(queue, kernel_merge_last, 1, NULL,
&global_size, &local_size, 0, NULL, NULL);
if(err < 0) {
perror("Couldn't enqueue the kernel");
exit(1);
}
/* Read the result */
err = clEnqueueReadBuffer(queue, data_buffer, CL_TRUE, 0,
sizeof(data), &data, 0, NULL, NULL);
if(err < 0) {
perror("Couldn't read the buffer");
exit(1);
}
check = 1;
/* Check ascending sort */
if(direction == 0) {
for(i=1; i<NUM_FLOATS; i++) {
if(data[i] < data[i-1]) {
check = 0;
break;
}
}
}
/* Check descending sort */
if(direction == -1) {
for(i=1; i<NUM_FLOATS; i++) {
if(data[i] > data[i-1]) {
check = 0;
break;
}
}
}
/* Display check result */
printf("Local size: %zu\n", local_size);
printf("Global size: %zu\n", global_size);
if(check)
printf("Bitonic sort succeeded.\n");
else
printf("Bitonic sort failed.\n");
/* Deallocate resources */
clReleaseMemObject(data_buffer);
clReleaseKernel(kernel_init);
clReleaseKernel(kernel_stage_0);
clReleaseKernel(kernel_stage_n);
clReleaseKernel(kernel_merge);
clReleaseKernel(kernel_merge_last);
clReleaseCommandQueue(queue);
clReleaseProgram(program);
clReleaseContext(context);
return 0;
}
void SendAttitude(void)
{
static byte i,j;
//Serial.print("!");
#if PRINT_EULER == 1
float AccMagnitude = sqrt(Acc_V[0]*Acc_V[0] + Acc_V[1]*Acc_V[1] + Acc_V[2]*Acc_V[2]);
AccMagnitude = (AccMagnitude / (float)GRAVITY); // Scale to gravity.
if(abs(last_roll-(Roll* 57.2957795131)) > 1 || abs(last_pitch-(Pitch* 57.2957795131)) > 1 || abs(last_yaw-(Yaw* 57.2957795131)) > 1 )
{
last_roll = Roll* 57.2957795131;
last_pitch = Pitch* 57.2957795131;
last_yaw = Yaw* 57.2957795131;
Serial.print(Roll * 57.2957795131);
Serial.print(",");
Serial.print(Pitch * 57.2957795131);
Serial.print(",");
Serial.print(Yaw * 57.2957795131);
Serial.print(",");
Serial.println(AccMagnitude);
int roll_d1 =(Roll * 57.2957795131); // Get the integer part (678).
float roll_f2 = (Roll * 57.2957795131) - roll_d1; // Get fractional part (678.0123 - 678 = 0.0123).
int roll_d2 = trunc(roll_f2 * 100);
int pitch_d1 =(Pitch * 57.2957795131); // Get the integer part (678).
float pitch_f2 = (Pitch * 57.2957795131) - pitch_d1; // Get fractional part (678.0123 - 678 = 0.0123).
int pitch_d2 = trunc(pitch_f2 * 100);
int yaw_d1 =(Yaw * 57.2957795131); // Get the integer part (678).
float yaw_f2 = (Yaw * 57.2957795131) - yaw_d1; // Get fractional part (678.0123 - 678 = 0.0123).
int yaw_d2 = trunc(yaw_f2 * 100);
int acc_d1 =AccMagnitude; // Get the integer part (678).
float acc_f2 = AccMagnitude - acc_d1; // Get fractional part (678.0123 - 678 = 0.0123).
int acc_d2 = trunc(acc_f2 * 100);
// enable Slave Select
digitalWrite(SS, LOW); // SS is pin 10
SPI.transfer (roll_d1);
SPI.transfer (roll_d2);
SPI.transfer (pitch_d1);
SPI.transfer (pitch_d2);
SPI.transfer (yaw_d1);
SPI.transfer (yaw_d2);
SPI.transfer (acc_d1);
SPI.transfer (acc_d2);
SPI.transfer ('\n');
// disable Slave Select
digitalWrite(SS, HIGH);
}
#endif // PRINT_EULER
#if PRINT_ANALOGS == 1
Serial.print("AN:");
for ( i = 0; i < 3 ; i++ )
{
Serial.print(GyroADC[Map[i]]);
Serial.print(",");
}
for ( i = 0; i < 3 ; i++ )
{
Serial.print(AccADC[i]);
Serial.print (",");
}
for ( i = 0; i < 3 ; i++ )
{
Serial.print(Mag[i]);
Serial.print (",");
}
#endif // PRINT_ANALOGS
#if PRINT_DCM == 1
Serial.print ("DCM:");
for ( i = 0; i < 3 ; i++ )
for ( j = 0; j < 3 ; j++ )
{
Serial.print(DCM_Matrix[i][j]*10000000);
Serial.print (",");
}
#endif // PRINT_DCM
} // SendAttitude
/*
* Save all of the undetermined messages at the top of "mbox"
* Save all untouched messages back in the system mailbox.
* Remove the system mailbox, if none saved there.
*/
void
quit()
{
int mcount, p, modify, autohold, anystat, holdbit, nohold;
FILE *ibuf, *obuf, *fbuf, *rbuf, *readstat, *abuf;
register struct message *mp;
register int c;
extern char tempQuit[], tempResid[];
struct stat minfo;
char *mbox;
/*
* If we are read only, we can't do anything,
* so just return quickly.
*/
if (readonly)
return;
/*
* If editing (not reading system mail box), then do the work
* in edstop()
*/
if (edit) {
edstop();
return;
}
/*
* See if there any messages to save in mbox. If no, we
* can save copying mbox to /tmp and back.
*
* Check also to see if any files need to be preserved.
* Delete all untouched messages to keep them out of mbox.
* If all the messages are to be preserved, just exit with
* a message.
*/
fbuf = Fopen(mailname, "r");
if (fbuf == NULL)
goto newmail;
flock(fileno(fbuf), LOCK_EX);
rbuf = NULL;
if (fstat(fileno(fbuf), &minfo) >= 0 && minfo.st_size > mailsize) {
printf("New mail has arrived.\n");
rbuf = Fopen(tempResid, "w");
if (rbuf == NULL || fbuf == NULL)
goto newmail;
#ifdef APPEND
fseek(fbuf, (long)mailsize, 0);
while ((c = getc(fbuf)) != EOF)
(void) putc(c, rbuf);
#else
p = minfo.st_size - mailsize;
while (p-- > 0) {
c = getc(fbuf);
if (c == EOF)
goto newmail;
(void) putc(c, rbuf);
}
#endif
Fclose(rbuf);
if ((rbuf = Fopen(tempResid, "r")) == NULL)
goto newmail;
rm(tempResid);
}
/*
* Adjust the message flags in each message.
*/
anystat = 0;
autohold = value("hold") != NOSTR;
holdbit = autohold ? MPRESERVE : MBOX;
nohold = MBOX|MSAVED|MDELETED|MPRESERVE;
if (value("keepsave") != NOSTR)
nohold &= ~MSAVED;
for (mp = &message[0]; mp < &message[msgCount]; mp++) {
if (mp->m_flag & MNEW) {
mp->m_flag &= ~MNEW;
mp->m_flag |= MSTATUS;
}
if (mp->m_flag & MSTATUS)
anystat++;
if ((mp->m_flag & MTOUCH) == 0)
mp->m_flag |= MPRESERVE;
if ((mp->m_flag & nohold) == 0)
mp->m_flag |= holdbit;
}
modify = 0;
if (Tflag != NOSTR) {
if ((readstat = Fopen(Tflag, "w")) == NULL)
Tflag = NOSTR;
}
for (c = 0, p = 0, mp = &message[0]; mp < &message[msgCount]; mp++) {
if (mp->m_flag & MBOX)
c++;
if (mp->m_flag & MPRESERVE)
p++;
if (mp->m_flag & MODIFY)
modify++;
if (Tflag != NOSTR && (mp->m_flag & (MREAD|MDELETED)) != 0) {
char *id;
if ((id = hfield("article-id", mp)) != NOSTR)
fprintf(readstat, "%s\n", id);
}
}
if (Tflag != NOSTR)
Fclose(readstat);
if (p == msgCount && !modify && !anystat) {
printf("Held %d message%s in %s\n",
p, p == 1 ? "" : "s", mailname);
Fclose(fbuf);
return;
}
if (c == 0) {
if (p != 0) {
writeback(rbuf);
Fclose(fbuf);
return;
}
goto cream;
}
/*
* Create another temporary file and copy user's mbox file
* darin. If there is no mbox, copy nothing.
* If he has specified "append" don't copy his mailbox,
* just copy saveable entries at the end.
*/
mbox = expand("&");
mcount = c;
if (value("append") == NOSTR) {
if ((obuf = Fopen(tempQuit, "w")) == NULL) {
perror(tempQuit);
Fclose(fbuf);
return;
}
if ((ibuf = Fopen(tempQuit, "r")) == NULL) {
perror(tempQuit);
rm(tempQuit);
Fclose(obuf);
Fclose(fbuf);
return;
}
rm(tempQuit);
if ((abuf = Fopen(mbox, "r")) != NULL) {
while ((c = getc(abuf)) != EOF)
(void) putc(c, obuf);
Fclose(abuf);
}
if (ferror(obuf)) {
perror(tempQuit);
Fclose(ibuf);
Fclose(obuf);
Fclose(fbuf);
return;
}
Fclose(obuf);
close(creat(mbox, 0600));
if ((obuf = Fopen(mbox, "r+")) == NULL) {
perror(mbox);
Fclose(ibuf);
Fclose(fbuf);
return;
}
}
if (value("append") != NOSTR) {
if ((obuf = Fopen(mbox, "a")) == NULL) {
perror(mbox);
Fclose(fbuf);
return;
}
fchmod(fileno(obuf), 0600);
}
for (mp = &message[0]; mp < &message[msgCount]; mp++)
if (mp->m_flag & MBOX)
if (send(mp, obuf, saveignore, NOSTR) < 0) {
perror(mbox);
Fclose(ibuf);
Fclose(obuf);
Fclose(fbuf);
return;
}
/*
* Copy the user's old mbox contents back
* to the end of the stuff we just saved.
* If we are appending, this is unnecessary.
*/
if (value("append") == NOSTR) {
rewind(ibuf);
c = getc(ibuf);
while (c != EOF) {
(void) putc(c, obuf);
if (ferror(obuf))
break;
c = getc(ibuf);
}
Fclose(ibuf);
}
fflush(obuf);
trunc(obuf);
if (ferror(obuf)) {
perror(mbox);
Fclose(obuf);
Fclose(fbuf);
return;
}
Fclose(obuf);
if (mcount == 1)
printf("Saved 1 message in mbox\n");
else
printf("Saved %d messages in mbox\n", mcount);
/*
* Now we are ready to copy back preserved files to
* the system mailbox, if any were requested.
*/
if (p != 0) {
writeback(rbuf);
Fclose(fbuf);
return;
}
/*
* Finally, remove his /usr/mail file.
* If new mail has arrived, copy it back.
*/
cream:
if (rbuf != NULL) {
abuf = Fopen(mailname, "r+");
if (abuf == NULL)
goto newmail;
while ((c = getc(rbuf)) != EOF)
(void) putc(c, abuf);
Fclose(rbuf);
trunc(abuf);
Fclose(abuf);
alter(mailname);
Fclose(fbuf);
return;
}
demail();
Fclose(fbuf);
return;
newmail:
printf("Thou hast new mail.\n");
if (fbuf != NULL)
Fclose(fbuf);
}
//Latest version
int getGridValuesFromExtent(QString extent,QSqlDatabase db, double &dbCellSize, double &xllCenter, double &yllCenter, int &ncols, int &nrows, int &bottom, int &left)
{
//(1.3333,32.1212321) (-4.12121,41.212121)
if (!extent.count(" ") == 1)
{
gbtLog(QObject::tr("Extent is invalid"));
return 1;
}
if (!extent.count(",") == 2)
{
gbtLog(QObject::tr("Extent is invalid"));
return 1;
}
if (!extent.count("(") == 2)
{
gbtLog(QObject::tr("Extent is invalid"));
return 1;
}
if (!extent.count(")") == 2)
{
gbtLog(QObject::tr("Extent is invalid"));
return 1;
}
int pos;
pos = extent.indexOf(" ");
QString from;
QString to;
from = extent.left(pos);
to = extent.mid(pos+1,extent.length()-pos+1);
from.replace("(","");
from.replace(")","");
to.replace("(","");
to.replace(")","");
//Get UpperLeft
QPointF dupperLeft;
pos = from.indexOf(",");
dupperLeft.setX(from.left(pos).toDouble());
dupperLeft.setY(from.mid(pos+1,from.length()-pos+1).toDouble());
//Get lower right
QPointF dlowerRight;
pos = to.indexOf(",");
dlowerRight.setX(to.left(pos).toDouble());
dlowerRight.setY(to.mid(pos+1,to.length()-pos+1).toDouble());
QSqlQuery qry(db);
QString sql;
dbCellSize = 0.0;
sql = "SELECT cellSize FROM gbtconfig";
if (qry.exec(sql))
{
if (qry.first())
dbCellSize = qry.value(0).toDouble();
else
return 1;
}
else
return 1;
if (dbCellSize == 0.0)
return 1;
//*************************
double v_xMin;
double v_xMax;
double v_yMin;
double v_yMax;
double halfx;
int v_cols;
int v_rows;
v_xMin = trunc(( dupperLeft.x() *1.0/ dbCellSize )) * dbCellSize;
v_yMin = trunc(( dlowerRight.y() *1.0/ dbCellSize )) * dbCellSize;
v_cols = trunc( ( dlowerRight.x() - dupperLeft.x() ) *1.0/ dbCellSize ) + 1;
v_rows = trunc( ( dupperLeft.y() - dlowerRight.y() ) *1.0/ dbCellSize ) + 1;
v_xMax = v_xMin + v_cols * dbCellSize;
v_yMax = v_yMin + v_rows * dbCellSize;
halfx = dbCellSize *1.0/ 2;
xllCenter = v_xMin + halfx;
yllCenter = (v_yMin + halfx) - 0.0200;
nrows = trunc( fabs( v_yMax - v_yMin ) *1.0/ dbCellSize );
ncols = trunc(fabs(v_xMax - v_xMin) / dbCellSize);
//*******************
left = ceil((dupperLeft.x() + 180) / dbCellSize);
bottom = ceil((90 - dupperLeft.y()) / dbCellSize);
return 0;
}
double JSValue::toIntegerPreserveNaN(ExecState* exec) const
{
if (isInt32())
return asInt32();
return trunc(toNumber(exec));
}
//Latest and most updated version
int calcBoundFromShapes(QString shapeConstraint,QSqlDatabase db, double &dbCellSize, double &xllCenter, double &yllCenter, int &ncols, int &nrows, int &bottom, int &left)
{
//ShapeDataSet:shapeID,ShapeID,
if (shapeConstraint.count(":") != 1)
{
gbtLog(QObject::tr("Error in shape constraint"));
return 1;
}
int pos;
pos = shapeConstraint.indexOf(":");
QString dataset = shapeConstraint.left(pos);
QSqlQuery qry(db);
QString sql;
dbCellSize = 0.0;
sql = "SELECT cellSize FROM gbtconfig";
if (qry.exec(sql))
{
if (qry.first())
dbCellSize = qry.value(0).toDouble();
else
return 1;
}
else
return 1;
sql = "SELECT count(*) FROM datasetinfo WHERE dataset_id = '" + dataset + "' and dataset_type = 2";
if (qry.exec(sql))
{
if (qry.first())
{
if (qry.value(0).toInt() == 0)
{
gbtLog(QObject::tr(" is not a shape dataset"));
return 1;
}
}
else
return 1;
}
else
return 1;
sql = "SELECT shapeid,AsText(max(envelope(ogc_geom))) as boundbox FROM " + dataset + " WHERE ";
QString shapes = shapeConstraint.mid(pos+1,shapeConstraint.length()-pos+1);
QString shape;
pos =0;
while (pos <= shapes.length()-1)
{
if (shapes[pos] != ',')
{
shape = shape + shapes[pos];
pos++;
}
else
{
sql = sql + " shapeid = " + shape + " OR ";
shape = "";
shapes = shapes.mid(pos+1,shapes.length()-pos+1);
pos = 0;
}
}
sql = sql + "shapeid = " + shape;
sql = sql + " GROUP BY shapeid";
double maxX;
double maxY;
double minX;
double minY;
maxX = -3000;
maxY = -3000;
minX = 3000;
minY = 3000;
QString cadena;
QString geopos;
double temp;
if (qry.exec(sql))
{
while (qry.next())
{
cadena = qry.value(1).toString();
pos = cadena.indexOf("(");
cadena = cadena.mid(pos+1,cadena.length()-pos+1); //Remove the type of shape
cadena = cadena.replace("(","");
cadena = cadena.replace(")","");
pos = 0;
while (pos <= cadena.length()-1)
{
if (cadena[pos] != ',')
{
geopos = geopos + cadena[pos];
pos++;
}
else
{
cadena = cadena.mid(pos+1,cadena.length()-pos+1);
pos = geopos.indexOf(" ");
temp = geopos.left(pos).toDouble();
if (temp > maxX)
maxX = temp;
if (temp < minX)
minX = temp;
temp = geopos.mid(pos+1,geopos.length()-pos+1).toDouble();
if (temp > maxY)
maxY = temp;
if (temp < minY)
minY = temp;
pos = 0;
geopos = "";
}
}
//Final string
pos = geopos.indexOf(" ");
temp = geopos.left(pos).toDouble();
if (temp > maxX)
maxX = temp;
if (temp < minX)
minX = temp;
temp = geopos.mid(pos+1,geopos.length()-pos+1).toDouble();
if (temp > maxY)
maxY = temp;
if (temp < minY)
minY = temp;
}
QPointF UL;
QPointF LR;
UL.setX(minX);
UL.setY(maxY);
LR.setX(maxX);
LR.setY(minY);
double v_xMin,v_xMax,v_yMin,v_yMax;
double halfx;
int v_cols,v_rows;
v_xMin = trunc(UL.x() / dbCellSize) * dbCellSize;
v_yMin = trunc(LR.y() / dbCellSize) * dbCellSize;
v_cols = trunc((LR.x() - UL.x()) / dbCellSize) + 1;
v_rows = trunc((UL.y() - LR.y()) / dbCellSize) + 1;
v_xMax = v_xMin + v_cols * dbCellSize;
v_yMax = v_yMin + v_rows * dbCellSize;
halfx = dbCellSize / 2;
xllCenter = v_xMin + halfx;
yllCenter = (v_yMin + halfx) - 0.0200;
ncols = trunc(fabs(v_xMax - v_xMin) / dbCellSize);
nrows = trunc(fabs(v_yMax - v_yMin) / dbCellSize);
left = ceil((UL.x() + 180) / dbCellSize);
bottom = ceil((90 - UL.y()) / dbCellSize);
return 0;
}
else
return 1;
}
float truncf (float x)
{
return (float) trunc( (double)x );
}
void CCareer::OnTextChangedName(LPTSTR text)
{
trunc(text,PLAYERNAMELEN - 1);
}
void Triangle::draw(Canvas& canvas, Texture* texture = 0) {
std::vector<TexturedPoint> points;
transform(points);
std::sort(points.begin(), points.end());
std::vector<Edge> edges;
int minY = (points.front().y() < 0) ? 0: points.front().y();
int maxY = (points.back().y() < this->maxY) ? points.back().y() : this->maxY - 1;
int curY = minY;
int i = 0;
while (curY < maxY) {
int nextY = maxY;
while ( i != (int)points.size() && trunc( points[i].y()) <= curY ){
TexturedPoint a = points[i];
TexturedPoint b = points[(points.size() - i - 1 ) % points.size()];
TexturedPoint c = points[(i + 1) % points.size()];
if (b.y() > curY ) {
edges.push_back(Edge(a,b));
if ( b.y() < nextY ) {
nextY = b.y() ;
}
}
if (c.y() > curY) {
edges.push_back(Edge(a,c));
if ( c.y() < nextY) {
nextY = c.y();
}
}
++i;
}
while(curY <= nextY && curY <= maxY) {
std::vector<TexturedPoint> borderX;
for (int i = 0; i < (int)edges.size(); ++i) {
int n = curY - edges[i].getA().y();
double curX = (edges[i].getA().x()) + n * edges[i].getK();
TexturedPoint texCoord(curX, curY);
texCoord.calcTextureCoordinates(edges[i].getA(), edges[i].getB());
borderX.push_back(texCoord);
}
std::sort(borderX.begin(), borderX.end(), TexturedPoint::compX);
int begin = borderX.front().x() > 0 ? borderX.front().x() : 0;
for (int x = begin; x < borderX.back().x() && x < maxX; ++x) {
TexturedPoint curPoint(x, curY);
curPoint.calcTextureCoordinates(borderX.front(),borderX.back());
if (0 == texture) {
canvas.drawPixel(x, curY, TexturedPoint::transformToColor(curPoint.getTexX(), curPoint.getTexY()));
}
else {
canvas.drawPixel(x, curY, texture->get_color(curPoint));
}
}
++curY;
}
std::vector<Edge>::iterator iter = edges.begin();
while (iter != edges.end()) {
if ( (*iter).getB().y() < curY) {
edges.erase(iter);
}
else {
++iter;
}
}
}
}
static uint8_t _point(double value)
{
return((uint8_t)(value*10 - trunc(value)*10)); // isolate the decimal point as an int
}
// Directs and executes one line of formatted input from protocol_process. While mostly
// incoming streaming g-code blocks, this also executes Grbl internal commands, such as
// settings, initiating the homing cycle, and toggling switch states. This differs from
// the realtime command module by being susceptible to when Grbl is ready to execute the
// next line during a cycle, so for switches like block delete, the switch only effects
// the lines that are processed afterward, not necessarily real-time during a cycle,
// since there are motions already stored in the buffer. However, this 'lag' should not
// be an issue, since these commands are not typically used during a cycle.
uint8_t system_execute_line(char *line)
{
uint8_t char_counter = 1;
uint8_t helper_var = 0; // Helper variable
float parameter, value;
switch( line[char_counter] ) {
case 0 : report_grbl_help(); break;
case '$': case 'G': case 'C': case 'X':
if ( line[(char_counter+1)] != 0 ) { return(STATUS_INVALID_STATEMENT); }
switch( line[char_counter] ) {
case '$' : // Prints Grbl settings
if ( sys.state & (STATE_CYCLE | STATE_HOLD) ) { return(STATUS_IDLE_ERROR); } // Block during cycle. Takes too long to print.
else { report_grbl_settings(); }
break;
case 'G' : // Prints gcode parser state
// TODO: Move this to realtime commands for GUIs to request this data during suspend-state.
report_gcode_modes();
break;
case 'C' : // Set check g-code mode [IDLE/CHECK]
// Perform reset when toggling off. Check g-code mode should only work if Grbl
// is idle and ready, regardless of alarm locks. This is mainly to keep things
// simple and consistent.
if ( sys.state == STATE_CHECK_MODE ) {
mc_reset();
report_feedback_message(MESSAGE_DISABLED);
} else {
if (sys.state) { return(STATUS_IDLE_ERROR); } // Requires no alarm mode.
sys.state = STATE_CHECK_MODE;
report_feedback_message(MESSAGE_ENABLED);
}
break;
case 'X' : // Disable alarm lock [ALARM]
if (sys.state == STATE_ALARM) {
report_feedback_message(MESSAGE_ALARM_UNLOCK);
sys.state = STATE_IDLE;
// Don't run startup script. Prevents stored moves in startup from causing accidents.
if (system_check_safety_door_ajar()) { // Check safety door switch before returning.
bit_true(sys_rt_exec_state, EXEC_SAFETY_DOOR);
protocol_execute_realtime(); // Enter safety door mode.
}
} // Otherwise, no effect.
break;
// case 'J' : break; // Jogging methods
// TODO: Here jogging can be placed for execution as a seperate subprogram. It does not need to be
// susceptible to other realtime commands except for e-stop. The jogging function is intended to
// be a basic toggle on/off with controlled acceleration and deceleration to prevent skipped
// steps. The user would supply the desired feedrate, axis to move, and direction. Toggle on would
// start motion and toggle off would initiate a deceleration to stop. One could 'feather' the
// motion by repeatedly toggling to slow the motion to the desired location. Location data would
// need to be updated real-time and supplied to the user through status queries.
// More controlled exact motions can be taken care of by inputting G0 or G1 commands, which are
// handled by the planner. It would be possible for the jog subprogram to insert blocks into the
// block buffer without having the planner plan them. It would need to manage de/ac-celerations
// on its own carefully. This approach could be effective and possibly size/memory efficient.
// }
// break;
}
break;
default :
// Block any system command that requires the state as IDLE/ALARM. (i.e. EEPROM, homing)
if ( !(sys.state == STATE_IDLE || sys.state == STATE_ALARM) ) { return(STATUS_IDLE_ERROR); }
switch( line[char_counter] ) {
case '#' : // Print Grbl NGC parameters
if ( line[++char_counter] != 0 ) { return(STATUS_INVALID_STATEMENT); }
else { report_ngc_parameters(); }
break;
case 'H' : // Perform homing cycle [IDLE/ALARM]
if (bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)) {
sys.state = STATE_HOMING; // Set system state variable
// Only perform homing if Grbl is idle or lost.
// TODO: Likely not required.
if (system_check_safety_door_ajar()) { // Check safety door switch before homing.
bit_true(sys_rt_exec_state, EXEC_SAFETY_DOOR);
protocol_execute_realtime(); // Enter safety door mode.
}
mc_homing_cycle();
if (!sys.abort) { // Execute startup scripts after successful homing.
sys.state = STATE_IDLE; // Set to IDLE when complete.
st_go_idle(); // Set steppers to the settings idle state before returning.
system_execute_startup(line);
}
} else { return(STATUS_SETTING_DISABLED); }
break;
case 'I' : // Print or store build info. [IDLE/ALARM]
if ( line[++char_counter] == 0 ) {
settings_read_build_info(line);
report_build_info(line);
} else { // Store startup line [IDLE/ALARM]
if(line[char_counter++] != '=') { return(STATUS_INVALID_STATEMENT); }
helper_var = char_counter; // Set helper variable as counter to start of user info line.
do {
line[char_counter-helper_var] = line[char_counter];
} while (line[char_counter++] != 0);
settings_store_build_info(line);
}
break;
case 'R' : // Restore defaults [IDLE/ALARM]
if (line[++char_counter] != 'S') { return(STATUS_INVALID_STATEMENT); }
if (line[++char_counter] != 'T') { return(STATUS_INVALID_STATEMENT); }
if (line[++char_counter] != '=') { return(STATUS_INVALID_STATEMENT); }
if (line[char_counter+2] != 0) { return(STATUS_INVALID_STATEMENT); }
switch (line[++char_counter]) {
case '$': settings_restore(SETTINGS_RESTORE_DEFAULTS); break;
case '#': settings_restore(SETTINGS_RESTORE_PARAMETERS); break;
case '*': settings_restore(SETTINGS_RESTORE_ALL); break;
default: return(STATUS_INVALID_STATEMENT);
}
report_feedback_message(MESSAGE_RESTORE_DEFAULTS);
mc_reset(); // Force reset to ensure settings are initialized correctly.
break;
case 'N' : // Startup lines. [IDLE/ALARM]
if ( line[++char_counter] == 0 ) { // Print startup lines
for (helper_var=0; helper_var < N_STARTUP_LINE; helper_var++) {
if (!(settings_read_startup_line(helper_var, line))) {
report_status_message(STATUS_SETTING_READ_FAIL);
} else {
report_startup_line(helper_var,line);
}
}
break;
} else { // Store startup line [IDLE Only] Prevents motion during ALARM.
if (sys.state != STATE_IDLE) { return(STATUS_IDLE_ERROR); } // Store only when idle.
helper_var = true; // Set helper_var to flag storing method.
// No break. Continues into default: to read remaining command characters.
}
default : // Storing setting methods [IDLE/ALARM]
if(!read_float(line, &char_counter, ¶meter)) { return(STATUS_BAD_NUMBER_FORMAT); }
if(line[char_counter++] != '=') { return(STATUS_INVALID_STATEMENT); }
if (helper_var) { // Store startup line
// Prepare sending gcode block to gcode parser by shifting all characters
helper_var = char_counter; // Set helper variable as counter to start of gcode block
do {
line[char_counter-helper_var] = line[char_counter];
} while (line[char_counter++] != 0);
// Execute gcode block to ensure block is valid.
helper_var = gc_execute_line(line); // Set helper_var to returned status code.
if (helper_var) { return(helper_var); }
else {
helper_var = trunc(parameter); // Set helper_var to int value of parameter
settings_store_startup_line(helper_var,line);
}
} else { // Store global setting.
if(!read_float(line, &char_counter, &value)) { return(STATUS_BAD_NUMBER_FORMAT); }
if((line[char_counter] != 0) || (parameter > 255)) { return(STATUS_INVALID_STATEMENT); }
return(settings_store_global_setting((uint8_t)parameter, value));
}
}
}
return(STATUS_OK); // If '$' command makes it to here, then everything's ok.
}
double Frac(double x)
{
return x-trunc(x);
}
/*
* getPacket
* description: retrieve data from the provides (input) port
*
* timeout: the amount of time to wait for data before a NULL is returned.
* Use 0.0 for non-blocking and -1 for blocking.
*/
BULKIO_dataFloat_In_i::dataTransfer *BULKIO_dataFloat_In_i::getPacket(float timeout, std::string streamID )
{
if (breakBlock) {
return NULL;
}
//if (workQueue.size() == 0) {
if ( (workQueue.size() == 0 ) or (( workQueue.size() != 0 ) and ( workQueue.size() == lastQueueSize )) ){
if (timeout == 0.0) {
lastQueueSize = workQueue.size();
return NULL;
} else if (timeout > 0){
uint64_t secs = (unsigned long)(trunc(timeout));
uint64_t msecs = (unsigned long)((timeout - secs) * 1e6);
boost::system_time to_time = boost::get_system_time() + boost::posix_time::seconds(secs) + boost::posix_time::microseconds(msecs);
boost::unique_lock< boost::mutex > lock(dataAvailableMutex);
if ( dataAvailable.timed_wait( lock, to_time) == false ) {
return NULL;
}
if (breakBlock) {
return NULL;
}
} else {
boost::unique_lock< boost::mutex > lock(dataAvailableMutex);
dataAvailable.wait(lock);
if (breakBlock) {
return NULL;
}
}
}
boost::mutex::scoped_lock lock1(dataBufferLock);
BULKIO_dataFloat_In_i::dataTransfer *tmp = NULL;
if ( streamID == "" ){
tmp = workQueue.front();
workQueue.pop_front();
} else {
std::deque< dataTransfer * >::iterator p = workQueue.begin();
while ( p != workQueue.end() ) {
if ( (*p)->streamID == streamID ) {
tmp = *p;
workQueue.erase(p);
break;
}
p++;
}
}
if ( tmp == NULL ) {
lastQueueSize = workQueue.size();
return NULL;
}
boost::mutex::scoped_lock lock2(sriUpdateLock);
if (tmp->EOS) {
RH_SRIMap::iterator target = currentHs.find(std::string(tmp->streamID));
if (target != currentHs.end()) {
if (target->second.first.blocking) {
RH_SRIMap::iterator currH;
bool keepBlocking = false;
for (currH = currentHs.begin(); currH != currentHs.end(); currH++) {
if (currH->second.first.blocking) {
keepBlocking = true;
break;
}
}
if (!keepBlocking) {
queueSem->setCurrValue(0);
blocking = false;
}
}
currentHs.erase(target);
}
}
if (blocking) {
queueSem->decr();
}
lastQueueSize=0;
return tmp;
}
// 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)
float target[3], offset[3];
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(X_AXIS, Z_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;
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 < 1 || 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 {
int_value--; // Adjust P index to EEPROM coordinate data indexing.
if (l == 20) {
settings_write_coord_data(int_value,gc.position);
// Update system coordinate system if currently active.
if (gc.coord_select == int_value) { memcpy(gc.coord_system,gc.position,sizeof(gc.position)); }
} else {
float coord_data[N_AXIS];
if (!settings_read_coord_data(int_value,coord_data)) { return(STATUS_SETTING_READ_FAIL); }
// Update axes defined only in block. Always in machine coordinates. Can change non-active system.
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)) ) { coord_data[i] = target[i]; }
}
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];
}
}
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], settings.default_seek_rate, false);
}
// 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); }
}
mc_line(coord_data[X_AXIS], coord_data[Y_AXIS], coord_data[Z_AXIS], settings.default_seek_rate, false);
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;
for (i=0; i<=2; 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;
for (i=0; i<=2; 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 { mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], settings.default_seek_rate, false); }
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 { mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS],
(gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode); }
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);
}
void trunc_l_d(double *source,long long *dest)
{
*dest = trunc(*source);
}
int main(int argc, char *argv[]) {
wq.size = 0;
wq.head = NULL;
wq.tail = NULL;
wq.push_wait = &push_wait;
wq.peek = &peek;
wq.pop = &pop;
rq.size = 0;
rq.head = NULL;
rq.tail = NULL;
rq.push_exponential = &push_exponential;
rq.peek = &peek;
rq.pop = &pop;
dq.size = 0;
dq.head = NULL;
dq.tail = NULL;
dq.push_wait = &push_wait;
dq.peek = &peek;
dq.pop = &pop;
node_counter = 0;
FILE *file = fopen(argv[1], "r");
fseek(file, 0, SEEK_END);
long fsize = ftell(file);
fseek(file, 0, SEEK_SET);
char *string =malloc(fsize+1);
fread(string, fsize, 1, file);
fclose(file);
string[fsize] = 0;
//add stuff to node -> waiting queue
char *token, *saveptr,*saveptr2, *temp, *ptr_end;
int j, k, a;
for(j = 0; ; j++, string = NULL){
token = strtok_r(string, "\n", &saveptr);
if (token == NULL) {
break;
}
//get current time + x
struct timeval tv;
//printf("current%d", time);
Node *n = (Node*)malloc(sizeof(Node));
n->name = malloc(11);
for (k = 0; ; k++, token = NULL){
temp = strtok_r(token, "\' \', \t", &saveptr2);
if (temp == NULL) {
break;
}
if(k == 0) {
strcpy(n->name, temp);
}
else if(k == 1) {
a = atoi(temp);
n->start_time = a;
}
else if(k == 2) {
a = atoi(temp);
n->cpu_time = a * 1000;
}
else {
a = atoi(temp);
n->io_count = a;
n->io_blocks_left = trunc((n->io_count + 8191) / 8192);
//printf("io blocks left = %d+8191 div 8192 = %d", n->io_count, n->io_blocks_left);
n->priority = 1;
n->cpu_completed= 0;
n->io_block_time = n->cpu_time / n->io_blocks_left;
n->io_block_next = n->io_block_time;
//printf("\nblock time: %d", n->io_blocks_left);
n->time_slice = 10;
wq.push_wait(&wq, n);
node_counter++;
}
}
}
// wait for things to go down
pthread_t waiting;
pthread_t ready;
pthread_t finished;
if (pthread_create(&waiting, NULL, &exponentialHold, NULL)){
printf("Could not create thread \n");
}
if (pthread_create(&ready, NULL, &exponentialReady, NULL)) {
printf("Could not create thread \n");
}
if (pthread_create(&finished, NULL, &done_queue, NULL)) {
printf("Could not create thread \n");
}
if(pthread_join(waiting, NULL)){
printf("Could not join thread\n");
}
if(pthread_join(ready, NULL)) {
printf("Could not join thread\n");
}
if(pthread_join(finished, NULL)) {
printf("Could not join thread\n");
}
//printf("%s", dq.head->name);
//pop
//run
//cpu_time != compl_time? push(ready)
return 0;
}
void trunc_w_d(double *source,int *dest)
{
*dest = trunc(*source);
}
npy_double npy_trunc(npy_double x)
{
return trunc(x);
}
///\brief Opens joystick port, reads from port and publishes while node is active
int main(int argc, char **argv)
{
diagnostic_.add("Joystick Driver Status", this, &Joystick::diagnostics);
diagnostic_.setHardwareID("none");
// Parameters
ros::NodeHandle nh_param("~");
pub_ = nh_.advertise<sensor_msgs::Joy>("joy", 1);
nh_param.getParam("dev", joy_dev_);
nh_param.param<double>("deadzone", deadzone_, 0.05);
nh_param.param<double>("autorepeat_rate", autorepeat_rate_, 0);
nh_param.param<double>("coalesce_interval", coalesce_interval_, 0.001);
// Checks on parameters
if (autorepeat_rate_ > 1 / coalesce_interval_)
ROS_WARN("joy_node: autorepeat_rate (%f Hz) > 1/coalesce_interval (%f Hz) does not make sense. Timing behavior is not well defined.", autorepeat_rate_, 1/coalesce_interval_);
if (deadzone_ >= 1)
{
ROS_WARN("joy_node: deadzone greater than 1 was requested. The semantics of deadzone have changed. It is now related to the range [-1:1] instead of [-32767:32767]. For now I am dividing your deadzone by 32767, but this behavior is deprecated so you need to update your launch file.");
deadzone_ /= 32767;
}
if (deadzone_ > 0.9)
{
ROS_WARN("joy_node: deadzone (%f) greater than 0.9, setting it to 0.9", deadzone_);
deadzone_ = 0.9;
}
if (deadzone_ < 0)
{
ROS_WARN("joy_node: deadzone_ (%f) less than 0, setting to 0.", deadzone_);
deadzone_ = 0;
}
if (autorepeat_rate_ < 0)
{
ROS_WARN("joy_node: autorepeat_rate (%f) less than 0, setting to 0.", autorepeat_rate_);
autorepeat_rate_ = 0;
}
if (coalesce_interval_ < 0)
{
ROS_WARN("joy_node: coalesce_interval (%f) less than 0, setting to 0.", coalesce_interval_);
coalesce_interval_ = 0;
}
// Parameter conversions
double autorepeat_interval = 1 / autorepeat_rate_;
double scale = -1. / (1. - deadzone_) / 32767.;
double unscaled_deadzone = 32767. * deadzone_;
js_event event;
struct timeval tv;
fd_set set;
int joy_fd;
event_count_ = 0;
pub_count_ = 0;
lastDiagTime_ = ros::Time::now().toSec();
// Big while loop opens, publishes
while (nh_.ok())
{
open_ = false;
diagnostic_.force_update();
bool first_fault = true;
while (true)
{
ros::spinOnce();
if (!nh_.ok())
goto cleanup;
joy_fd = open(joy_dev_.c_str(), O_RDONLY);
if (joy_fd != -1)
{
// There seems to be a bug in the driver or something where the
// initial events that are to define the initial state of the
// joystick are not the values of the joystick when it was opened
// but rather the values of the joystick when it was last closed.
// Opening then closing and opening again is a hack to get more
// accurate initial state data.
close(joy_fd);
joy_fd = open(joy_dev_.c_str(), O_RDONLY);
}
if (joy_fd != -1)
break;
if (first_fault)
{
ROS_ERROR("Couldn't open joystick %s. Will retry every second.", joy_dev_.c_str());
first_fault = false;
}
sleep(1.0);
diagnostic_.update();
}
ROS_INFO("Opened joystick: %s. deadzone_: %f.", joy_dev_.c_str(), deadzone_);
open_ = true;
diagnostic_.force_update();
bool tv_set = false;
bool publication_pending = false;
tv.tv_sec = 1;
tv.tv_usec = 0;
sensor_msgs::Joy joy_msg; // Here because we want to reset it on device close.
while (nh_.ok())
{
ros::spinOnce();
bool publish_now = false;
bool publish_soon = false;
FD_ZERO(&set);
FD_SET(joy_fd, &set);
//ROS_INFO("Select...");
int select_out = select(joy_fd+1, &set, NULL, NULL, &tv);
//ROS_INFO("Tick...");
if (select_out == -1)
{
tv.tv_sec = 0;
tv.tv_usec = 0;
//ROS_INFO("Select returned negative. %i", ros::isShuttingDown());
continue;
// break; // Joystick is probably closed. Not sure if this case is useful.
}
if (FD_ISSET(joy_fd, &set))
{
if (read(joy_fd, &event, sizeof(js_event)) == -1 && errno != EAGAIN)
break; // Joystick is probably closed. Definitely occurs.
//ROS_INFO("Read data...");
joy_msg.header.stamp = ros::Time().now();
event_count_++;
switch(event.type)
{
case JS_EVENT_BUTTON:
case JS_EVENT_BUTTON | JS_EVENT_INIT:
if(event.number >= joy_msg.buttons.size())
{
int old_size = joy_msg.buttons.size();
joy_msg.buttons.resize(event.number+1);
for(unsigned int i=old_size; i<joy_msg.buttons.size(); i++)
joy_msg.buttons[i] = 0.0;
}
joy_msg.buttons[event.number] = (event.value ? 1 : 0);
// For initial events, wait a bit before sending to try to catch
// all the initial events.
if (!(event.type & JS_EVENT_INIT))
publish_now = true;
else
publish_soon = true;
break;
case JS_EVENT_AXIS:
case JS_EVENT_AXIS | JS_EVENT_INIT:
if(event.number >= joy_msg.axes.size())
{
int old_size = joy_msg.axes.size();
joy_msg.axes.resize(event.number+1);
for(unsigned int i=old_size; i<joy_msg.axes.size(); i++)
joy_msg.axes[i] = 0.0;
}
if (!(event.type & JS_EVENT_INIT)) // Init event.value is wrong.
{
double val = event.value;
// Allows deadzone to be "smooth"
if (val > unscaled_deadzone)
val -= unscaled_deadzone;
else if (val < -unscaled_deadzone)
val += unscaled_deadzone;
else
val = 0;
joy_msg.axes[event.number] = val * scale;
}
// Will wait a bit before sending to try to combine events.
publish_soon = true;
break;
default:
ROS_WARN("joy_node: Unknown event type. Please file a ticket. time=%u, value=%d, type=%Xh, number=%d", event.time, event.value, event.type, event.number);
break;
}
}
else if (tv_set) // Assume that the timer has expired.
publish_now = true;
if (publish_now)
{
// Assume that all the JS_EVENT_INIT messages have arrived already.
// This should be the case as the kernel sends them along as soon as
// the device opens.
//ROS_INFO("Publish...");
pub_.publish(joy_msg);
publish_now = false;
tv_set = false;
publication_pending = false;
publish_soon = false;
pub_count_++;
}
// If an axis event occurred, start a timer to combine with other
// events.
if (!publication_pending && publish_soon)
{
tv.tv_sec = trunc(coalesce_interval_);
tv.tv_usec = (coalesce_interval_ - tv.tv_sec) * 1e6;
publication_pending = true;
tv_set = true;
//ROS_INFO("Pub pending...");
}
// If nothing is going on, start a timer to do autorepeat.
if (!tv_set && autorepeat_rate_ > 0)
{
tv.tv_sec = trunc(autorepeat_interval);
tv.tv_usec = (autorepeat_interval - tv.tv_sec) * 1e6;
tv_set = true;
//ROS_INFO("Autorepeat pending... %i %i", tv.tv_sec, tv.tv_usec);
}
if (!tv_set)
{
tv.tv_sec = 1;
tv.tv_usec = 0;
}
diagnostic_.update();
} // End of joystick open loop.
close(joy_fd);
ros::spinOnce();
if (nh_.ok())
ROS_ERROR("Connection to joystick device lost unexpectedly. Will reopen.");
}
cleanup:
ROS_INFO("joy_node shut down.");
return 0;
}