void ULibraries::SetHome(FRotator h)
{
float q1, q2, q3, q4;
bool sent = FemtoduinoPointer->WriteData("w\n", 32);
char parse1[10], parse2[10], parse3[10], parse4[10];
char incomingData[250];
int dataLength = 250;
Sleep(50);
FemtoduinoPointer->ReadData(incomingData, dataLength);
_memccpy(&parse1, incomingData, ',', 8);
_memccpy(&parse2, &incomingData[9], ',', 8);
_memccpy(&parse3, &incomingData[18], ',', 8);
_memccpy(&parse4, &incomingData[27], ',', 8);
hexasciitofloat(q1, parse1);
hexasciitofloat(q2, parse2);
hexasciitofloat(q3, parse3);
hexasciitofloat(q4, parse4);
FQuat newQ = FQuat(-q2, -q3, -q4, q1);
hq = newQ;
}
/**
* Uma função para adicionar informação ao registo de histórico (AppendThreadLog(…));
* */
void WINAPI AppendThreadLog(LPTSTR message){
if (message == NULL) RaiseException(NULL_POINTER_EXPECTION,0,0,NULL);
LogDef* logger = (LogDef*)TlsGetValue(tlsIDX);
if (logger->_loggerBase == NULL)
CreateThreadLog(__DEFAULT_SIZE__);
DWORD size = _tcsclen(message);
//Usar controlo de excepções
//Usar memcopy para copiar a mensagem para o endereço de _loggerBaseFree
//Se falhar a excepção chamar a função que vai fazer commit da proxima pagina
//ou paginas suficientes para agregar as páginas
//Actualizar o valor de _loggerBaseFree para o proximo byte apos ter sido colocada a mensagem
if (isFull()) RaiseException(BUFFER_FULL,0,0,NULL);
__try
{
_memccpy((LPVOID)( (int)logger->_loggerBase + (logger->_endPtr)),message,0,size);
logger->_endPtr = (logger->_endPtr + size)%(logger->_pageSize * logger->_pageLimit);
}
__except ( PageFaultExceptionFilter( GetExceptionCode(),GetExceptionInformation() ) )
{
//one more page commited
}
}
static HGLOBAL
convert_to_handle_as_ascii (void)
{
HGLOBAL htext = NULL;
int nbytes;
int truelen;
unsigned char *src;
unsigned char *dst;
ONTRACE (fprintf (stderr, "convert_to_handle_as_ascii\n"));
nbytes = SBYTES (current_text) + 1;
src = SDATA (current_text);
/* We need to add to the size the number of LF chars where we have
to insert CR chars (the standard CF_TEXT clipboard format uses
CRLF line endings, while Emacs uses just LF internally). */
truelen = nbytes + current_num_nls;
if ((htext = GlobalAlloc (GMEM_MOVEABLE | GMEM_DDESHARE, truelen)) == NULL)
return NULL;
if ((dst = (unsigned char *) GlobalLock (htext)) == NULL)
{
GlobalFree (htext);
return NULL;
}
/* convert to CRLF line endings expected by clipboard */
while (1)
{
unsigned char *next;
/* copy next line or remaining bytes including '\0' */
next = _memccpy (dst, src, '\n', nbytes);
if (next)
{
/* copied one line ending with '\n' */
int copied = next - dst;
nbytes -= copied;
src += copied;
/* insert '\r' before '\n' */
next[-1] = '\r';
next[0] = '\n';
dst = next + 1;
}
else
/* copied remaining partial line -> now finished */
break;
}
GlobalUnlock (htext);
return htext;
}
FRotator ULibraries::GetQRotation()
{
float q1, q2, q3, q4;
bool sent = FemtoduinoPointer->WriteData("w\n", 32);
char parse1[10], parse2[10], parse3[10], parse4[10];
char incomingData[250];
int dataLength = 250;
//Sleep(50);
FemtoduinoPointer->ReadData(incomingData, dataLength);
_memccpy(&parse1, incomingData,',' ,8);
_memccpy(&parse2, &incomingData[9],',', 8);
_memccpy(&parse3, &incomingData[18], ',', 8);
_memccpy(&parse4, &incomingData[27], ',', 8);
hexasciitofloat(q1, parse1);
hexasciitofloat(q2, parse2);
hexasciitofloat(q3, parse3);
hexasciitofloat(q4, parse4);
FQuat newQ = FQuat(q2,q3,q4,q1);
if (!hq.Equals(FQuat::Identity))
{
return (hq*newQ).Rotator();
}
//newQ.Normalize(1.1);
//FQuat hq = FQuat::Identity;
//FQuat next = newQ*hq; //FQuat::Slerp(newQ,current.Quaternion(),.5);
//FRotator test = next.Rotator();
return newQ.Rotator();
}
/*
* read a line of text
*
* return -1 on error or bytecount
*/
static int readline(char *buf,int max,netbuf *ctl)
{
int x,retval = 0;
char *end,*bp=buf;
int eof = 0;
if ((ctl->dir != FTPLIB_CONTROL) && (ctl->dir != FTPLIB_READ))
return -1;
if (max == 0)
return 0;
do
{
if (ctl->cavail > 0)
{
x = (max >= ctl->cavail) ? ctl->cavail : max-1;
end = _memccpy(bp,ctl->cget,'\n',x);
if (end != NULL)
x = end - bp;
retval += x;
bp += x;
*bp = '\0';
max -= x;
ctl->cget += x;
ctl->cavail -= x;
if (end != NULL)
{
bp -= 2;
if (strcmp(bp,"\r\n") == 0)
{
*bp++ = '\n';
*bp++ = '\0';
--retval;
}
break;
}
}
if (max == 1)
{
*buf = '\0';
break;
}
if (ctl->cput == ctl->cget)
{
ctl->cput = ctl->cget = ctl->buf;
ctl->cavail = 0;
ctl->cleft = FTPLIB_BUFSIZ;
}
if (eof)
{
if (retval == 0)
retval = -1;
break;
}
if (!socket_wait(ctl))
return retval;
if ((x = net_read(ctl->handle,ctl->cput,ctl->cleft)) == -1)
{
perror("read");
retval = -1;
break;
}
if (x == 0)
eof = 1;
ctl->cleft -= x;
ctl->cavail += x;
ctl->cput += x;
}
while (1);
return retval;
}
void addKnights(int currentAttack, int currentKnight, int currentCoverage, int counter)
{
solution sol;
FILE* outFile[MAX_GRID * MAX_GRID];
FILE* inFile = NULL;
int knightsAttacks[10];
int knightsAttacksCount = 0;
memset(outFile, 0, sizeof(outFile));
memset(placements, 0, sizeof(placements));
initialiseXYs();
int y = knightsYX[currentAttack][0];
int x = knightsYX[currentAttack][1];
knightsAttacks[knightsAttacksCount] = currentAttack;
knightsAttacksCount++;
if ((y >= STEP_X) && (x >= STEP_Y))
{
knightsAttacks[knightsAttacksCount] = knights[y - STEP_X][x - STEP_Y];
knightsAttacksCount++;
}
if (y >= STEP_X) {
knightsAttacks[knightsAttacksCount] = knights[y - STEP_X][x + STEP_Y];
knightsAttacksCount++;
}
if (x >= STEP_Y) {
knightsAttacks[knightsAttacksCount] = knights[y + STEP_X][x - STEP_Y];
knightsAttacksCount++;
}
knightsAttacks[knightsAttacksCount] = knights[y + STEP_X][x + STEP_Y];
knightsAttacksCount++;
if ((y >= STEP_Y) && (x >= STEP_X))
{
knightsAttacks[knightsAttacksCount] = knights[y - STEP_Y][x - STEP_X];
knightsAttacksCount++;
}
if (y >= STEP_Y)
{
knightsAttacks[knightsAttacksCount] = knights[y - STEP_Y][x + STEP_X];
knightsAttacksCount++;
}
if (x >= STEP_X)
{
knightsAttacks[knightsAttacksCount] = knights[y + STEP_Y][x - STEP_X];
knightsAttacksCount++;
}
knightsAttacks[knightsAttacksCount] = knights[y + STEP_Y][x + STEP_X];
knightsAttacksCount++;
size_t loop = 1;
if (currentAttack == 0)
{
memset(&sol, 0, sizeof(solution));
}
else
{
inFile = mkSolDir(currentAttack, currentCoverage, currentKnight, tidiedFile, true, false, counter);
loop = readSolution(&sol, inFile);
}
while (loop)
{
solution newSol;
for (int i = 0; i < knightsAttacksCount; i++)
{
int coverage = 0;
_memccpy(&newSol, &sol, 1, sizeof(solution));
addKnight(&newSol, knightsAttacks[i], &coverage);
if (outFile[coverage] == NULL)
{
outFile[coverage] = mkSolDir(currentAttack, coverage, currentKnight + 1, solutionFile, false, false, 0);
}
writeSolution(&newSol, outFile[coverage]);
}
if (inFile)
{
loop = readSolution(&sol, inFile);
}
else
{
loop = 0;
}
}
for (int i = 0; i < MAX_GRID * MAX_GRID; i++)
{
if (outFile[i] != NULL)
{
fclose(outFile[i]);
}
}
}