uint8_t Decimate::configure(AVDMGenericVideoStream *in)
{
_in=in;
#define PX(x) &(_param->x)
#define SZT(x) sizeof(x)/sizeof(diaMenuEntry *)
ELEM_TYPE_FLOAT t1=(ELEM_TYPE_FLOAT)_param->threshold;
ELEM_TYPE_FLOAT t2=(ELEM_TYPE_FLOAT)_param->threshold2;
diaMenuEntry tMode[]={
{0, QT_TR_NOOP("Discard closer"),NULL},
{1, QT_TR_NOOP("Replace (interpolate)"),NULL},
{2, QT_TR_NOOP("Discard longer dupe (animés)"),NULL},
{3, QT_TR_NOOP("Pulldown dupe removal"),NULL}
};
diaMenuEntry tQuality[]={
{0, QT_TR_NOOP("Fastest (no chroma, partial luma)"),NULL},
{1, QT_TR_NOOP("Fast (partial luma and chroma)"),NULL},
{2, QT_TR_NOOP("Medium (full luma, no chroma)"),NULL},
{3, QT_TR_NOOP("Slow (full luma and chroma)"),NULL}
};
diaElemMenu menuMode(PX(mode),QT_TR_NOOP("_Mode:"), 4,tMode);
diaElemMenu menuQuality(PX(quality),QT_TR_NOOP("_Quality:"), 4,tQuality);
diaElemFloat menuThresh1(&t1,QT_TR_NOOP("_Threshold 1:"),0,100.);
diaElemFloat menuThresh2(&t2,QT_TR_NOOP("T_hreshold 2:"),0,100.);
diaElemUInteger cycle(PX(cycle),QT_TR_NOOP("C_ycle:"),2,40);
diaElem *elems[]={&cycle,&menuMode,&menuQuality,&menuThresh1,&menuThresh2};
if(diaFactoryRun(QT_TR_NOOP("Decomb Decimate"),5,elems))
{
_param->threshold=(double )t1;
_param->threshold2=(double )t2;
return 1;
}
return 0;
}
/* is if the starting node has been repeated */
int
cycle( uint to, uint start, uint length )
{
uint i;
if( length > SIZE )
return 1;
if( to == start )
return 1;
for( i = 0; i < SIZE; ++i )
{
if( graph[to][i] != NO_CONN )
{
if( cycle( i, start, length + 1 ) )
return 1;
}
}
return 0;
} /**~ end cycle ~**/
void ServiceSuspender::onEvent(BusEvent event, va_list ap) {
if (event == BusEvent::CYCLE) {
cycle();
} else if (eb_inGroup(event, BusEventGroup::BUTTON)) {
if (suspendStart == 0) {
#if LOG
log(F("SU SU"));
#endif
eb_fire(BusEvent::SERVICE_SUSPEND);
}
suspendStart = util_ms();
} else if (event == BusEvent::SERVICE_RESUME) {
#if LOG
log(F("SU D RS"));
#endif
suspendStart = 0;
}
}
void
s128_stream(s128_ctx *c, UCHAR *buf, int nbytes)
{
UCHAR *endbuf;
WORD t = 0;
if ((nbytes & 3) != 0)
abort();
endbuf = &buf[nbytes];
/* do small or odd size buffers the slow way, at least at first */
while ((nbytes % (N*4)) != 0) {
cycle(c->R);
t = nltap(c);
XORWORD(t, buf);
buf += 4;
nbytes -= 4;
}
/* now do lots at a time, if there's any left */
while (buf < endbuf)
{
SROUND(0);
SROUND(1);
SROUND(2);
SROUND(3);
SROUND(4);
SROUND(5);
SROUND(6);
SROUND(7);
SROUND(8);
SROUND(9);
SROUND(10);
SROUND(11);
SROUND(12);
SROUND(13);
SROUND(14);
SROUND(15);
SROUND(16);
buf += 4*17;
}
}
static void execute(void)
{
struct _Address Destination;
unsigned int DValue;
JSR_Instr Instr;
Memory_RetrWordFromPC(&Instr.Code);
if(!EA_GetFromPC(&Destination, 32, Instr.Bits.EAMode, Instr.Bits.EARegister)) return;
EA_GetEA(&DValue, &Destination);
Stack_Push(32,memory_core.pc);
/* Set the new PC */
memory_core.pc=DValue;
/* Condition codes are not affected */
cycle(JSRTime[cycle_EA(Instr.Bits.EARegister,Instr.Bits.EAMode)]);
return;
}
static void execute(void)
{
struct _Address Source,Destination;
unsigned int Result, SValue, DValue;
ADDA_Instr Instr;
Memory_RetrWordFromPC(&Instr.Code);
if(!EA_GetFromPC(&Source, 32, Instr.Bits.EAMode, Instr.Bits.EARegister)) return;
if(!EA_GetFromPC(&Destination, 32, 1, Instr.Bits.Register)) return;
EA_GetValue(&SValue, &Source);
EA_GetValue(&DValue, &Destination);
Result = SValue + DValue;
/* Status register is not affected */
EA_PutValue(&Destination, Result);
cycle(ADDATime[cycle_EA(Instr.Bits.EARegister,Instr.Bits.EAMode)]);
}
static void execute(void)
{
struct _Address Source,Destination;
unsigned int SValue;
LEA_Instr Instr;
Memory_RetrWordFromPC(&Instr.Code);
if(!EA_GetFromPC(&Source, 32, Instr.Bits.EAMode, Instr.Bits.EARegister)) return;
if(!EA_GetFromPC(&Destination, 32, 1, Instr.Bits.Register)) return;
EA_GetEA(&SValue, &Source);
/* Condition codes are not affected */
EA_PutValue(&Destination, SValue);
cycle(LEATime[cycle_EA(Instr.Bits.EARegister,Instr.Bits.EAMode)]);
return;
}
/**
Initialize an Sober128 context (only the key)
@param c [out] The destination of the Sober128 state
@param key The secret key
@param keylen The length of the secret key (octets)
@return CRYPT_OK if successful
*/
int sober128_stream_setup(sober128_state *c, const unsigned char *key, unsigned long keylen)
{
ulong32 i, k;
LTC_ARGCHK(c != NULL);
LTC_ARGCHK(key != NULL);
LTC_ARGCHK(keylen > 0);
/* keylen must be multiple of 4 bytes */
if ((keylen & 3) != 0) {
return CRYPT_INVALID_KEYSIZE;
}
/* Register initialised to Fibonacci numbers */
c->R[0] = 1;
c->R[1] = 1;
for (i = 2; i < N; ++i) {
c->R[i] = c->R[i-1] + c->R[i-2];
}
c->konst = INITKONST;
for (i = 0; i < keylen; i += 4) {
k = BYTE2WORD((unsigned char *)&key[i]);
ADDKEY(k);
cycle(c->R);
XORNL(nltap(c));
}
/* also fold in the length of the key */
ADDKEY(keylen);
/* now diffuse */
s128_diffuse(c);
s128_genkonst(c);
s128_savestate(c);
c->nbuf = 0;
return CRYPT_OK;
}
bool NETHER::gamecycle(int w,int h)
{
int i;
bool retval=true;
unsigned char *keyboard;
SDL_PumpEvents();
keyboard = SDL_GetKeyState(NULL);
#ifdef _WRITE_REPORT_
fprintf(debug_fp,"Cycle start.\n");
fprintf(debug_fp,"game_state: %i\n",game_state);
fflush(debug_fp);
#endif
switch(game_state) {
case STATE_PLAYING:
retval=cycle(keyboard);
break;
case STATE_CONSTRUCTION:
retval=construction_cycle(keyboard);
break;
case STATE_PAUSE:
case STATE_SAVINGGAME:
case STATE_LOADINGGAME:
retval=option_cycle(keyboard);
break;
} /* switch */
for(i=0;i<SDLK_LAST;i++) old_keyboard[i]=keyboard[i];
#ifdef _WRITE_REPORT_
fprintf(debug_fp,"Cycle end: %i\n",retval);
fflush(debug_fp);
#endif
return retval;
} /* NETHER::gamecycle */
/**
* Draws a label near the edge
*/
void GuiEdge::drawText (QPainter * painter) const
{
int len = edgeLabel().length();
float k = 0.8f;
float start = 0.1f;
if (Edge::pred() && !addAux (Edge::pred())->real())//press letters to the virtual nodes
{
k += start;
start = 0;
}
if (Edge::succ() && !addAux (Edge::succ())->real())//press letters to the virtual nodes
{
k = 1 - start;
}
if (reverse)
{
k = -k;
start = 1 - start;
}
QPointF delta = edge_end_point_priv - edge_start_point_priv;
float letter_size = 1.3*edge_curve_priv.length()/len;
if (letter_size > 25) letter_size = 25;
if (letter_size > 8 && !cycle())
{
for (int i = 0; i < len; ++i)
{
float slope = edge_curve_priv.slopeAtPercent (start + k*float(i)/len);
float ang = atan(slope);
QPointF pos = edge_curve_priv.pointAtPercent(start + k*float(i)/len);
pos += QPointF (cos (Pi/2 + ang), sin(Pi/2 + ang))*letter_size;
QFont curf = painter->font();
curf.setPixelSize (letter_size);
painter->setFont (curf);
painter->drawText (pos, QString(edgeLabel().at(i)));
}
}
}
void decrypt_sap(unsigned char* sapIn, unsigned char* sapOut)
{
uint32_t key_schedule[11][4];
unsigned char* iv;
print_block("Base sap: ", &sapIn[0xf0]);
z_xor(sapIn, sapOut, 16);
generate_key_schedule(sap_key_material, key_schedule);
print_block("lastSap before cycle: ", &sapOut[0xf0]);
for (int i = 0xf0; i >= 0x00; i-=0x10)
{
printf("Ready to cycle %02X\n", i);
cycle(&sapOut[i], key_schedule);
print_block("After cycling, block is: ", &sapOut[i]);
if (i > 0)
{ // xor with previous block
iv = &sapOut[i-0x10];
}
else
{ // xor with sap IV
iv = sap_iv;
}
for (int j = 0; j < 16; j++)
{
printf("%02X ^ %02X -> %02X\n", sapOut[i+j], iv[j], sapOut[i+j] ^ iv[j]);
sapOut[i+j] = sapOut[i+j] ^ iv[j];
}
printf("Decrypted SAP %02X-%02X:\n", i, i+0xf);
print_block("", &sapOut[i]);
}
// Lastly grind the whole thing through x_key. This is the last time we modify sap
x_xor(sapOut, sapOut, 16);
printf("Sap is decrypted to\n");
for (int i = 0xf0; i >= 0x00; i-=0x10)
{
printf("Final SAP %02X-%02X: ", i, i+0xf);
print_block("", &sapOut[i]);
}
}
static void trinkle(unsigned char *head, size_t width, cmpfun cmp, size_t pp[2], int pshift, int trusty, size_t lp[])
{
unsigned char *stepson,
*rt, *lf;
size_t p[2];
unsigned char *ar[14 * sizeof(size_t) + 1];
int i = 1;
int trail;
p[0] = pp[0];
p[1] = pp[1];
ar[0] = head;
while(p[0] != 1 || p[1] != 0) {
stepson = head - lp[pshift];
if((*cmp)(stepson, ar[0]) <= 0) {
break;
}
if(!trusty && pshift > 1) {
rt = head - width;
lf = head - width - lp[pshift - 2];
if((*cmp)(rt, stepson) >= 0 || (*cmp)(lf, stepson) >= 0) {
break;
}
}
ar[i++] = stepson;
head = stepson;
trail = pntz(p);
shr(p, trail);
pshift += trail;
trusty = 0;
}
if(!trusty) {
cycle(width, ar, i);
sift(head, width, cmp, pshift, lp);
}
}
static void
s128_loadkey(s128_ctx *c, UCHAR key[], int keylen)
{
int i;
WORD k;
/* start folding in key, reject odd sized keys */
if ((keylen & 3) != 0)
abort();
for (i = 0; i < keylen; i += 4)
{
k = BYTE2WORD(&key[i]);
ADDKEY(k);
cycle(c->R);
XORNL(nltap(c));
}
/* also fold in the length of the key */
ADDKEY(keylen);
/* now diffuse */
s128_diffuse(c);
}
void cycle (int n,int *counter) {
short int isOne = 0;
if ( n == 1) { //these are 3n+1 rules, just apply it
isOne = 1;
} else if ( n%2 == 1 ){
n = n*3 + 1;
} else {
n /= 2;
}
*counter = *counter + 1; //circle-length +1
if (isOne) { //if the item is 1, then end this recursive func.
return;
} else {
cycle ( n , counter); //otherwise, continue the recursive func.
}
return;
}
static void execute(void)
{
struct _Address Source,Destination;
unsigned int Result, SValue, DValue;
CMPI_Instr Instr;
Memory_RetrWordFromPC(&Instr.Code);
if(!EA_GetFromPC(&Source, 32, 7, 4)) return;
if(!EA_GetFromPC(&Destination, 32, 0, Instr.Bits.Register)) return;
EA_GetValue(&SValue, &Source);
EA_GetValue(&DValue, &Destination);
Result = DValue - SValue;
/* Set the status register */
SR_Set(I_CMPI, SValue, DValue, Result);
cycle(CMPITime);
return;
}
/* maybe you can find a better algorithm? */
int
traverse() {
/* Vars */
uint i, j;
/*------*/
/* For each row in the graph... */
for( i = 0; i < SIZE; ++i )
{
for( j = 0; j < SIZE; ++j )
{
/* For each column... */
/* Is there a directed edge i->j? */
if( graph[i][j] != NO_CONN )
{
/* Try to follow this edge */
if( cycle( j, i, 0 ) )
return 1;
}
}
}
return 0;
} /**~ end traverse ~**/
int main(int argc, const char *argv[]) {
if(!allocate_registers()) {
printf("error allocating registers\n");
return EXIT_FAILURE;
}
if(!allocate_ram()) {
printf("error allocating ram\n");
return EXIT_FAILURE;
}
if(!load_program(argv[1])) {
printf("error loading program\n");
return EXIT_FAILURE;
}
while(running) {
cycle();
}
//debug_ram();
return EXIT_SUCCESS;
}
int MQTTYield(Client* c, int timeout_ms)
{
int rc = SUCCESS;
Timer timer;
if (!c->isconnected) {
return FAILURE;
}
InitTimer(&timer);
countdown_ms(&timer, timeout_ms);
while (!expired(&timer))
{
if (cycle(c, &timer) == FAILURE)
{
rc = FAILURE;
break;
}
}
DeInitTimer(&timer); //STM: added this line
return rc;
}
void
BlockingResourceBase::CheckAcquire(const CallStack& aCallContext)
{
if (eCondVar == mDDEntry->mType) {
NS_NOTYETIMPLEMENTED(
"FIXME bug 456272: annots. to allow CheckAcquire()ing condvars");
return;
}
BlockingResourceBase* chainFront = ResourceChainFront();
nsAutoPtr<DDT::ResourceAcquisitionArray> cycle(
sDeadlockDetector->CheckAcquisition(
chainFront ? chainFront->mDDEntry : 0, mDDEntry,
aCallContext));
if (!cycle)
return;
fputs("###!!! ERROR: Potential deadlock detected:\n", stderr);
nsCAutoString out("Potential deadlock detected:\n");
bool maybeImminent = PrintCycle(cycle, out);
if (maybeImminent) {
fputs("\n###!!! Deadlock may happen NOW!\n\n", stderr);
out.Append("\n###!!! Deadlock may happen NOW!\n\n");
} else {
fputs("\nDeadlock may happen for some other execution\n\n",
stderr);
out.Append("\nDeadlock may happen for some other execution\n\n");
}
// XXX can customize behavior on whether we /think/ deadlock is
// XXX about to happen. for example:
// XXX if (maybeImminent)
// NS_RUNTIMEABORT(out.get());
NS_ERROR(out.get());
}
SimADC::SimADC()
:runner(*this, "Runner",
epicsThreadGetStackSize(epicsThreadStackSmall),
epicsThreadPriorityMedium)
,runner_stop(false)
{
mult.value= rate.value= 1.0;
shift.value= offset.value= 0.0;
freq.value=90.0;
offset.egu = mult.egu = "V";
rate.egu = "Hz";
rate.displayLow = rate.ctrlLow = 0.0;
rate.displayHigh = rate.ctrlHigh = 1e6;
shift.displayLow = shift.ctrlLow = 0.0;
shift.displayHigh = shift.ctrlHigh = 99.999999;
shift.egu = "%";
freq.egu = "deg/pt";
nSamples.value = 10;
prev_nSamples = 0;
cycle();
operation.choices.resize(2);
operation.choices[0] = "Stop";
operation.choices[1] = "Run";
operation.value = 0;
data.remoteWritable = false;
X.remoteWritable = false;
runner.start();
}
int main(void)
{
int i,j;
while(scanf("%d %d",&i,&j))
{
printf("%d %d ",i,j);
if(i>j)
{
i^=j;
j^=i;
i^=j;
}
int result=cycle(i,j);
printf("%d\n",result);
}
return 0;
}
void dfs_visit(struct wait_graph_node *ptr_wait)
{
struct wait_graph_edge *ptr_edge;
//printf("\nInside DFS visit");
//fflush(stdout);
ptr_wait->visited = 1;/*paint it grey*/
ptr_edge = ptr_wait->edge;
while(ptr_edge != NULL)
{
if((ptr_edge->edge_to)->visited == 1)/*hey its grey, I have visited it before and I just traveled a back edge*/
{
printf("\nA cycle has been detected ");
fflush(stdout);
cycle(ptr_edge->edge_to);
}
if ((ptr_edge->edge_to)->visited == 0)
{
dfs_visit(ptr_edge->edge_to);
}
ptr_edge = ptr_edge->next_edge;
}
ptr_wait->visited = 2;
}
/**
Read from the PRNG
@param out Destination
@param outlen Length of output
@param prng The active PRNG to read from
@return Number of octets read
*/
unsigned long sober128_read(unsigned char *out, unsigned long outlen, prng_state *prng)
{
struct sober128_prng *c;
ulong32 t, tlen;
LTC_ARGCHK(out != NULL);
LTC_ARGCHK(prng != NULL);
#ifdef LTC_VALGRIND
zeromem(out, outlen);
#endif
c = &(prng->sober128);
t = 0;
tlen = outlen;
/* handle any previously buffered bytes */
while (c->nbuf != 0 && outlen != 0) {
*out++ ^= c->sbuf & 0xFF;
c->sbuf >>= 8;
c->nbuf -= 8;
--outlen;
}
#ifndef LTC_SMALL_CODE
/* do lots at a time, if there's enough to do */
while (outlen >= N*4) {
SROUND(0);
SROUND(1);
SROUND(2);
SROUND(3);
SROUND(4);
SROUND(5);
SROUND(6);
SROUND(7);
SROUND(8);
SROUND(9);
SROUND(10);
SROUND(11);
SROUND(12);
SROUND(13);
SROUND(14);
SROUND(15);
SROUND(16);
out += 4*N;
outlen -= 4*N;
}
#endif
/* do small or odd size buffers the slow way */
while (4 <= outlen) {
cycle(c->R);
t = nltap(c);
XORWORD(t, out);
out += 4;
outlen -= 4;
}
/* handle any trailing bytes */
if (outlen != 0) {
cycle(c->R);
c->sbuf = nltap(c);
c->nbuf = 32;
while (c->nbuf != 0 && outlen != 0) {
*out++ ^= c->sbuf & 0xFF;
c->sbuf >>= 8;
c->nbuf -= 8;
--outlen;
}
}
int main(int argc, char *argv[]) {
char** tokens;
int count = 1;
int addr1 = 0;
int addr2 = 0;
int num_inst = 0;
int i = 100; //for loop
int mem_dump_set = 0;
int debug_set = 0;
int num_inst_set = 0;
int pipe_dump_set = 0;
/* Error Checking */
if (argc < 2)
{
printf("Error: usage: %s [-m addr1:addr2] [-d] [-n num_instr] inputBinary\n", argv[0]);
exit(1);
}
initialize(argv[argc-1]);
//for checking parse result
//print_parse_result();
while(count != argc-1){
if(strcmp(argv[count], "-m") == 0){
tokens = str_split(argv[++count],':');
addr1 = (int)strtol(*(tokens), NULL, 16);
addr2 = (int)strtol(*(tokens+1), NULL, 16);
mem_dump_set = 1;
}
else if(strcmp(argv[count], "-d") == 0)
debug_set = 1;
else if(strcmp(argv[count], "-n") == 0){
num_inst = (int)strtol(argv[++count], NULL, 10);
num_inst_set = 1;
}
else if(strcmp(argv[count], "-p") == 0)
pipe_dump_set = 1;
else{
printf("Error: usage: %s [-nobp] [-f] [-m addr1:addr2] [-d] [-p] [-n num_instr] inputBinary\n", argv[0]);
//You must add nobp and f option yourself
exit(1);
}
count++;
}
if(num_inst_set) i = num_inst;
if(debug_set){
printf("Simulating for %d cycles...\n\n", i);
for(; i > 0; i--){
if (RUN_BIT == FALSE){
printf("Simulator halted\n\n");
break;
}
cycle();
if(pipe_dump_set) pdump();
rdump();
if(mem_dump_set) mdump(addr1, addr2);
}
}
else{
run(i);
if(pipe_dump_set) pdump();
rdump();
if(mem_dump_set) mdump(addr1, addr2);
}
return 0;
}
command()
{
double tval;
int i,echeck;
int pmin,pmax,pstep;
while (echeck = getstring(": ")) {
if (echeck == EOF) {
fileinput(EOF);
}
else if (in[0] == '\0') {
errprint("");
}
else if (startsame(in,"cycle")) {
cycle();
}
else if (startsame(in,"clear")) {
clear();
}
else if (startsame(in,"coarticulation")) {
getint(&coartflag);
}
else if (startsame(in,"rc")) {
zarrays();
cycle();
}
else if (startsame(in,"wordacts")) {
scr_words(printmin,printmax,3,0,"MAX");
}
else if (startsame(in,"wtacts")) {
scr_words(printmin,printmax,3,0,"ALL");
}
else if (startsame(in,"owtacts")) {
getstring("word: ");
scr_words(printmin,printmax,3,0,in);
}
else if (startsame(in,"phonacts")) {
scr_phonemes(printmin,printmax,3,0);
}
else if (startsame(in,"featacts")) {
scr_features();
}
else if (startsame(in,"sfeatacts")) {
getstring("fname: ");
sfeatacts(in);
}
else if (startsame(in,"memo")) {
getstring("string: ");
strcpy(memo,in);
}
else if (startsame(in,"expr")) {
setex();
}
else if (startsame(in,"fcopt")) {
getint(&fcflag);
}
else if (startsame(in,"fpcyc")) {
getint(&fpcyc);
}
else if (startsame(in,"finput")) {
fileinput(NONSTD);
}
else if (startsame(in,"inoise")) {
getval(&inoise);
}
else if (startsame(in,"inspecs")) {
getstring("File name (- = stdin): ");
inspecs(in);
}
else if (startsame(in,"infeatures")) {
getstring("File name: ");
infeats(in);
}
/* NOT PRESENTLY OPERATIVE -- JLM 10-5-82
else if (startsame(in,"wsubset")) {
wordsubset();
}
*/
else if (startsame(in,"test")) {
getstring("test string: ");
strcpy(memo,in);
test(in);
}
else if (startsame(in,"topdown")) {
topdown();
}
else if (startsame(in,"output")) {
setout();
}
else if (startsame(in,"ofile")) {
getstring("give filename (or - for none): ");
setoutfile(in);
}
else if (in[0] == '?') {
help();
}
else if (startsame(in,"help")) {
help();
}
else if (startsame(in,"lexicon")) {
getlex();
}
else if (startsame(in,"params")) {
getpars();
}
else if (startsame(in,"quit")) {
quit();
}
else if (startsame(in,"decay")) {
getdouble(decay,NLEVS,levlabs);
}
else if (startsame(in,"alpha")) {
getdouble(alpha,NPARAMS,conlabs);
}
else if (startsame(in,"gamma")) {
getdouble(ga,NLEVS,levlabs);
}
else if (startsame(in,"grace")) {
getint(&grace);
}
else if (startsame(in,"rest")) {
tval = rest[W];
getdouble(rest,NLEVS,levlabs);
if (tval != rest[W]) {
initialize();
}
}
else if (startsame(in,"fweight")) {
getdouble(fweight,NCONTINS,contname);
}
else if (startsame(in,"pthresh")) {
getdouble(pthresh,NLEVS,levlabs);
}
else if (startsame(in,"ngraph")) {
newgraph(pmin,ng_max,pstep);
}
else if (startsame(in,"ngmax")) {
getint(&ng_max);
}
else if (startsame(in,"ngwscale")) {
getval(&ng_wscale);
}
else if (startsame(in,"ngsscale")) {
getval(&ng_sscale);
}
else if (startsame(in,"ngpscale")) {
getval(&ng_pscale);
}
else if (startsame(in,"nreps")) {
getint(&nreps);
}
else if (startsame(in,"pfreq")) {
getint(&printfreq);
}
else if (startsame(in,"rarate")) {
getval(&rarate);
}
else if (startsame(in,"sumpr")) {
scr_sum(pmin,pmax,pstep);
}
else if (startsame(in,"sinspec")) {
sinspec();
}
else if (startsame(in,"sfeatures")) {
getstring("Filename: ");
sfeatures(in);
}
else if (startsame(in,"dinspec")) {
dinspec();
}
else if (startsame(in,"sumopt")) {
getint(&sumflag);
}
else if (startsame(in,"pmin")) {
getint(&pmin);
}
else if (startsame(in,"pmax")) {
getint(&pmax);
}
else if (startsame(in,"pstep")) {
getint(&pstep);
}
else if (startsame(in,"min")) {
getval(&min);
}
else if (startsame(in,"max")) {
getval(&max);
}
else if (startsame(in,"windowcent")) {
getval(&windowcent);
}
else if (startsame(in,"wbase")) {
getval(&wbase);
}
else if (startsame(in,"wgraph")) {
wgraph(pmin,ng_max,pstep);
}
else if (startsame(in,"wchange")) {
getval(&wchange);
}
else if (startsame(in,"wgain")) {
getval(&wgain);
}
else if (startsame(in,"wramp")) {
getval(&wramp);
}
else if (startsame(in,"imax")) {
getval(&imax);
}
else if (startsame(in,"sscale")) {
getval(&sscale);
}
else if (startsame(in,"nsscale")) {
getval(&nsscale);
}
else if (startsame(in,"freqscale")) {
tval = fscale;
getval(&fscale);
if (tval != fscale) {
initialize();
}
}
else if (startsame(in,"abort")) {
abort(); /* to get a core dump for sdb */
}
else {
errprint("Unrecognized request: For help type ?.");
if (infp != stdin) fileinput(STD);
}
wait(0);
}
}
int main(int argc, char **argv)
{
Q_INIT_RESOURCE(stickman);
QApplication app(argc, argv);
StickMan *stickMan = new StickMan;
stickMan->setDrawSticks(false);
#if defined(Q_WS_S60) || defined(Q_WS_MAEMO_5) || defined(Q_WS_SIMULATOR)
RectButton *buttonJump = new RectButton("Jump"); buttonJump->setPos(100, 125);
RectButton *buttonDance = new RectButton("Dance"); buttonDance->setPos(100, 200);
RectButton *buttonChill = new RectButton("Chill"); buttonChill->setPos(100, 275);
#else
QGraphicsTextItem *textItem = new QGraphicsTextItem();
textItem->setHtml("<font color=\"white\"><b>Stickman</b>"
"<p>"
"Tell the stickman what to do!"
"</p>"
"<p><i>"
"<li>Press <font color=\"purple\">J</font> to make the stickman jump.</li>"
"<li>Press <font color=\"purple\">D</font> to make the stickman dance.</li>"
"<li>Press <font color=\"purple\">C</font> to make him chill out.</li>"
"<li>When you are done, press <font color=\"purple\">Escape</font>.</li>"
"</i></p>"
"<p>If he is unlucky, the stickman will get struck by lightning, and never jump, dance or chill out again."
"</p></font>");
qreal w = textItem->boundingRect().width();
QRectF stickManBoundingRect = stickMan->mapToScene(stickMan->boundingRect()).boundingRect();
textItem->setPos(-w / 2.0, stickManBoundingRect.bottom() + 25.0);
#endif
QGraphicsScene scene;
scene.addItem(stickMan);
#if defined(Q_WS_S60) || defined(Q_WS_MAEMO_5) || defined(Q_WS_SIMULATOR)
scene.addItem(buttonJump);
scene.addItem(buttonDance);
scene.addItem(buttonChill);
#else
scene.addItem(textItem);
#endif
scene.setBackgroundBrush(Qt::black);
GraphicsView view;
view.setRenderHints(QPainter::Antialiasing);
view.setTransformationAnchor(QGraphicsView::NoAnchor);
view.setScene(&scene);
QRectF sceneRect = scene.sceneRect();
// making enough room in the scene for stickman to jump and die
view.resize(sceneRect.width() + 100, sceneRect.height() + 100);
view.setSceneRect(sceneRect);
#if defined(Q_WS_S60) || defined(Q_WS_MAEMO_5) || defined(Q_WS_SIMULATOR)
view.setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff);
view.setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff);
view.showMaximized();
view.fitInView(scene.sceneRect(), Qt::KeepAspectRatio);
#else
view.show();
view.setFocus();
#endif
LifeCycle cycle(stickMan, &view);
cycle.setDeathAnimation(":/animations/dead");
#if defined(Q_WS_S60) || defined(Q_WS_MAEMO_5) || defined(Q_WS_SIMULATOR)
cycle.addActivity(":/animations/jumping", Qt::Key_J, buttonJump, SIGNAL(clicked()));
cycle.addActivity(":/animations/dancing", Qt::Key_D, buttonDance, SIGNAL(clicked()));
cycle.addActivity(":/animations/chilling", Qt::Key_C, buttonChill, SIGNAL(clicked()));
#else
cycle.addActivity(":/animations/jumping", Qt::Key_J);
cycle.addActivity(":/animations/dancing", Qt::Key_D);
cycle.addActivity(":/animations/chilling", Qt::Key_C);
#endif
cycle.start();
return app.exec();
}
int RunGLTest (void)
{
#ifdef GL
int mm=1;
#else
int mm = 0;
#endif
int mode=0;
cam.x=0;
cam.y=0;
int bpp;
int w = 200;
int h = 200;
int done = 0;
int shrink=0;
int grow=0;
int gofullscreen=0;
int mustresize = 1;
int justresized = 0;
xy ss = parsemodes(w,h,"mode",1,0,0);
printf("wtf\n");
if (ss.x!=-1){w=ss.x;h=ss.y;};
SDL_Surface* s;
#ifdef GL
s=initsdl(w,h,&bpp,SDL_OPENGL
#else
gltextsdlsurface=s=initsdl(w,h,&bpp,
#endif
+0);printf("inito\n");
SDL_InitSubSystem( SDL_INIT_TIMER);
SDL_EnableUNICODE(1);
SDL_EnableKeyRepeat(SDL_DEFAULT_REPEAT_DELAY/2, SDL_DEFAULT_REPEAT_INTERVAL*2);
#ifdef GL
newtermmsg=GetFileIntoCharPointer1("newtermmsg");
printf("pretty far\n");
wm(w,h);
int down=0;
glEnable(GL_BLEND);
glShadeModel(GL_FLAT);
glBlendFunc(GL_SRC_ALPHA, GL_ONE);
glClearColor( 0.0, 0.0, 0.04, 0.0 );
glLineWidth(lv);
#else
gltextsdlsurface=s;
#endif
roteface *face1;
roteface *activeface;
face1=add_face();
activeface =face1 ;
face1->next=add_face();
printf("still?\n");
add_terminal(face1);
printf("2threaad\n");
loadl2();
struct state *nerv=0;
#ifdef nerve
nerv=nerverot_init(w,h);
#endif
int dirty=1;
printf("mainloop descent commencing\n");
while( !done )
{
lockterms(face1);
if(dirty||faces_dirty(face1))
{
dirty=0;
facesclean(face1);
#ifdef GL
glClear(GL_COLOR_BUFFER_BIT);
#else
SDL_FillRect ( s, NULL, 0 );
#endif
#ifndef GL
#else
if(nerv)
{
shownerv(nerv);
dirty=1;
}
glPushMatrix();
glScalef(sx,sy,0.004);
glTranslatef(cam.x,cam.y,0);
#endif
Uint8 * k;
int integer;
k=SDL_GetKeyState(&integer);
if(k[SDLK_RCTRL])
focusline(activeface);
int nf;
switch(mode)
{
case 0:
showfaces(face1);
break;
#ifdef GL
case 1:
krychlus(face1);
break;
#endif
}
#ifdef GL
glPopMatrix();
#endif
#ifndef GL
SDL_UpdateRect(s,0,0,0,0);
#else
SDL_GL_SwapBuffers( );
#endif
facesclean(face1);
}
#ifdef GL
GLenum gl_error;
gl_error = glGetError( );
if( gl_error != GL_NO_ERROR )
{
if(gl_error==GL_STACK_OVERFLOW)
printf("QUACK QUACK QUACK, OVERFLOVING STACK\n");
else if(gl_error==GL_INVALID_OPERATION)
printf("INVALID OPERATION, PATIENT EXPLODED\n");
else fprintf( stderr, "testgl: OpenGL error: %d\n", gl_error );
}
#endif
char* sdl_error;
sdl_error = SDL_GetError( );
if( sdl_error[0] != '\0' )
{
fprintf(stderr, "testgl: SDL error '%s'\n", sdl_error);
SDL_ClearError();
}
SDL_TimerID x=0;
if(dirty)
x= SDL_AddTimer(55, NewTimerCallback, 0);
unlockterms(face1);
// printf("---------unlocked wating\n");
SDL_Event event;
if(SDL_WaitEvent( &event ))
{
lockterms(face1);
// printf("---------locked goooin %i\n", event.type);
if(x)SDL_RemoveTimer(x);x=0;
do {
int mod=event.key.keysym.mod;
int key=event.key.keysym.sym;
Uint8 *keystate = SDL_GetKeyState(NULL);
switch( event.type )
{
#ifdef GL
case SDL_MOUSEMOTION:
if((SDL_BUTTON(1)|SDL_BUTTON(2))&SDL_GetMouseState(0,0))
{
activeface->x+=event.motion.xrel;
activeface->y+=event.motion.yrel;
}
if((SDL_BUTTON(3)|SDL_BUTTON(2))&SDL_GetMouseState(0,0))
{
cam.x-=event.motion.xrel;
cam.y-=event.motion.yrel;
}
break;
#endif
case SDL_KEYUP:
{
if ( (key == SDLK_RCTRL) )
{
dirty=1;
}
}
break;
case SDL_KEYDOWN:
if(mod&KMOD_RSHIFT&&(key==SDLK_PAGEUP||key==SDLK_PAGEDOWN))
{
if(key==SDLK_PAGEUP)
tscroll+=9;
if(key==SDLK_PAGEDOWN)
tscroll-=9;
if(tscroll<0)tscroll=0;
// printf("scroll:%i,logl:%i, log&%i, t:%i ,b:%i\n", tscroll,activeface->t->logl, activeface->t->log,activeface->t->scrolltop,activeface->t->scrollbottom);
}
else
if(key==SDLK_RCTRL||mod&KMOD_RCTRL)
{
dirty=1;
switch (key)
{
case SDLK_TAB:
cycle(face1, &activeface);
break;
case SDLK_F2:
gofullscreen=1;
break;
case SDLK_F3:
rastio+=0.05;
mustresize=1;
dirty=1;
break;
case SDLK_F4:
rastio-=0.05;
mustresize=1;
dirty=1;
break;
case SDLK_F5:
sx-=0.001;
sy+=0.001;
break;
case SDLK_F6:
sx+=0.001;
sy-=0.001;
break;
case SDLK_F7:
savemode(w,h);
break;
case SDLK_F8:
loadl2();
break;
case SDLK_p:
saveScreenshot();
break;
#ifdef GL
case SDLK_F9:
lv-=1; glLineWidth(lv);
break;
case SDLK_F10:
lv+=1; glLineWidth(lv);
break;
#endif
case SDLK_F11:
shrink=1;
break;
case SDLK_F12:
grow=1;
break;
case SDLK_PAGEUP:
mode++;
if(mode>mm)mode= mm;
break;
case SDLK_INSERT:
mode--;
if(mode<0)mode= 0;
break;
case SDLK_END:
resizooo(activeface, 0,1,keystate);
break;
case SDLK_HOME:
resizooo(activeface, 0,-1,keystate);
break;
case SDLK_DELETE:
resizooo(activeface, -1,0,keystate);
break;
case SDLK_PAGEDOWN:
resizooo(activeface, 1,0,keystate);
break;
#ifdef nerve
case SDLK_F1:
if(nerv)
{
nerverot_free(nerv);
dirty=1;
nerv=0;
}
else
{
nerv=nerverot_init(w,h);
dirty=1;
}
break;
#endif
}
}
else
{
if(activeface->t==0)
{
printf("debug messages r fun\n");
add_terminal(activeface);
activeface->next=add_face();
}
if ( (key >= SDLK_F1) && (key <= SDLK_F15) )
{
char *k;
if(asprintf(&k ,"kf%i", key-SDLK_F1+1)!=-1)
{
rote_vt_terminfo(activeface->t, k);
free(k);
}
}
else
if ( (key == SDLK_SPACE) )
keyp(activeface,32);
else
#define magic(x) rote_vt_terminfo(activeface->t, x )
if ( (key == SDLK_BACKSPACE) )
magic( "kbs");
else
if ( (key == SDLK_ESCAPE) )
keyp(activeface,27);
else
if ( (key == SDLK_LEFT) )
magic("kcub1");
else
if ( (key == SDLK_RIGHT) )
magic( "kcuf1");
else
if ( (key == SDLK_UP) )
magic( "kcuu1");
else
if ( (key == SDLK_DOWN) )
magic( "kcud1");
else
if ( (key == SDLK_END) )
magic( "kend");
else
if ( (key == SDLK_HOME) )
magic("khome");
else
if ( (key == SDLK_DELETE) )
magic( "kdch1");
else
if ( (key == SDLK_PAGEDOWN) )
magic( "knp");
else
if ( (key == SDLK_INSERT) )
magic( "kich1");
else
if ( (key == SDLK_PAGEUP) )
magic ( "kpp" );
else
if ( (key == SDLK_RETURN) )
keyp(activeface,10);
else
if( event.key.keysym.unicode && ( (event.key.keysym.unicode & 0xFF80) == 0 ) )
keyp(activeface, event.key.keysym.unicode);
}
break;
case SDL_QUIT:
done = 1;
break;
#ifndef GL
case SDL_MOUSEBUTTONDOWN:
rote_vt_mousedown(activeface->t,event.button.x/13, event.button.y/26);
break;
case SDL_MOUSEBUTTONUP:
rote_vt_mouseup (activeface->t,event.button.x/13, event.button.y/26);
break;
case SDL_MOUSEMOTION:
rote_vt_mousemove(activeface->t,event.button.x/13, event.button.y/26);
break;
#endif
case SDL_VIDEORESIZE:
{
w=event.resize.w;h=event.resize.h;
// printf("videoresize %i %i\n", w,h);
dirty=1;
if (s=SDL_SetVideoMode( w,h, bpp, s->flags ) )
// printf("hmm\n");
wm(w,h);
if(!justresized)
mustresize=1;
justresized=0;
}
break;
case SDL_USEREVENT:
if(event.user.code==1)
RemoveTerm(&activeface,&face1, event.user.data1);
break;
}
}
while (SDL_PollEvent(&event));
if (shrink||grow)
{
resize(&w,&h,&bpp,&s->flags,&shrink,&grow);
wm(w,h);
}
if (mustresize)
{
mustresize=0;
justresized=1;
// if(activeface->t->cols!=event.resize.w/13/rastio||
// activeface->t->rows!=event.resize.h/26/rastio)
//rote_vt_resize(activeface->t,event.resize.h/26/rastio ,event.resize.w/13/rastio);
}
if(gofullscreen)
if(s->flags & SDL_FULLSCREEN )
{
s=SDL_SetVideoMode( w,h, bpp, (s->flags & ~SDL_FULLSCREEN ));
}
else
s=SDL_SetVideoMode( w,h, bpp, (s->flags | SDL_FULLSCREEN ));
gofullscreen=0;
unlockterms(face1);
}
}
SDL_Quit( );
return(0);
}
/**
* Main function
* Navigate randomly using distance sensor data and Particle Filter-based
* localization.
*/
int main(int argc, char** argv)
{
std::string robot_name = "/robot_0";
// Open file to store robot poses
outfile.open("Output.txt");
// Write file header
outfile << "Estimated and real pose of the robot\n\n"
<< "[T]: Odometry [X Y Theta] Real [X Y Theta] Particle [X Y Theta]\n\n";
//
// Create map related variables
//
// We will create an image with the map wich contains the original map plus
// a border around it, so as to allow estimates outside of the original
// map.
std::string map_file_path(getenv("HOME"));
map_file_path += "/ros/worlds/stage/cave.png";
cv::Mat cave_map = cv::imread(map_file_path, CV_LOAD_IMAGE_COLOR);
if( cave_map.data == 0 )
{
std::cerr << "Error reading map!" << std::endl;
return -1;
}
// Read original map and resize it to our resolution
cv::resize(cave_map, map, map.size(), 0, 0, cv::INTER_AREA );
// Read original map and resize it to our resolution
// We need the original map, black and white, so that it is faster to find
// occupied cells
cv::resize(cv::imread(map_file_path, CV_LOAD_IMAGE_GRAYSCALE),
org_map,
org_map.size(),
0, 0, cv::INTER_AREA );
// We need a temporary map
tmp_map = map.clone();
// Create window for the map
cv::namedWindow("Debug", 0);
//
// Particle filter related variables with their initial values
//
// This variable will hold our final estimation at each iteration
geometry_msgs::Pose2D pose_estimate;
// Initialize particles with uniform random distribution across all space
cv::randu( particles_x,
-MAP_LENGTH/2.0+SAFETY_BORDER,
MAP_LENGTH/2.0-SAFETY_BORDER); // X
cv::randu( particles_y,
-MAP_LENGTH/2.0+SAFETY_BORDER,
MAP_LENGTH/2.0-SAFETY_BORDER); // Y
cv::randu( particles_theta, -CV_PI, CV_PI ); // Theta
// Re-add particles that are outside of the map or inside an obstacle, until
// it is in a free space inside the map.
for(int n = 0; n < NUM_PARTICLES; n++)
{
while( ( (particles_x(n,0) > MAP_LENGTH/2.0) ||
(particles_x(n,0) < -MAP_LENGTH/2.0) ||
(particles_y(n,0) > MAP_LENGTH/2.0) ||
(particles_y(n,0) < -MAP_LENGTH/2.0) ) ||
(org_map.at<uchar>(
cvRound(org_map.size().height/2.0
- particles_y(n,0)/MAP_RESOLUTION),
cvRound(org_map.size().width/2.0
+ particles_x(n,0)/MAP_RESOLUTION)) < 127) )
{
particles_x(n,0) = rng.uniform(-MAP_LENGTH/2.0+SAFETY_BORDER, MAP_LENGTH/2.0-SAFETY_BORDER);
particles_y(n,0) = rng.uniform(-MAP_LENGTH/2.0+SAFETY_BORDER, MAP_LENGTH/2.0-SAFETY_BORDER);
}
}
// Init ROS
ros::init(argc, argv, "tp7");
// ROS variables/objects
ros::NodeHandle nh; // Node handle
// Get parameters
ros::NodeHandle n_private("~");
n_private.param("min_front_dist", min_front_dist, 1.0);
n_private.param("stop_front_dist", stop_front_dist, 0.6);
std::cout << "Random navigation with obstacle avoidance and particle filter"
<< " based localization\n---------------------------" << std::endl;
/// Setup subscribers
// Odometry
ros::Subscriber sub_odom = nh.subscribe(robot_name + "/odom", 10, odomCallback);
// Real, error-free robot pose (for debug purposes only)
ros::Subscriber sub_real_pose = nh.subscribe(robot_name + "/base_pose_ground_truth", 1, realPoseCallback);
// Laser scans
ros::Subscriber sub_laser = nh.subscribe(robot_name + "/base_scan", 1, laserCallback);
// Markers detected
ros::Subscriber sub_markers = nh.subscribe(robot_name + "/markers", 1, markersCallback);
// Setup publisher for linear and angular velocity
vel_pub = nh.advertise<geometry_msgs::Twist>(robot_name + "/cmd_vel", 1);
ros::Rate cycle(10.0); // Cycle rate
// Wait until we get the robot pose (from odometry)
while( odom_updated == false )
{
ros::spinOnce();
cycle.sleep();
}
// Stop the robot (if not stopped already)
vel_cmd.angular.z = 0;
vel_cmd.linear.x = 0;
vel_pub.publish(vel_cmd);
// Infinite loop (will call the callbacks whenever information is available,
// until ros::shutdown() is called.
ros::spin();
// If we are quitting, stop the robot
vel_cmd.angular.z = 0;
vel_cmd.linear.x = 0;
vel_pub.publish(vel_cmd);
// Close file
outfile.close();
// Store final map
cv::imwrite("mapa.png", map);
return 1;
}
void multiGrid::setup(int order, input *params, solver &Solver)
{
this->order = order;
this->params = params;
pInputs.assign(order, *params);
pGrids.resize(order);
/* H-P Multigrid using Refinement Method */
if (params->HMG)
{
if (params->lowOrder != 0)
FatalError("H-Multigrid only supported for PMG lowOrder = 0.");
if (params->nDims == 3)
FatalError("H-Multigrid only supported for 2D currently.");
geo coarse_grid;
coarse_grid.setup(params,true);
hInputs.assign(params->n_h_levels, *params);
hGrids.resize(params->n_h_levels);
hGeos.resize(params->n_h_levels);
pGeos.resize(params->order);
for (int H = 0; H < params->n_h_levels; H++)
{
if (params->rank == 0) cout << endl << "H-Multigrid: Setting up H = " << H << endl;
hInputs[H].dataFileName += "_H" + std::to_string(H) + "_";
hGrids[H] = make_shared<solver>();
hGeos[H] = make_shared<geo>();
/* Refine the initial coarse grid to produce the fine grids */
setup_h_level(coarse_grid, *hGeos[H], params->n_h_levels - H - 1);
hGrids[H]->setup(&hInputs[H], params->lowOrder, &(*hGeos[H]));
hGrids[H]->initializeSolution(true);
}
/* Create final fine grid and re-setup the given solver */
if (params->rank == 0) cout << endl << "H-Multigrid: Setting up fine grid solver" << endl;
fine_grid = make_shared<geo>();
setup_h_level(coarse_grid, *fine_grid, params->n_h_levels);
Solver.setup(params, order, &(*fine_grid));
Solver.initializeSolution();
/* Instantiate P-grid solvers using finest mesh */
for (int P = 0; P < order; P++)
{
if (P < params->lowOrder)
{
pGrids[P] = NULL;
}
else
{
if (params->rank == 0) cout << endl << "P-Multigrid: Setting up P = " << P << endl;
pInputs[P].dataFileName += "_P" + std::to_string(P) + "_";
pGeos[P] = make_shared<geo>(*fine_grid);
pGrids[P] = make_shared<solver>();
pGeos[P]->setup_hmg(&pInputs[P], fine_grid->gridID, fine_grid->gridRank, fine_grid->nProcGrid, fine_grid->gridIdList);
pGrids[P]->setup(&pInputs[P], P, &(*pGeos[P]));
pGrids[P]->initializeSolution(true);
}
}
}
/* P-Multigrid Alone */
else {
/* Instantiate coarse grid solvers */
for (int P = 0; P < order; P++)
{
if (P < params->lowOrder)
{
pGrids[P] = NULL;
}
else
{
if (params->rank == 0) cout << endl << "P-Multigrid: Setting up P = " << P << endl;
pInputs[P].dataFileName += "_P" + std::to_string(P) + "_";
pGrids[P] = make_shared<solver>();
pGrids[P]->setup(&pInputs[P], P);
pGrids[P]->initializeSolution(true);
}
}
if (params->rank == 0) cout << endl << "P-Multigrid: Setting up P = " << params->order << endl;
Solver.setup(params, params->order);
Solver.initializeSolution();
}
/* Still some weird bug in initialization of PMG solvers; this is a
* workaround for the moment */
cycle(Solver);
Solver.initializeSolution();
}
void
sc_trace_file_base::simulation_phase_callback()
{
// delta cycle is traced at the end of an update phase
cycle( simcontext()->get_status() == SC_END_OF_UPDATE );
}
