/***************************************************************
* void vprintf_tests
*
*
****************************************************************/
void vprintf_tests() {
set_col(2); set_row(4);
vprintf_custom("Esto es una prueba: %x %x %x %x", 1000, 10000, 1000000, 99999999);
set_col(2); set_row(5);
vprintf_custom("Esto es una prueba: %d %d %d %d", 1000, 10000, 1000000, 99999999);
set_col(2); set_row(6);
vprintf_custom("Esto es una prueba: %x %d %x %d", 1000, 10000, 1000000, 999999);
set_col(2); set_row(7);
vprintf_custom("Esto es una prueba: %x %d %s %x", 1000, 10000, "Hello!!", 99999999);
set_col(2); set_row(10);
vprintf_custom("Esto es una prueba: %s %s", "dos strings muuuuuuuuuuuy largooooooos", "012345678901234567890123456789001234567890123456789");
print_vscreen();
}
void free_command(char* params) {
char option[20];
bool leave = false;
unsigned int address = 0;
unsigned int i = 0;
unsigned int len = 0;
void* result = 0;
len = strlen_custom(params);
memset_custom(option, sizeof(option), 0);
sscanf_custom(params, "%s", option);
if( strcmp(option, "info") == 0 ) {
newline();
set_col(2);
vprintf_custom("Usa el comando \'free\' seguido de una direccion"
" de memoria en base hexadecimal (es decir el numero precedido"
" por 0x o 0X), para liberar la porcion de memoria que empieza"
" en ese lugar.");
} else {
for(i = 0; i < len; i++) {
if( ishex(option[i] == false) ) {
leave = true;
break;
}
}
//compruebo que el numero esté en hexa
if( leave == false
&& option[0] == '0'
&& (option[1] == 'x' || option[1] == 'X')) {
sscanf_custom(option, "%x", &address);
result = (void*)free((unsigned int*)address);
if( result == (void*)address && (unsigned int)result > 0xF) {
newline(); set_col(2);
vprintf_custom("La memoria con direccion %x fue liberada.", address);
} else {
newline(); set_col(2);
vprintf_custom("No existe ningun segmento de memoria alocado"
"en esa direccion.");
}
} else {
newline(); set_col(2);
vprintf_custom("Debes ingresar un direccion de memoria en "
"hexa para liberar esa porcion de memoria.");
}
}
}
// Matrix-Vector algorithm for multiplying matrices
void mat_vec(float* A, float* B, int m, int p, int n, float* C)
{
float* x = (float*) calloc(p,sizeof(float));
float* y = (float*) calloc(m,sizeof(float));
// If memory allocation fails then exit with error
if(x == NULL || y == NULL) { exit(1); }
int j;
// Loop through columns of B
for(j=0; j < n; j++)
{
// Get column j of B and load into x
get_col(B,p,n,j,x);
// Get column j of C and load into y
get_col(C,m,n,j,y);
// Saxpy level 2 operation y <- alpha*A*x + y
cblas_sgemv(CblasRowMajor,CblasNoTrans,m,p,1.0,A,p,x,1,1.0,y,1);
// Set column j of C to y
set_col(C,m,n,j,y);
}
// Free intermediate values
free(x);
free(y);
}
void raytrace(t_env *env)
{
int x;
int y;
t_ray ray;
t_col col;
y = 0;
while (y <= WIN_Y)
{
x = 0;
while (x <= WIN_X)
{
ray.start = OBJ.cam_s;
ray.start.x += x;
ray.start.y += y;
ray.dir = OBJ.cam_dir;
vector_norm(&ray.dir);
set_col(&OBJ.col, 0, 0, 0);
env->br = 0;
col = shoot_ray(ray, 10, env);
save_to_img(env, col, x, y);
x++;
}
y++;
}
}
mat get_cols(int start_col, int end_col){
assert(start_col < end_offset - start_offset);
assert(start_offset + end_col <= end_offset);
mat retmat = zeros(end-start, end_col - start_col);
for (int i=start_col; i< end_col; i++)
set_col(retmat, i-start_col, this->operator[](i-start_col).to_vec());
return retmat;
}
/***************************************************************
* int malloc_custom
*
****************************************************************/
void init_malloc_data() {
if(system_memory_size == 0) {
vprintf_custom("System memory size is 0.");
print_vscreen();
while(1) {}
}
bool show_data = true;
unsigned int i = 0;
unsigned int table_size = 0;
unsigned int check = 0;
memset_custom((char*)info, sizeof(info), 0);
// Aloco el 10% de mi memoria total para mi
// registro de headers de memoria.
table_size = (unsigned int)(system_memory_size * 0.1);
malloc_registry = (memory_segment*)SAFE_ADDRESS;
user_space_start = (byte*)(SAFE_ADDRESS + table_size + sizeof(memory_segment));
user_memory_size = (unsigned int)(system_memory_size - (unsigned int)user_space_start);
segments_amount = (unsigned int)(table_size / sizeof(memory_segment));
minimum_segment_size = (unsigned int)(user_memory_size / segments_amount);
memset_custom((char*)malloc_registry, table_size, 0);
// Imprime las características del mapa de memoria.
if(show_data == true) {
clear_vscreen();
set_row(3); set_col(0);
vprintf_custom("Memory Table Size: %x", table_size);
set_row(4); set_col(0);
vprintf_custom("Malloc Registry Start: %x", malloc_registry);
set_row(5); set_col(0);
vprintf_custom("Total Memory Size: %x", system_memory_size);
set_row(6); set_col(0);
vprintf_custom("User Space Start: %x", user_space_start);
set_row(7); set_col(0);
vprintf_custom("User Memory Size: %x", user_memory_size);
set_row(8); set_col(0);
vprintf_custom("Number of Table Segments: %d", segments_amount);
set_row(9); set_col(0);
vprintf_custom("Minimum Allocable Space: %d(bytes)", minimum_segment_size);
set_row(10); set_col(0);
vprintf_custom("Memory Header Size: %x", sizeof(memory_segment));
print_vscreen();
}
q_to_continue(SCREEN_LENGTH - 1, 2, 1);
}
MAT *m_inverse(const MAT *A, MAT *out)
{
unsigned int i;
char MatrixTempBuffer[ 4000 ];
VEC *tmp = VNULL, *tmp2 = VNULL;
MAT *A_cp = MNULL;
PERM *pivot = PNULL;
if ( ! A )
error(E_NULL,"m_inverse");
if ( A->m != A->n )
error(E_SQUARE,"m_inverse");
if ( ! out || out->m < A->m || out->n < A->n )
out = m_resize(out,A->m,A->n);
if( SET_MAT_SIZE( A->m, A->n ) < 1000 )
mat_get( &A_cp, (void *)( MatrixTempBuffer + 2000 ), A->m, A->n );
else
A_cp = matrix_get( A->m, A->n );
A_cp = m_copy( A, A_cp );
if( SET_VEC_SIZE( A->m ) < 1000 ) {
vec_get( &tmp, (void *)MatrixTempBuffer, A->m );
vec_get( &tmp2, (void *)(MatrixTempBuffer + 1000), A->m );
} else {
tmp = v_get( A->m );
tmp2 = v_get( A->m );
}
if( SET_PERM_SIZE( A->m ) < 1000 ) {
perm_get( &pivot, (void *)( MatrixTempBuffer + 3000 ), A->m );
} else {
pivot = px_get( A->m );
}
LUfactor(A_cp,pivot);
//tracecatch_matrix(LUfactor(A_cp,pivot),"m_inverse");
for ( i = 0; i < A->n; i++ ){
v_zero(tmp);
tmp->ve[i] = 1.0;
LUsolve(A_cp,pivot,tmp,tmp2);
//tracecatch_matrix(LUsolve(A_cp,pivot,tmp,tmp2),"m_inverse");
set_col(out,i,tmp2);
}
if( tmp != (VEC *)(MatrixTempBuffer ) ) // память выделялась, надо освободить
V_FREE(tmp);
if( tmp2 != (VEC *)(MatrixTempBuffer + 1000) ) // память выделялась, надо освободить
V_FREE(tmp2);
if( A_cp != (MAT *)(MatrixTempBuffer + 2000) ) // память выделялась, надо освободить
M_FREE(A_cp);
if( pivot != (PERM *)(MatrixTempBuffer + 3000) ) // память выделялась, надо освободить
PX_FREE( pivot );
return out;
}
void col_gradient_green(void)
{
int x;
for (x=0; x<WIDTH; x++)
{
set_col(x,0, 0,(int)(255.0*(float)x/(float)WIDTH));
//set_col(x, 0,(int)(255.0*(float)x/(float)WIDTH),0);
//set_col(x, (int)(255.0*(float)x/(float)WIDTH),0,0);
}
send();
}
MAT *makeQ(const MAT *QR,const VEC *diag, MAT *Qout)
#endif
{
STATIC VEC *tmp1=VNULL,*tmp2=VNULL;
unsigned int i, limit;
Real beta, r_ii, tmp_val;
int j;
limit = min(QR->m,QR->n);
if ( ! QR || ! diag )
error(E_NULL,"makeQ");
if ( diag->dim < limit )
error(E_SIZES,"makeQ");
if ( Qout==(MAT *)NULL || Qout->m < QR->m || Qout->n < QR->m )
Qout = m_get(QR->m,QR->m);
tmp1 = v_resize(tmp1,QR->m); /* contains basis vec & columns of Q */
tmp2 = v_resize(tmp2,QR->m); /* contains H/holder vectors */
MEM_STAT_REG(tmp1,TYPE_VEC);
MEM_STAT_REG(tmp2,TYPE_VEC);
for ( i=0; i<QR->m ; i++ )
{ /* get i-th column of Q */
/* set up tmp1 as i-th basis vector */
for ( j=0; j<QR->m ; j++ )
tmp1->ve[j] = 0.0;
tmp1->ve[i] = 1.0;
/* apply H/h transforms in reverse order */
for ( j=limit-1; j>=0; j-- )
{
get_col(QR,j,tmp2);
r_ii = fabs(tmp2->ve[j]);
tmp2->ve[j] = diag->ve[j];
tmp_val = (r_ii*fabs(diag->ve[j]));
beta = ( tmp_val == 0.0 ) ? 0.0 : 1.0/tmp_val;
/* hhtrvec(tmp2,beta->ve[j],j,tmp1,tmp1); */
hhtrvec(tmp2,beta,j,tmp1,tmp1);
}
/* insert into Q */
set_col(Qout,i,tmp1);
}
#ifdef THREADSAFE
V_FREE(tmp1); V_FREE(tmp2);
#endif
return (Qout);
}
static MAT *calc_VinvIminAw(MAT *Vw, MAT *X, MAT *VinvIminAw, int calc_Aw) {
/*
* calculate V_w^-1(I-A_w) (==VinvIminAw),
* A = X(X'X)^-1 X' (AY = XBeta; Beta = (X'X)^-1 X'Y)
*
* on second thought (Nov 1998 -- more than 4 years later :-))
* calc (I-Aw) only once and keep this constant during iteration.
*/
MAT *tmp = MNULL, *V = MNULL;
VEC *b = VNULL, *rhs = VNULL;
int i, j;
if (X->m != Vw->n || VinvIminAw->m != X->m)
ErrMsg(ER_IMPOSVAL, "calc_VinvIminAw: sizes don't match");
if (calc_Aw) {
IminAw = m_resize(IminAw, X->m, X->m);
tmp = m_resize(tmp, X->n, X->n);
tmp = mtrm_mlt(X, X, tmp); /* X'X */
m_inverse(tmp, tmp); /* (X'X)-1 */
/* X(X'X)-1 -> X(X'X)-1 X') */
IminAw = XVXt_mlt(X, tmp, IminAw);
for (i = 0; i < IminAw->m; i++) /* I - Aw */
for (j = 0; j <= i; j++)
if (i == j)
IminAw->me[i][j] = 1.0 - IminAw->me[i][j];
else
IminAw->me[i][j] = IminAw->me[j][i] = -IminAw->me[i][j];
}
V = m_copy(Vw, V);
LDLfactor(V);
rhs = v_resize(rhs, X->m);
b = v_resize(b, X->m);
for (i = 0; i < X->m; i++) { /* solve Vw X = (I-A) for X -> V-1(I-A) */
rhs = get_col(IminAw, i, rhs);
LDLsolve(V, rhs, b);
set_col(VinvIminAw, i, b);
}
v_free(rhs);
v_free(b);
m_free(V);
if (tmp)
m_free(tmp);
return VinvIminAw;
}
MAT *makeHQ(MAT *H, VEC *diag, VEC *beta, MAT *Qout)
#endif
{
int i, j, limit;
STATIC VEC *tmp1 = VNULL, *tmp2 = VNULL;
if ( H==(MAT *)NULL || diag==(VEC *)NULL || beta==(VEC *)NULL )
error(E_NULL,"makeHQ");
limit = H->m - 1;
if ( diag->dim < limit || beta->dim < limit )
error(E_SIZES,"makeHQ");
if ( H->m != H->n )
error(E_SQUARE,"makeHQ");
Qout = m_resize(Qout,H->m,H->m);
tmp1 = v_resize(tmp1,H->m);
tmp2 = v_resize(tmp2,H->m);
MEM_STAT_REG(tmp1,TYPE_VEC);
MEM_STAT_REG(tmp2,TYPE_VEC);
for ( i = 0; i < H->m; i++ )
{
/* tmp1 = i'th basis vector */
for ( j = 0; j < H->m; j++ )
/* tmp1->ve[j] = 0.0; */
v_set_val(tmp1,j,0.0);
/* tmp1->ve[i] = 1.0; */
v_set_val(tmp1,i,1.0);
/* apply H/h transforms in reverse order */
for ( j = limit-1; j >= 0; j-- )
{
get_col(H,(unsigned int)j,tmp2);
/* tmp2->ve[j+1] = diag->ve[j]; */
v_set_val(tmp2,j+1,v_entry(diag,j));
hhtrvec(tmp2,beta->ve[j],j+1,tmp1,tmp1);
}
/* insert into Qout */
set_col(Qout,(unsigned int)i,tmp1);
}
#ifdef THREADSAFE
V_FREE(tmp1); V_FREE(tmp2);
#endif
return (Qout);
}
int main(int argc, char **argv){
char b[SIZE];
int **board;
FILE *f;
board = calloc(SIZE, sizeof(int));
int i;
for(i=0; i<SIZE; i++){
if (board == NULL) {
printf("Memory allocation failed\n");
exit(-1);
}
board[i] = calloc(SIZE, sizeof(int));
}
f = fopen(argv[1], "r");
while ( (fgets(b, SIZE , f) != NULL) ){
char *comm = strtok(b, " ");
int roc = 0;
int val = 0;
if( (strspn(comm, sc) == strlen(sc))){
roc = atoi(strtok(NULL, " "));
val = atoi(strtok(NULL, " "));
set_col(board, roc, val);
}else if( (strspn(comm, sr)) == strlen(sr) ){
roc = atoi(strtok(NULL, " "));
val = atoi(strtok(NULL, " "));
set_row(board, roc, val);
}else if( (strspn(comm, qc)) == strlen(qc) ){
roc = atoi(strtok(NULL, " "));
printf("%d\n", query_col(board, roc));
}else if( (strspn(comm, qr)) == strlen(qr) ){
roc = atoi(strtok(NULL, " "));
printf("%d\n", query_row(board, roc));
}
}
for(i=0; i<SIZE; i++){
free(board[i]);
}
free(board);
fclose(f);
}
MAT *m_inverse(const MAT *A, MAT *out)
#endif
{
int i;
STATIC VEC *tmp = VNULL, *tmp2 = VNULL;
STATIC MAT *A_cp = MNULL;
STATIC PERM *pivot = PNULL;
if ( ! A )
error(E_NULL,"m_inverse");
if ( A->m != A->n )
error(E_SQUARE,"m_inverse");
if ( ! out || out->m < A->m || out->n < A->n )
out = m_resize(out,A->m,A->n);
A_cp = m_resize(A_cp,A->m,A->n);
A_cp = m_copy(A,A_cp);
tmp = v_resize(tmp,A->m);
tmp2 = v_resize(tmp2,A->m);
pivot = px_resize(pivot,A->m);
MEM_STAT_REG(A_cp,TYPE_MAT);
MEM_STAT_REG(tmp, TYPE_VEC);
MEM_STAT_REG(tmp2,TYPE_VEC);
MEM_STAT_REG(pivot,TYPE_PERM);
tracecatch(LUfactor(A_cp,pivot),"m_inverse");
for ( i = 0; i < A->n; i++ )
{
v_zero(tmp);
tmp->ve[i] = 1.0;
tracecatch(LUsolve(A_cp,pivot,tmp,tmp2),"m_inverse");
set_col(out,i,tmp2);
}
#ifdef THREADSAFE
V_FREE(tmp); V_FREE(tmp2);
M_FREE(A_cp); PX_FREE(pivot);
#endif
return out;
}
int call_tabla(void *nombre, int argc, char **argv, char **azColName)
{
char tmp[255];
char *formato="<L></B/5>%s";
char *coltitle[10], *rowtitle[10], *mesg[10];
int colwidth[10], colvalue[10];
int i=0;
//char *titulo="<L></B/5>%s";
char titulo[255];
//sprintf(&titulo,nombre);
CDKSCREEN *pantalla;
initscr();
pantalla=initCDKScreen(stdscr);
CDKMATRIX *matriz;
sprintf(titulo,formato,nombre);
for(i=0;i<argc;i++)
{
sprintf(tmp,formato,azColName[i]);
rowtitle[i+1]=copyChar(tmp);
printf(tmp);
// set_row(i+1,azColName[i]);
}
set_col(1,57,titulo);
matriz=newCDKMatrix(
pantalla,
CENTER,CENTER,
argc,1,argc,1,
"",rowtitle,coltitle,colwidth,colvalue,-1,-1,'.',COL,TRUE,FALSE,FALSE
);
activateCDKMatrix(matriz,0);
/* for(i=0;i<argc;i++)
{
printf("->%s: %s\n",azColName[i],argv[i]);
}
printf("%s",(char*)nombre);*/
return 0;
}
void ClearScreen(){
unsigned char page;
u16 i,j;
page=0xB0;
for(i=0;i<1024;i+=128) {
P5OUT &= ~AA0;
P4OUT=page; //page number Setting
P5OUT &= ~AWRN;
P5OUT |= AWRN;
set_col(); // setup low/high column
P5OUT |= AA0;
for(j=0;j<128;j++){
//page_data=pic[pic_index][j+i*128];
//page_data=DC[j+(i<<7)];
P4OUT=0x00;
//P4OUT=0X03;
P5OUT &= ~AWRN;
P5OUT |= AWRN;
}
page++;
}
}
// Saxpy operations for multiplying matrices
void mat_saxpy(float* A, float* B, int m, int p, int n, float* C)
{
float* x = (float*) calloc(m,sizeof(float));
float* y = (float*) calloc(m,sizeof(float));
// If memory allocation fails then exit with error
if(x == NULL || y == NULL) { exit(1); }
int j,k;
// Loop through columns of B
for(j=0; j < n; j++)
{
// Loop through columns of A
for(k=0; k < p; k++)
{
// Get column k of A and load into x
get_col(A,m,p,k,x);
// Get column j of C and load into y
get_col(C,m,n,j,y);
// Get alpha
float alpha = B[k*n+j];
// Saxpy level 1 operation y <- alpha*x + y
cblas_saxpy(m,alpha,x,1,y,1);
// Set column j of C to y
set_col(C,m,n,j,y);
}
}
// Free intermediate values
free(x);
free(y);
}
void ShowProgress(u8 progress){
unsigned char page;
u16 j;
//ClearScreen();
page=0xB0;
P5OUT &= ~AA0;
P4OUT=page; //page number Setting
P5OUT &= ~AWRN;
P5OUT |= AWRN;
set_col(); // setup low/high column
P5OUT |= AA0;
for(j=0;j<128;j++){
//page_data=pic[pic_index][j+i*128];
//page_data=DC[j+(i<<7)];
if (j<=progress){
P4OUT=0xFF;
}else{
P4OUT=0x00;
};
//P4OUT=0X03;
P5OUT &= ~AWRN;
P5OUT |= AWRN;
}
}
void get_col(t_env *env, t_col *col)
{
if (env->info.wallside == 0)
{
if (env->wall_type == 0)
if (env->info.step.x < 0)
set_col(col, 51, 0, 102);
else
set_col(col, 0, 0, 102);
else
set_col(col, 255, 215, 0);
}
else if (env->info.wallside == 1)
{
if (env->wall_type == 0)
if (env->info.step.y < 0)
set_col(col, 0, 51, 0);
else
set_col(col, 192, 192, 192);
else
set_col(col, 255, 215, 0);
}
}
VEC *iter_gmres(ITER *ip)
#endif
{
STATIC VEC *u=VNULL, *r=VNULL, *rhs = VNULL;
STATIC VEC *givs=VNULL, *givc=VNULL, *z = VNULL;
STATIC MAT *Q = MNULL, *R = MNULL;
VEC *rr, v, v1; /* additional pointers (not real vectors) */
int i,j, done;
Real nres;
/* Real last_h; */
if (ip == INULL)
error(E_NULL,"iter_gmres");
if ( ! ip->Ax || ! ip->b )
error(E_NULL,"iter_gmres");
if ( ! ip->stop_crit )
error(E_NULL,"iter_gmres");
if ( ip->k <= 0 )
error(E_BOUNDS,"iter_gmres");
if (ip->x != VNULL && ip->x->dim != ip->b->dim)
error(E_SIZES,"iter_gmres");
if (ip->eps <= 0.0) ip->eps = MACHEPS;
r = v_resize(r,ip->k+1);
u = v_resize(u,ip->b->dim);
rhs = v_resize(rhs,ip->k+1);
givs = v_resize(givs,ip->k); /* Givens rotations */
givc = v_resize(givc,ip->k);
MEM_STAT_REG(r,TYPE_VEC);
MEM_STAT_REG(u,TYPE_VEC);
MEM_STAT_REG(rhs,TYPE_VEC);
MEM_STAT_REG(givs,TYPE_VEC);
MEM_STAT_REG(givc,TYPE_VEC);
R = m_resize(R,ip->k+1,ip->k);
Q = m_resize(Q,ip->k,ip->b->dim);
MEM_STAT_REG(R,TYPE_MAT);
MEM_STAT_REG(Q,TYPE_MAT);
if (ip->x == VNULL) { /* ip->x == 0 */
ip->x = v_get(ip->b->dim);
ip->shared_x = FALSE;
}
v.dim = v.max_dim = ip->b->dim; /* v and v1 are pointers to rows */
v1.dim = v1.max_dim = ip->b->dim; /* of matrix Q */
if (ip->Bx != (Fun_Ax)NULL) { /* if precondition is defined */
z = v_resize(z,ip->b->dim);
MEM_STAT_REG(z,TYPE_VEC);
}
done = FALSE;
for (ip->steps = 0; ip->steps < ip->limit; ) {
/* restart */
ip->Ax(ip->A_par,ip->x,u); /* u = A*x */
v_sub(ip->b,u,u); /* u = b - A*x */
rr = u; /* rr is a pointer only */
if (ip->Bx) {
(ip->Bx)(ip->B_par,u,z); /* tmp = B*(b-A*x) */
rr = z;
}
nres = v_norm2(rr);
if (ip->steps == 0) {
if (ip->info) ip->info(ip,nres,VNULL,VNULL);
ip->init_res = nres;
}
if ( nres == 0.0 ) {
done = TRUE;
break;
}
v.ve = Q->me[0];
sv_mlt(1.0/nres,rr,&v);
v_zero(r);
v_zero(rhs);
rhs->ve[0] = nres;
for ( i = 0; i < ip->k && ip->steps < ip->limit; i++ ) {
ip->steps++;
v.ve = Q->me[i];
(ip->Ax)(ip->A_par,&v,u);
rr = u;
if (ip->Bx) {
(ip->Bx)(ip->B_par,u,z);
rr = z;
}
if (i < ip->k - 1) {
v1.ve = Q->me[i+1];
v_copy(rr,&v1);
for (j = 0; j <= i; j++) {
v.ve = Q->me[j];
/* r->ve[j] = in_prod(&v,rr); */
/* modified Gram-Schmidt algorithm */
r->ve[j] = in_prod(&v,&v1);
v_mltadd(&v1,&v,-r->ve[j],&v1);
}
r->ve[i+1] = nres = v_norm2(&v1);
if (nres <= MACHEPS*ip->init_res) {
for (j = 0; j < i; j++)
rot_vec(r,j,j+1,givc->ve[j],givs->ve[j],r);
set_col(R,i,r);
done = TRUE;
break;
}
sv_mlt(1.0/nres,&v1,&v1);
}
else { /* i == ip->k - 1 */
/* Q->me[ip->k] need not be computed */
for (j = 0; j <= i; j++) {
v.ve = Q->me[j];
r->ve[j] = in_prod(&v,rr);
}
nres = in_prod(rr,rr) - in_prod(r,r);
if (sqrt(fabs(nres)) <= MACHEPS*ip->init_res) {
for (j = 0; j < i; j++)
rot_vec(r,j,j+1,givc->ve[j],givs->ve[j],r);
set_col(R,i,r);
done = TRUE;
break;
}
if (nres < 0.0) { /* do restart */
i--;
ip->steps--;
break;
}
r->ve[i+1] = sqrt(nres);
}
/* QR update */
/* last_h = r->ve[i+1]; */ /* for test only */
for (j = 0; j < i; j++)
rot_vec(r,j,j+1,givc->ve[j],givs->ve[j],r);
givens(r->ve[i],r->ve[i+1],&givc->ve[i],&givs->ve[i]);
rot_vec(r,i,i+1,givc->ve[i],givs->ve[i],r);
rot_vec(rhs,i,i+1,givc->ve[i],givs->ve[i],rhs);
set_col(R,i,r);
nres = fabs((double) rhs->ve[i+1]);
if (ip->info) ip->info(ip,nres,VNULL,VNULL);
if ( ip->stop_crit(ip,nres,VNULL,VNULL) ) {
done = TRUE;
break;
}
}
/* use ixi submatrix of R */
if (i >= ip->k) i = ip->k - 1;
R = m_resize(R,i+1,i+1);
rhs = v_resize(rhs,i+1);
/* test only */
/* test_gmres(ip,i,Q,R,givc,givs,last_h); */
Usolve(R,rhs,rhs,0.0); /* solve a system: R*x = rhs */
/* new approximation */
for (j = 0; j <= i; j++) {
v.ve = Q->me[j];
v_mltadd(ip->x,&v,rhs->ve[j],ip->x);
}
if (done) break;
/* back to old dimensions */
rhs = v_resize(rhs,ip->k+1);
R = m_resize(R,ip->k+1,ip->k);
}
#ifdef THREADSAFE
V_FREE(u);
V_FREE(r);
V_FREE(rhs);
V_FREE(givs);
V_FREE(givc);
V_FREE(z);
M_FREE(Q);
M_FREE(R);
#endif
return ip->x;
}
MAT *iter_arnoldi(ITER *ip, Real *h_rem, MAT *Q, MAT *H)
#endif
{
STATIC VEC *u=VNULL, *r=VNULL;
VEC v; /* auxiliary vector */
int i,j;
Real h_val, c;
if (ip == INULL)
error(E_NULL,"iter_arnoldi");
if ( ! ip->Ax || ! Q || ! ip->x )
error(E_NULL,"iter_arnoldi");
if ( ip->k <= 0 )
error(E_BOUNDS,"iter_arnoldi");
if ( Q->n != ip->x->dim || Q->m != ip->k )
error(E_SIZES,"iter_arnoldi");
m_zero(Q);
H = m_resize(H,ip->k,ip->k);
m_zero(H);
u = v_resize(u,ip->x->dim);
r = v_resize(r,ip->k);
MEM_STAT_REG(u,TYPE_VEC);
MEM_STAT_REG(r,TYPE_VEC);
v.dim = v.max_dim = ip->x->dim;
c = v_norm2(ip->x);
if ( c <= 0.0)
return H;
else {
v.ve = Q->me[0];
sv_mlt(1.0/c,ip->x,&v);
}
v_zero(r);
for ( i = 0; i < ip->k; i++ )
{
v.ve = Q->me[i];
u = (ip->Ax)(ip->A_par,&v,u);
for (j = 0; j <= i; j++) {
v.ve = Q->me[j];
/* modified Gram-Schmidt */
r->ve[j] = in_prod(&v,u);
v_mltadd(u,&v,-r->ve[j],u);
}
h_val = v_norm2(u);
/* if u == 0 then we have an exact subspace */
if ( h_val <= 0.0 )
{
*h_rem = h_val;
return H;
}
set_col(H,i,r);
if ( i == ip->k-1 )
{
*h_rem = h_val;
continue;
}
/* H->me[i+1][i] = h_val; */
m_set_val(H,i+1,i,h_val);
v.ve = Q->me[i+1];
sv_mlt(1.0/h_val,u,&v);
}
#ifdef THREADSAFE
V_FREE(u);
V_FREE(r);
#endif
return H;
}
MAT *iter_arnoldi_iref(ITER *ip, Real *h_rem, MAT *Q, MAT *H)
#endif
{
STATIC VEC *u=VNULL, *r=VNULL, *s=VNULL, *tmp=VNULL;
VEC v; /* auxiliary vector */
int i,j;
Real h_val, c;
if (ip == INULL)
error(E_NULL,"iter_arnoldi_iref");
if ( ! ip->Ax || ! Q || ! ip->x )
error(E_NULL,"iter_arnoldi_iref");
if ( ip->k <= 0 )
error(E_BOUNDS,"iter_arnoldi_iref");
if ( Q->n != ip->x->dim || Q->m != ip->k )
error(E_SIZES,"iter_arnoldi_iref");
m_zero(Q);
H = m_resize(H,ip->k,ip->k);
m_zero(H);
u = v_resize(u,ip->x->dim);
r = v_resize(r,ip->k);
s = v_resize(s,ip->k);
tmp = v_resize(tmp,ip->x->dim);
MEM_STAT_REG(u,TYPE_VEC);
MEM_STAT_REG(r,TYPE_VEC);
MEM_STAT_REG(s,TYPE_VEC);
MEM_STAT_REG(tmp,TYPE_VEC);
v.dim = v.max_dim = ip->x->dim;
c = v_norm2(ip->x);
if ( c <= 0.0)
return H;
else {
v.ve = Q->me[0];
sv_mlt(1.0/c,ip->x,&v);
}
v_zero(r);
v_zero(s);
for ( i = 0; i < ip->k; i++ )
{
v.ve = Q->me[i];
u = (ip->Ax)(ip->A_par,&v,u);
for (j = 0; j <= i; j++) {
v.ve = Q->me[j];
/* modified Gram-Schmidt */
r->ve[j] = in_prod(&v,u);
v_mltadd(u,&v,-r->ve[j],u);
}
h_val = v_norm2(u);
/* if u == 0 then we have an exact subspace */
if ( h_val <= 0.0 )
{
*h_rem = h_val;
return H;
}
/* iterative refinement -- ensures near orthogonality */
do {
v_zero(tmp);
for (j = 0; j <= i; j++) {
v.ve = Q->me[j];
s->ve[j] = in_prod(&v,u);
v_mltadd(tmp,&v,s->ve[j],tmp);
}
v_sub(u,tmp,u);
v_add(r,s,r);
} while ( v_norm2(s) > 0.1*(h_val = v_norm2(u)) );
/* now that u is nearly orthogonal to Q, update H */
set_col(H,i,r);
/* check once again if h_val is zero */
if ( h_val <= 0.0 )
{
*h_rem = h_val;
return H;
}
if ( i == ip->k-1 )
{
*h_rem = h_val;
continue;
}
/* H->me[i+1][i] = h_val; */
m_set_val(H,i+1,i,h_val);
v.ve = Q->me[i+1];
sv_mlt(1.0/h_val,u,&v);
}
#ifdef THREADSAFE
V_FREE(u);
V_FREE(r);
V_FREE(s);
V_FREE(tmp);
#endif
return H;
}
/***************************************************************
* void malloc_tests
*
*
****************************************************************/
void malloc_tests() {
unsigned int row = 3;
unsigned int col = 3;
unsigned int size1 = 30000000;
unsigned int* test_ptr1 = malloc_custom(size1, 0);
set_row(row); set_col(col);
vprintf_custom("1 Asked: %d, starts at: %x", size1, test_ptr1);
set_row(row++); set_col(col + 40);
vprintf_custom("mem left: %d", user_memory_size - allocated_space);
unsigned int size2 = 1000;
unsigned int* test_ptr2 = malloc_custom(size2, 0);
set_row(row); set_col(col);
vprintf_custom("2 Asked: %d, starts at: %x", size2, test_ptr2);
set_row(row++); set_col(col + 40);
vprintf_custom("mem left: %d", user_memory_size - allocated_space);
unsigned int size3 = 0x00A00000; // 10MB
unsigned int* test_ptr3 = malloc_custom(size3, 0);
set_row(row); set_col(col);
vprintf_custom("3 Asked: %d, starts at: %x", size3, test_ptr3);
set_row(row++); set_col(col + 40);
vprintf_custom("mem left: %d", user_memory_size - allocated_space);
unsigned int size4 = 0x00A00000; // 10MB
unsigned int* test_ptr4 = malloc_custom(size4, 0);
set_row(row); set_col(col);
vprintf_custom("4 Asked: %d, starts at: %x", size4, test_ptr4);
set_row(row++); set_col(col + 40);
vprintf_custom("mem left: %d", user_memory_size - allocated_space);
unsigned int size5 = 0x00100000; // 1MB
unsigned int* test_ptr5 = malloc_custom(size5, 1);
set_row(row); set_col(col);
vprintf_custom("5 Asked: %d, starts at: %x", size5, test_ptr5);
set_row(row++); set_col(col + 40);
vprintf_custom("mem left: %d", user_memory_size - allocated_space);
unsigned int size6 = 0x00100000; // 1MB
unsigned int* test_ptr6 = malloc_custom(size6, 0);
set_row(row); set_col(col);
vprintf_custom("6 Asked: %d, starts at: %x", size6, test_ptr6);
set_row(row++); set_col(col + 40);
vprintf_custom("mem left: %d", user_memory_size - allocated_space);
unsigned int size7 = 0x00100000; // 1MB
unsigned int* test_ptr7 = malloc_custom(size7, 0);
set_row(row); set_col(col);
vprintf_custom("7 Asked: %d, starts at: %x", size7, test_ptr7);
set_row(row++); set_col(col + 40);
vprintf_custom("mem left: %d", user_memory_size - allocated_space);
unsigned int size8 = 0x00400000; // 4MB
unsigned int* test_ptr8 = malloc_custom(size8, 0);
set_row(row); set_col(col);
vprintf_custom("8 Asked: %d, starts at: %x", size8, test_ptr8);
set_row(row++); set_col(col + 40);
vprintf_custom("mem left: %d", user_memory_size - allocated_space);
unsigned int size9 = 0x00100000; // 64KB
unsigned int* test_ptr9 = malloc_custom(size9, 0);
set_row(row); set_col(col);
vprintf_custom("9 Asked: %d, starts at: %x", size9, test_ptr9);
set_row(row++); set_col(col + 40);
vprintf_custom("mem left: %d", user_memory_size - allocated_space);
unsigned int size10 = 0x00100000; // 64KB
unsigned int* test_ptr10 = malloc_custom(size10, 0);
set_row(row); set_col(col);
vprintf_custom("10 Asked: %d, starts at: %x", size10, test_ptr10);
set_row(row++); set_col(col + 40);
vprintf_custom("mem left: %d", user_memory_size - allocated_space);
unsigned int size11 = 0x00100000; // 64KB
unsigned int* test_ptr11 = malloc_custom(size11, 0);
set_row(row); set_col(col);
vprintf_custom("11 Asked: %d, starts at: %x", size11, test_ptr11);
set_row(row++); set_col(col + 40);
vprintf_custom("mem left: %d", user_memory_size - allocated_space);
free(test_ptr6);
set_row(row); set_col(col);
vprintf_custom("Released: %x (bytes)", size7);
set_row(row++); set_col(col + 40);
vprintf_custom("mem left: %d", user_memory_size - allocated_space);
free(test_ptr7);
set_row(row); set_col(col);
vprintf_custom("Released: %x (bytes)", size7);
set_row(row++); set_col(col + 40);
vprintf_custom("mem left: %d", user_memory_size - allocated_space);
test_ptr9 = malloc_custom(size9, 0);
set_row(row); set_col(col);
vprintf_custom("12 Asked: %d, starts at: %x", size9, test_ptr9);
set_row(row++); set_col(col + 40);
vprintf_custom("mem left: %d", user_memory_size - allocated_space);
print_vscreen();
}
void Data_Elise_Gra_Win::set_fill_style(Data_Fill_St * dfst)
{
set_active();
set_col(dfst->dcp());
}
void Data_Elise_Gra_Win::set_line_style(Data_Line_St * dlst)
{
set_active();
_degd->set_line_witdh(dlst->witdh());
set_col(dlst->dcp());
}
/*
* This program demonstrates the Cdk matrix widget.
*/
int main (int argc, char **argv)
{
/* *INDENT-EQLS* */
CDKSCREEN *cdkscreen = 0;
CDKMATRIX *courseList = 0;
WINDOW *cursesWin = 0;
const char *title = 0;
int rows = 8;
int cols = 5;
int vrows = 3;
int vcols = 5;
bool use_coltitles;
bool use_rowtitles;
const char *coltitle[MY_COLS];
const char *rowtitle[MY_COLS];
const char *mesg[MY_COLS];
int colwidth[MY_COLS];
int colvalue[MY_COLS];
CDK_PARAMS params;
CDKparseParams (argc, argv, ¶ms, "trcT:" CDK_MIN_PARAMS);
/* invert, so giving -S causes the shadow to turn off */
params.Shadow = !params.Shadow;
/* cancel the default title, or supply a new one */
if (CDKparamValue (¶ms, 't', FALSE))
{
title = 0;
}
else if ((title = CDKparamString (¶ms, 'T')) == 0)
{
title = "<C>This is the CDK\n<C>matrix widget.\n<C><#LT><#HL(30)><#RT>";
}
/* allow cancelling of column and/or row titles with -c and/or -r */
use_coltitles = !CDKparamValue (¶ms, 'c', FALSE);
use_rowtitles = !CDKparamValue (¶ms, 'r', FALSE);
/* Set up CDK. */
cursesWin = initscr ();
cdkscreen = initCDKScreen (cursesWin);
/* Start CDK Colors. */
initCDKColor ();
/* Create the horizontal and vertical matrix labels. */
#define set_col(n, width, string) \
coltitle[n] = use_coltitles ? string : 0 ;\
colwidth[n] = width ;\
colvalue[n] = vUMIXED
set_col (1, 7, "</B/5>Course");
set_col (2, 7, "</B/33>Lec 1");
set_col (3, 7, "</B/33>Lec 2");
set_col (4, 7, "</B/33>Lec 3");
set_col (5, 1, "</B/7>Flag");
#define set_row(n, string) \
rowtitle[n] = use_rowtitles ? "<C></B/6>" string : 0
set_row (1, "Course 1");
set_row (2, "Course 2");
set_row (3, "Course 3");
set_row (4, "Course 4");
set_row (5, "Course 5");
set_row (6, "Course 6");
set_row (7, "Course 7");
set_row (8, "Course 8");
/* Create the matrix object. */
courseList = newCDKMatrix (cdkscreen,
CDKparamValue (¶ms, 'X', CENTER),
CDKparamValue (¶ms, 'Y', CENTER),
rows, cols, vrows, vcols,
title,
(CDK_CSTRING2) rowtitle,
(CDK_CSTRING2) coltitle,
colwidth, colvalue,
-1, -1, '.',
COL, params.Box,
params.Box,
params.Shadow);
/* Check to see if the matrix is null. */
if (courseList == 0)
{
/* Clean up. */
destroyCDKScreen (cdkscreen);
endCDK ();
printf ("Cannot create the matrix widget.\n");
printf ("Is the window too small ?\n");
ExitProgram (EXIT_FAILURE);
}
/* Activate the matrix. */
activateCDKMatrix (courseList, 0);
/* Check if the user hit escape or not. */
if (courseList->exitType == vESCAPE_HIT)
{
mesg[0] = "<C>You hit escape. No information passed back.";
mesg[1] = "";
mesg[2] = "<C>Press any key to continue.";
popupLabel (cdkscreen, (CDK_CSTRING2) mesg, 3);
}
else if (courseList->exitType == vNORMAL)
{
char temp[80];
sprintf (temp, "Current cell (%d,%d)", courseList->crow, courseList->ccol);
mesg[0] = "<L>You exited the matrix normally.";
mesg[1] = temp;
mesg[2] = "<L>To get the contents of the matrix cell, you can";
mesg[3] = "<L>use getCDKMatrixCell():";
mesg[4] = getCDKMatrixCell (courseList, courseList->crow, courseList->ccol);
mesg[5] = "";
mesg[6] = "<C>Press any key to continue.";
popupLabel (cdkscreen, (CDK_CSTRING2) mesg, 7);
}
/* Clean up. */
destroyCDKMatrix (courseList);
destroyCDKScreen (cdkscreen);
endCDK ();
ExitProgram (EXIT_SUCCESS);
}
void Ukf(VEC *omega, VEC *mag_vec, VEC *mag_vec_I, VEC *sun_vec, VEC *sun_vec_I, VEC *Torq_ext, double t, double h, int eclipse, VEC *state, VEC *st_error, VEC *residual, int *P_flag, double sim_time)
{
static VEC *omega_prev = VNULL, *mag_vec_prev = VNULL, *sun_vec_prev = VNULL, *q_s_c = VNULL, *x_prev = VNULL, *Torq_prev, *x_m_o;
static MAT *Q = {MNULL}, *R = {MNULL}, *Pprev = {MNULL};
static double alpha, kappa, lambda, sqrt_lambda, w_m_0, w_c_0, w_i, beta;
static int n_states, n_sig_pts, n_err_states, iter_num, initialize=0;
VEC *x = VNULL, *x_priori = VNULL, *x_err_priori = VNULL, *single_sig_pt = VNULL, *v_temp = VNULL, *q_err_quat = VNULL,
*err_vec = VNULL, *v_temp2 = VNULL, *x_ang_vel = VNULL, *meas = VNULL, *meas_priori = VNULL,
*v_temp3 = VNULL, *x_posteriori_err = VNULL, *x_b_m = VNULL, *x_b_g = VNULL;
MAT *sqrt_P = {MNULL}, *P = {MNULL}, *P_priori = {MNULL}, *sig_pt = {MNULL}, *sig_vec_mat = {MNULL},
*err_sig_pt_mat = {MNULL}, *result = {MNULL}, *result_larger = {MNULL}, *result1 = {MNULL}, *Meas_err_mat = {MNULL},
*P_zz = {MNULL}, *iP_vv = {MNULL}, *P_xz = {MNULL}, *K = {MNULL}, *result2 = {MNULL}, *result3 = {MNULL}, *C = {MNULL};
int update_mag_vec, update_sun_vec, update_omega, i, j;
double d_res;
if (inertia == MNULL)
{
inertia = m_get(3,3);
m_ident(inertia);
inertia->me[0][0] = 0.007;
inertia->me[1][1] = 0.014;
inertia->me[2][2] = 0.015;
}
if (initialize == 0){
iter_num = 1;
n_states = (7+6);
n_err_states = (6+6);
n_sig_pts = 2*n_err_states+1;
alpha = sqrt(3);
kappa = 3 - n_states;
lambda = alpha*alpha * (n_err_states+kappa) - n_err_states;
beta = -(1-(alpha*alpha));
w_m_0 = (lambda)/(n_err_states + lambda);
w_c_0 = (lambda/(n_err_states + lambda)) + (1 - (alpha*alpha) + beta);
w_i = 0.5/(n_err_states +lambda);
initialize = 1;
sqrt_lambda = (lambda+n_err_states);
if(q_s_c == VNULL)
{
q_s_c = v_get(4);
q_s_c->ve[0] = -0.020656;
q_s_c->ve[1] = 0.71468;
q_s_c->ve[2] = -0.007319;
q_s_c->ve[3] = 0.6991;
}
if(Torq_prev == VNULL)
{
Torq_prev = v_get(3);
v_zero(Torq_prev);
}
quat_normalize(q_s_c);
}
result = m_get(9,9);
m_zero(result);
result1 = m_get(n_err_states, n_err_states);
m_zero(result1);
if(x_m_o == VNULL)
{
x_m_o = v_get(n_states);
v_zero(x_m_o);
}
x = v_get(n_states);
v_zero(x);
x_err_priori = v_get(n_err_states);
v_zero(x_err_priori);
x_ang_vel = v_get(3);
v_zero(x_ang_vel);
sig_pt = m_get(n_states, n_err_states);
m_zero(sig_pt);
if (C == MNULL)
{
C = m_get(9, 12);
m_zero(C);
}
if (P_priori == MNULL)
{
P_priori = m_get(n_err_states, n_err_states);
m_zero(P_priori);
}
if (Q == MNULL)
{
Q = m_get(n_err_states, n_err_states);
m_ident(Q);
//
Q->me[0][0] = 0.0001;
Q->me[1][1] = 0.0001;
Q->me[2][2] = 0.0001;
Q->me[3][3] = 0.0001;
Q->me[4][4] = 0.0001;
Q->me[5][5] = 0.0001;
Q->me[6][6] = 0.000001;
Q->me[7][7] = 0.000001;
Q->me[8][8] = 0.000001;
Q->me[9][9] = 0.000001;
Q->me[10][10] = 0.000001;
Q->me[11][11] = 0.000001;
}
if( Pprev == MNULL)
{
Pprev = m_get(n_err_states, n_err_states);
m_ident(Pprev);
Pprev->me[0][0] = 1e-3;
Pprev->me[1][1] = 1e-3;
Pprev->me[2][2] = 1e-3;
Pprev->me[3][3] = 1e-3;
Pprev->me[4][4] = 1e-3;
Pprev->me[5][5] = 1e-3;
Pprev->me[6][6] = 1e-4;
Pprev->me[7][7] = 1e-4;
Pprev->me[8][8] = 1e-4;
Pprev->me[9][9] = 1e-3;
Pprev->me[10][10] = 1e-3;
Pprev->me[11][11] = 1e-3;
}
if (R == MNULL)
{
R = m_get(9,9);
m_ident(R);
R->me[0][0] = 0.034;
R->me[1][1] = 0.034;
R->me[2][2] = 0.034;
R->me[3][3] = 0.00027;
R->me[4][4] = 0.00027;
R->me[5][5] = 0.00027;
R->me[6][6] = 0.000012;
R->me[7][7] = 0.000012;
R->me[8][8] = 0.000012;
}
if(eclipse==0)
{
R->me[0][0] = 0.00034;
R->me[1][1] = 0.00034;
R->me[2][2] = 0.00034;
R->me[3][3] = 0.00027;
R->me[4][4] = 0.00027;
R->me[5][5] = 0.00027;
R->me[6][6] = 0.0000012;
R->me[7][7] = 0.0000012;
R->me[8][8] = 0.0000012;
Q->me[0][0] = 0.00001;
Q->me[1][1] = 0.00001;
Q->me[2][2] = 0.00001;
Q->me[3][3] = 0.0001;//0.000012;//0.0175;//1e-3;
Q->me[4][4] = 0.0001;//0.0175;//1e-3;
Q->me[5][5] = 0.0001;//0.0175;//1e-3;
Q->me[6][6] = 0.0000000001;//1e-6;
Q->me[7][7] = 0.0000000001;
Q->me[8][8] = 0.0000000001;
Q->me[9][9] = 0.0000000001;
Q->me[10][10] = 0.0000000001;
Q->me[11][11] = 0.0000000001;
}
else
{
R->me[0][0] = 0.34;
R->me[1][1] = 0.34;
R->me[2][2] = 0.34;
R->me[3][3] = 0.0027;
R->me[4][4] = 0.0027;
R->me[5][5] = 0.0027;
R->me[6][6] = 0.0000012;
R->me[7][7] = 0.0000012;
R->me[8][8] = 0.0000012;
Q->me[0][0] = 0.00001;
Q->me[1][1] = 0.00001;
Q->me[2][2] = 0.00001;
Q->me[3][3] = 0.0001;
Q->me[4][4] = 0.0001;
Q->me[5][5] = 0.0001;
Q->me[6][6] = 0.0000000001;
Q->me[7][7] = 0.0000000001;
Q->me[8][8] = 0.0000000001;
Q->me[9][9] = 0.0000000001;
Q->me[10][10] = 0.0000000001;
Q->me[11][11] = 0.0000000001;
}
if(omega_prev == VNULL)
{
omega_prev = v_get(3);
v_zero(omega_prev);
}
if(mag_vec_prev == VNULL)
{
mag_vec_prev = v_get(3);
v_zero(mag_vec_prev);
}
if(sun_vec_prev == VNULL)
{
sun_vec_prev = v_get(3);
v_zero(sun_vec_prev);
}
if (err_sig_pt_mat == MNULL)
{
err_sig_pt_mat = m_get(n_err_states, n_sig_pts);
m_zero(err_sig_pt_mat);
}
if(q_err_quat == VNULL)
{
q_err_quat = v_get(4);
// q_err_quat = v_resize(q_err_quat,4);
v_zero(q_err_quat);
}
if(err_vec == VNULL)
{
err_vec = v_get(3);
v_zero(err_vec);
}
v_temp = v_get(9);
v_resize(v_temp,3);
if(x_prev == VNULL)
{
x_prev = v_get(n_states);
v_zero(x_prev);
x_prev->ve[3] = 1;
quat_mul(x_prev,q_s_c,x_prev);
x_prev->ve[4] = 0.0;
x_prev->ve[5] = 0.0;
x_prev->ve[6] = 0.0;
x_prev->ve[7] = 0.0;
x_prev->ve[8] = 0.0;
x_prev->ve[9] = 0.0;
x_prev->ve[10] = 0.0;
x_prev->ve[11] = 0.0;
x_prev->ve[12] = 0.0;
}
sqrt_P = m_get(n_err_states, n_err_states);
m_zero(sqrt_P);
//result = m_resize(result, n_err_states, n_err_states);
result_larger = m_get(n_err_states, n_err_states);
int n, m;
for(n = 0; n < result->n; n++)
{
for(m = 0; m < result->m; m++)
{
result_larger->me[m][n] = result->me[m][n];
}
}
//v_resize(v_temp, n_err_states);
V_FREE(v_temp);
v_temp = v_get(n_err_states);
symmeig(Pprev, result_larger, v_temp);
i = 0;
for (j=0;j<n_err_states;j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
m_copy(Pprev, result1);
sm_mlt(sqrt_lambda, result1, result_larger);
catchall(CHfactor(result_larger), printerr(sim_time));
for(i=0; i<n_err_states; i++){
for(j=i+1; j<n_err_states; j++){
result_larger->me[i][j] = 0;
}
}
expandstate(result_larger, x_prev, sig_pt);
sig_vec_mat = m_get(n_states, n_sig_pts);
m_zero(sig_vec_mat);
for(j = 0; j<(n_err_states+1); j++)
{
for(i = 0; i<n_states; i++)
{
if(j==0)
{
sig_vec_mat->me[i][j] = x_prev->ve[i];
}
else if(j>0)
{
sig_vec_mat->me[i][j] = sig_pt->me[i][j-1];
}
}
}
sm_mlt(-1,result_larger,result_larger);
expandstate(result_larger, x_prev, sig_pt);
for(j = (n_err_states+1); j<n_sig_pts; j++)
{
for(i = 0; i<n_states; i++)
{
sig_vec_mat->me[i][j] = sig_pt->me[i][j-(n_err_states+1)];
}
}
single_sig_pt = v_get(n_states);
quat_rot_vec(q_s_c, Torq_ext);
for(j=0; j<(n_sig_pts); j++)
{
//v_temp = v_resize(v_temp,n_states);
V_FREE(v_temp);
v_temp = v_get(n_states);
get_col(sig_vec_mat, j, single_sig_pt);
v_copy(single_sig_pt, v_temp);
rk4(t, v_temp, h, Torq_prev);
set_col(sig_vec_mat, j, v_temp);
}
v_copy(Torq_ext, Torq_prev);
x_priori = v_get(n_states);
v_zero(x_priori);
v_resize(v_temp,n_states);
v_zero(v_temp);
for(j=0; j<n_sig_pts; j++)
{
get_col( sig_vec_mat, j, v_temp);
if(j == 0)
{
v_mltadd(x_priori, v_temp, w_m_0, x_priori);
}
else
{
v_mltadd(x_priori, v_temp, w_i, x_priori);
}
}
v_copy(x_priori, v_temp);
v_resize(v_temp,4);
quat_normalize(v_temp);//zaroori hai ye
for(i=0; i<4; i++)
{
x_priori->ve[i] = v_temp->ve[i];
}
v_resize(v_temp, n_states);
v_copy(x_priori, v_temp);
v_resize(v_temp, 4);
quat_inv(v_temp, v_temp);
for(i=0; i<3; i++)
{
x_ang_vel->ve[i] = x_priori->ve[i+4];
}
x_b_m = v_get(3);
v_zero(x_b_m);
x_b_g = v_get(3);
v_zero(x_b_g);
/////////////////////////check it!!!!!!!! checked... doesnt change much the estimate
for(i=0; i<3; i++)
{
x_b_m->ve[i] = x_priori->ve[i+7];
x_b_g->ve[i] = x_priori->ve[i+10];
}
v_temp2 = v_get(n_states);
v_zero(v_temp2);
for(j=0; j<n_sig_pts; j++)
{
v_resize(v_temp2, n_states);
get_col( sig_vec_mat, j, v_temp2);
for(i=0; i<3; i++)
{
err_vec->ve[i] = v_temp2->ve[i+4];
}
v_resize(v_temp2, 4);
quat_mul(v_temp2, v_temp, q_err_quat);
v_resize(q_err_quat, n_err_states);
v_sub(err_vec, x_ang_vel, err_vec);
for(i=3; i<6; i++)
{
q_err_quat->ve[i] = err_vec->ve[i-3];
}
for(i=0; i<3; i++)
{
err_vec->ve[i] = v_temp2->ve[i+7];
}
v_sub(err_vec, x_b_m, err_vec);
for(i=6; i<9; i++)
{
q_err_quat->ve[i] = err_vec->ve[i-6];
}
for(i=0; i<3; i++)
{
err_vec->ve[i] = v_temp2->ve[i+10];
}
v_sub(err_vec, x_b_g, err_vec);
for(i=9; i<12; i++)
{
q_err_quat->ve[i] = err_vec->ve[i-9];
}
set_col(err_sig_pt_mat, j, q_err_quat);
if(j==0){
v_mltadd(x_err_priori, q_err_quat, w_m_0, x_err_priori);
}
else{
v_mltadd(x_err_priori, q_err_quat, w_i, x_err_priori);
}
}
v_resize(v_temp,n_err_states);
for (j=0;j<13;j++)
{
get_col(err_sig_pt_mat, j, v_temp);
v_sub(v_temp, x_err_priori, v_temp);
get_dyad(v_temp, v_temp, result_larger);
if(j==0){
sm_mlt(w_c_0, result_larger, result_larger);
}
else{
sm_mlt(w_i, result_larger, result_larger);
}
m_add(P_priori, result_larger, P_priori);
}
symmeig(P_priori, result_larger, v_temp);
i = 0;
for (j=0;j<n_err_states;j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
m_add(P_priori, Q, P_priori);
v_resize(v_temp,3);
meas = v_get(9);
if (!(is_vec_equal(sun_vec, sun_vec_prev)) /*&& (eclipse==0)*/ ){
update_sun_vec =1;
v_copy(sun_vec, sun_vec_prev);
v_copy(sun_vec, v_temp);
normalize_vec(v_temp);
quat_rot_vec(q_s_c, v_temp);
normalize_vec(v_temp);
for(i = 0; i<3;i++){
meas->ve[i] = v_temp->ve[i];
}
}
else{
update_sun_vec =0;
for(i = 0; i<3;i++){
meas->ve[i] = 0;
}
}
if (!(is_vec_equal(mag_vec, mag_vec_prev)) ){
update_mag_vec =1;
v_copy(mag_vec, mag_vec_prev);
v_copy(mag_vec, v_temp);
normalize_vec(v_temp);
quat_rot_vec(q_s_c, v_temp);
normalize_vec(v_temp);
for(i=3; i<6; i++){
meas->ve[i] = v_temp->ve[i-3];
}
}
else{
update_mag_vec =0;
for(i=3; i<6; i++){
meas->ve[i] = 0;//mag_vec_prev->ve[i-3];
}
}
if (!(is_vec_equal(omega, omega_prev) ) ){
update_omega =1;
v_copy(omega, omega_prev);
v_copy(omega, v_temp);
quat_rot_vec(q_s_c, v_temp);
for(i=6; i<9; i++){
meas->ve[i] = v_temp->ve[i-6];
}
}
else{
update_omega =0;
for(i=6; i<9; i++){
meas->ve[i] = 0;
}
}
v_resize(v_temp, 9);
v_resize(v_temp2, n_states);
v_temp3 = v_get(3);
Meas_err_mat = m_get(9, n_sig_pts);
m_zero(Meas_err_mat);
meas_priori = v_get(9);
v_zero(meas_priori);
for(j=0; j<n_sig_pts; j++)
{
get_col( sig_vec_mat, j, v_temp2);
if(update_omega){
for(i=6;i<9;i++){
v_temp->ve[i] = v_temp2->ve[i-2] + x_b_g->ve[i-6];
}
}
else{
for(i=6;i<9;i++){
v_temp->ve[i] = 0;
}
}
v_resize(v_temp2, 4);
if(update_sun_vec){
for(i=0;i<3;i++){
v_temp3->ve[i] = sun_vec_I->ve[i];
}
quat_rot_vec(v_temp2, v_temp3);
normalize_vec(v_temp3);
for(i=0;i<3;i++){
v_temp->ve[i] = v_temp3->ve[i];
}
}
else{
for(i=0;i<3;i++){
v_temp->ve[i] = 0;
}
}
if(update_mag_vec){
for(i=0;i<3;i++){
v_temp3->ve[i] = mag_vec_I->ve[i];
}
normalize_vec(v_temp3);
for(i=0;i<3;i++){
v_temp3->ve[i] = v_temp3->ve[i] + x_b_m->ve[i];
}
quat_rot_vec(v_temp2, v_temp3);
normalize_vec(v_temp3);
for(i=3;i<6;i++){
v_temp->ve[i] = v_temp3->ve[i-3];
}
}
else{
for(i=3;i<6;i++){
v_temp->ve[i] = 0;
}
}
set_col(Meas_err_mat, j, v_temp);
if(j==0){
v_mltadd(meas_priori, v_temp, w_m_0, meas_priori);
}
else{
v_mltadd(meas_priori, v_temp, w_i, meas_priori);
}
}
v_resize(v_temp, 9);
m_resize(result_larger, 9, 9);
m_zero(result_larger);
P_zz = m_get(9, 9);
m_zero(P_zz);
iP_vv = m_get(9, 9);
m_zero(iP_vv);
P_xz = m_get(n_err_states, 9);
m_zero(P_xz);
v_resize(v_temp2, n_err_states);
result1 = m_resize(result1,n_err_states,9);
for (j=0; j<n_sig_pts; j++)
{
get_col( Meas_err_mat, j, v_temp);
get_col( err_sig_pt_mat, j, v_temp2);
v_sub(v_temp, meas_priori, v_temp);
get_dyad(v_temp, v_temp, result_larger);
get_dyad(v_temp2, v_temp, result1);
if(j==0){
sm_mlt(w_c_0, result_larger, result_larger);
sm_mlt(w_c_0, result1, result1);
}
else{
sm_mlt(w_i, result_larger, result_larger);
sm_mlt(w_i, result1, result1);
}
m_add(P_zz, result_larger, P_zz);
m_add(P_xz, result1, P_xz);
}
symmeig(P_zz, result_larger, v_temp);
i = 0;
for (j=0; j<9; j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
m_add(P_zz, R, P_zz);
m_inverse(P_zz, iP_vv);
K = m_get(n_err_states, 9);
m_zero(K);
m_mlt(P_xz, iP_vv, K);
if(x_posteriori_err == VNULL)
{
x_posteriori_err = v_get(n_err_states);
v_zero(x_posteriori_err);
}
v_resize(v_temp,9);
v_sub(meas, meas_priori, v_temp);
v_copy(v_temp, residual);
mv_mlt(K, v_temp, x_posteriori_err);
v_resize(v_temp2,3);
for(i=0;i<3;i++){
v_temp2->ve[i] = x_posteriori_err->ve[i];
}
for(i=4; i<n_states; i++){
x_prev->ve[i] = (x_posteriori_err->ve[i-1] + x_priori->ve[i]);
}
d_res = v_norm2(v_temp2);
v_resize(v_temp2,4);
if(d_res<=1 /*&& d_res!=0*/){
v_temp2->ve[0] = v_temp2->ve[0];
v_temp2->ve[1] = v_temp2->ve[1];
v_temp2->ve[2] = v_temp2->ve[2];
v_temp2->ve[3] = sqrt(1-d_res);
}
else//baad main daala hai
{
v_temp2->ve[0] = (v_temp2->ve[0])/(sqrt(1+d_res));
v_temp2->ve[1] = (v_temp2->ve[1])/(sqrt(1+d_res));
v_temp2->ve[2] = (v_temp2->ve[2])/(sqrt(1+d_res));
v_temp2->ve[3] = 1/sqrt(1 + d_res);
}
v_resize(x_posteriori_err, n_states);
for(i=(n_states-1); i>3; i--){
x_posteriori_err->ve[i] = x_posteriori_err->ve[i-1];
}
for(i=0; i<4; i++){
x_posteriori_err->ve[i] = v_temp2->ve[i];
}
quat_mul(v_temp2, x_priori, v_temp2);
for(i=0;i<4;i++){
x_prev->ve[i] = v_temp2->ve[i];
}
m_resize(result_larger, n_err_states, 9);
m_mlt(K, P_zz, result_larger);
result2 = m_get(9, n_err_states);
m_transp(K,result2);
m_resize(result1, n_err_states, n_err_states);
m_mlt(result_larger, result2, result1);
v_resize(v_temp, n_err_states);
m_sub(P_priori, result1, Pprev);
symmeig(Pprev, result1 , v_temp);
i = 0;
for (j=0;j<n_err_states;j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
v_copy(x_prev, v_temp);
v_resize(v_temp,4);
v_copy(x_prev, v_temp2);
v_resize(v_temp2,4);
v_copy(x_prev, x_m_o);
//v_resize(x_m_o, 4);
v_resize(v_temp,3);
quat_inv(q_s_c, v_temp2);
v_copy( x_prev, state);
quat_mul(state, v_temp2, state);
for(i=0; i<3; i++){
v_temp->ve[i] = state->ve[i+4];
}
quat_rot_vec(v_temp2, v_temp);
for(i=0; i<3; i++){
state->ve[i+4] = v_temp->ve[i];
}
v_copy( x_posteriori_err, st_error);
iter_num++;
V_FREE(x);
V_FREE(x_priori);
V_FREE(x_err_priori);
V_FREE(single_sig_pt);
V_FREE(v_temp);
V_FREE(q_err_quat);
V_FREE(err_vec);
V_FREE(v_temp2);
V_FREE(x_ang_vel);
V_FREE(meas);
V_FREE(meas_priori);
V_FREE(v_temp3);
V_FREE(x_posteriori_err);
V_FREE(x_b_m);
V_FREE(x_b_g);
M_FREE(sqrt_P);
M_FREE(P);
M_FREE(P_priori);
M_FREE(sig_pt);
M_FREE(sig_vec_mat);
M_FREE(err_sig_pt_mat);
M_FREE(result);
M_FREE(result_larger);
M_FREE(result1);
M_FREE(Meas_err_mat);
M_FREE(P_zz);
M_FREE(iP_vv);
M_FREE(P_xz);
M_FREE(K);
M_FREE(result2);
M_FREE(result3);
}
void turtle_t::exec(turtle_com_t *com)
{
if (com->cname==F)
{
turtle_fwd_t* fcom = dynamic_cast<turtle_fwd_t*>(com);
if (fcom) forward(fcom->dist);
}
else if (com->cname==B)
{
turtle_bck_t* bcom = dynamic_cast<turtle_bck_t*>(com);
if (bcom) back(bcom->dist);
}
else if (com->cname==L)
{
turtle_lft_t* lcom = dynamic_cast<turtle_lft_t*>(com);
if (lcom) turn_left(lcom->angl);
}
else if (com->cname==R)
{
turtle_rht_t* rcom = dynamic_cast<turtle_rht_t*>(com);
if (rcom) turn_right(rcom->angl);
}
else if (com->cname==MF)
{
turtle_mfwd_t* mfcom = dynamic_cast<turtle_mfwd_t*>(com);
if (mfcom) forward_move(mfcom->dist);
}
else if (com->cname==MB)
{
turtle_mbck_t* mbcom = dynamic_cast<turtle_mbck_t*>(com);
if (mbcom) backward_move(mbcom->dist);
}
else if (com->cname==CLS)
{
turtle_cls_t* clscom = dynamic_cast<turtle_cls_t*>(com);
if (clscom) clear();
}
else if (com->cname==RESET)
{
turtle_rst_t* rstcom = dynamic_cast<turtle_rst_t*>(com);
if (rstcom) reset();
}
else if (com->cname==COL)
{
turtle_col_t* colcom = dynamic_cast<turtle_col_t*>(com);
if (colcom) set_col(colcom->r, colcom->g, colcom->b);
}
else if (com->cname==BGCOL)
{
turtle_bgcol_t* bgcolcom = dynamic_cast<turtle_bgcol_t*>(com);
if (bgcolcom) set_bgcol(bgcolcom->r, bgcolcom->g, bgcolcom->b);
}
else if (com->cname==SCALE)
{
turtle_scale_t* scalecom = dynamic_cast<turtle_scale_t*>(com);
if (scalecom) scale(scalecom->s);
}
else if (com->cname==PAUSE)
{
turtle_pause_t* pausecom = dynamic_cast<turtle_pause_t*>(com);
if (pausecom) pause(pausecom->t);
}
else if (com->cname==REPEAT)
{
turtle_rep_t *repcom = dynamic_cast<turtle_rep_t*>(com);
if (repcom)
{
unsigned int times = repcom->times;
turtle_com_list_t sublist = repcom->replist;
repeat(times, sublist);
}
}
else if ((com->cname==ENDREP) || (com->cname==END) || (com->cname==BEGIN))
{
//These commands are place holders and used for program structure
//But no execution is necessary - generate a NoOP
;
}
else
{
std::cerr<<"Unknown Command: Ignoring"<<std::endl;
exit(-1);
}
}
VEC *iter_mgcr(ITER *ip)
#endif
{
STATIC VEC *As=VNULL, *beta=VNULL, *alpha=VNULL, *z=VNULL;
STATIC MAT *N=MNULL, *H=MNULL;
VEC *rr, v, s; /* additional pointer and structures */
Real nres; /* norm of a residual */
Real dd; /* coefficient d_i */
int i,j;
int done; /* if TRUE then stop the iterative process */
int dim; /* dimension of the problem */
/* ip cannot be NULL */
if (ip == INULL) error(E_NULL,"mgcr");
/* Ax, b and stopping criterion must be given */
if (! ip->Ax || ! ip->b || ! ip->stop_crit)
error(E_NULL,"mgcr");
/* at least one direction vector must exist */
if ( ip->k <= 0) error(E_BOUNDS,"mgcr");
/* if the vector x is given then b and x must have the same dimension */
if ( ip->x && ip->x->dim != ip->b->dim)
error(E_SIZES,"mgcr");
if (ip->eps <= 0.0) ip->eps = MACHEPS;
dim = ip->b->dim;
As = v_resize(As,dim);
alpha = v_resize(alpha,ip->k);
beta = v_resize(beta,ip->k);
MEM_STAT_REG(As,TYPE_VEC);
MEM_STAT_REG(alpha,TYPE_VEC);
MEM_STAT_REG(beta,TYPE_VEC);
H = m_resize(H,ip->k,ip->k);
N = m_resize(N,ip->k,dim);
MEM_STAT_REG(H,TYPE_MAT);
MEM_STAT_REG(N,TYPE_MAT);
/* if a preconditioner is defined */
if (ip->Bx) {
z = v_resize(z,dim);
MEM_STAT_REG(z,TYPE_VEC);
}
/* if x is NULL then it is assumed that x has
entries with value zero */
if ( ! ip->x ) {
ip->x = v_get(ip->b->dim);
ip->shared_x = FALSE;
}
/* v and s are additional pointers to rows of N */
/* they must have the same dimension as rows of N */
v.dim = v.max_dim = s.dim = s.max_dim = dim;
done = FALSE;
for (ip->steps = 0; ip->steps < ip->limit; ) {
(*ip->Ax)(ip->A_par,ip->x,As); /* As = A*x */
v_sub(ip->b,As,As); /* As = b - A*x */
rr = As; /* rr is an additional pointer */
/* if a preconditioner is defined */
if (ip->Bx) {
(*ip->Bx)(ip->B_par,As,z); /* z = B*(b-A*x) */
rr = z;
}
/* norm of the residual */
nres = v_norm2(rr);
dd = nres; /* dd = ||r_i|| */
/* check if the norm of the residual is zero */
if (ip->steps == 0) {
/* information for a user */
if (ip->info) (*ip->info)(ip,nres,As,rr);
ip->init_res = fabs(nres);
}
if (nres == 0.0) {
/* iterative process is finished */
done = TRUE;
break;
}
/* save this residual in the first row of N */
v.ve = N->me[0];
v_copy(rr,&v);
for (i = 0; i < ip->k && ip->steps < ip->limit; i++) {
ip->steps++;
v.ve = N->me[i]; /* pointer to a row of N (=s_i) */
/* note that we must use here &v, not v */
(*ip->Ax)(ip->A_par,&v,As);
rr = As; /* As = A*s_i */
if (ip->Bx) {
(*ip->Bx)(ip->B_par,As,z); /* z = B*A*s_i */
rr = z;
}
if (i < ip->k - 1) {
s.ve = N->me[i+1]; /* pointer to a row of N (=s_{i+1}) */
v_copy(rr,&s); /* s_{i+1} = B*A*s_i */
for (j = 0; j <= i-1; j++) {
v.ve = N->me[j+1]; /* pointer to a row of N (=s_{j+1}) */
/* beta->ve[j] = in_prod(&v,rr); */ /* beta_{j,i} */
/* modified Gram-Schmidt algorithm */
beta->ve[j] = in_prod(&v,&s); /* beta_{j,i} */
/* s_{i+1} -= beta_{j,i}*s_{j+1} */
v_mltadd(&s,&v,- beta->ve[j],&s);
}
/* beta_{i,i} = ||s_{i+1}||_2 */
beta->ve[i] = nres = v_norm2(&s);
if ( nres <= MACHEPS*ip->init_res) {
/* s_{i+1} == 0 */
i--;
done = TRUE;
break;
}
sv_mlt(1.0/nres,&s,&s); /* normalize s_{i+1} */
v.ve = N->me[0];
alpha->ve[i] = in_prod(&v,&s); /* alpha_i = (s_0 , s_{i+1}) */
}
else {
for (j = 0; j <= i-1; j++) {
v.ve = N->me[j+1]; /* pointer to a row of N (=s_{j+1}) */
beta->ve[j] = in_prod(&v,rr); /* beta_{j,i} */
}
nres = in_prod(rr,rr); /* rr = B*A*s_{k-1} */
for (j = 0; j <= i-1; j++)
nres -= beta->ve[j]*beta->ve[j];
if (sqrt(fabs(nres)) <= MACHEPS*ip->init_res) {
/* s_k is zero */
i--;
done = TRUE;
break;
}
if (nres < 0.0) { /* do restart */
i--;
ip->steps--;
break;
}
beta->ve[i] = sqrt(nres); /* beta_{k-1,k-1} */
v.ve = N->me[0];
alpha->ve[i] = in_prod(&v,rr);
for (j = 0; j <= i-1; j++)
alpha->ve[i] -= beta->ve[j]*alpha->ve[j];
alpha->ve[i] /= beta->ve[i]; /* alpha_{k-1} */
}
set_col(H,i,beta);
/* other method of computing dd */
/* if (fabs((double)alpha->ve[i]) > dd) {
nres = - dd*dd + alpha->ve[i]*alpha->ve[i];
nres = sqrt((double) nres);
if (ip->info) (*ip->info)(ip,-nres,VNULL,VNULL);
break;
} */
/* to avoid overflow/underflow in computing dd */
/* dd *= cos(asin((double)(alpha->ve[i]/dd))); */
nres = alpha->ve[i]/dd;
if (fabs(nres-1.0) <= MACHEPS*ip->init_res)
dd = 0.0;
else {
nres = 1.0 - nres*nres;
if (nres < 0.0) {
nres = sqrt((double) -nres);
if (ip->info) (*ip->info)(ip,-dd*nres,VNULL,VNULL);
break;
}
dd *= sqrt((double) nres);
}
if (ip->info) (*ip->info)(ip,dd,VNULL,VNULL);
if ( ip->stop_crit(ip,dd,VNULL,VNULL) ) {
/* stopping criterion is satisfied */
done = TRUE;
break;
}
} /* end of for */
if (i >= ip->k) i = ip->k - 1;
/* use (i+1) by (i+1) submatrix of H */
H = m_resize(H,i+1,i+1);
alpha = v_resize(alpha,i+1);
Usolve(H,alpha,alpha,0.0); /* c_i is saved in alpha */
for (j = 0; j <= i; j++) {
v.ve = N->me[j];
v_mltadd(ip->x,&v,alpha->ve[j],ip->x);
}
if (done) break; /* stop the iterative process */
alpha = v_resize(alpha,ip->k);
H = m_resize(H,ip->k,ip->k);
} /* end of while */
#ifdef THREADSAFE
V_FREE(As);
V_FREE(beta);
V_FREE(alpha);
V_FREE(z);
M_FREE(N);
M_FREE(H);
#endif
return ip->x; /* return the solution */
}
/*
* This program demonstrates the Cdk matrix widget.
*/
int main (int argc, char **argv)
{
/* Declare local variables. */
CDKSCREEN *cdkscreen = 0;
CDKMATRIX *form_prov = 0;
WINDOW *cursesWin = 0;
char *title = 0;
int rows = 5;
int cols = 1;
int vrows = 5;
int vcols = 1;
char *coltitle[10], *rowtitle[10], *mesg[10];
int colwidth[10], colvalue[10];
// CDK_PARAMS params;
// CDKparseParams (argc, argv, ¶ms, CDK_MIN_PARAMS);
/* Set up CDK. */
cursesWin = initscr();
cdkscreen = initCDKScreen (cursesWin);
/* Start CDK Colors. */
initCDKColor();
/* Create the horizontal and vertical matrix labels. */
#define set_col(n, width, string) \
coltitle[n] = string; colwidth[n] = width ; colvalue[n] = vUMIXED
set_col(1, 7, "</B/5>Proveedor");
set_col(2, 7, "</B/33>Lec 1");
set_col(3, 7, "</B/33>Lec 2");
set_col(4, 7, "</B/33>Lec 3");
set_col(5, 1, "</B/7>Flag");
#define set_row(n, string) \
rowtitle[n] = "</B/8>" string
set_row(1, "ID");
set_row(2, "Nombre");
set_row(3, "Apellido");
set_row(4, "Correo");
set_row(5, "Direccion");
set_row(6, "Course 6");
set_row(7, "Course 7");
set_row(8, "Course 8");
/* Create the title. */
/* Create the matrix object. */
form_prov = newCDKMatrix (cdkscreen,
CENTER,
CENTER,
rows, cols, vrows, vcols,
title, rowtitle, coltitle,
colwidth, colvalue,
-1, -1, '.',
COL, TRUE,
TRUE,
FALSE);
/* Check to see if the matrix is null. */
if (form_prov == 0)
{
/* Clean up. */
destroyCDKScreen (cdkscreen);
endCDK();
/* Print out a little message. */
printf ("Oops. Can't seem to create the matrix widget. Is the window too small ?\n");
exit (EXIT_FAILURE);
}
/* Activate the matrix. */
activateCDKMatrix (form_prov, 0);
/* Check if the user hit escape or not. */
if (form_prov->exitType == vESCAPE_HIT)
{
mesg[0] = "<C>You hit escape. No information passed back.";
mesg[1] = "",
mesg[2] = "<C>Press any key to continue.";
popupLabel (cdkscreen, mesg, 3);
}
else if (form_prov->exitType == vNORMAL)
{
char temp[80];
sprintf(temp, "Current cell (%d,%d)", form_prov->crow, form_prov->ccol);
mesg[0] = "<L>You exited the matrix normally.";
mesg[1] = temp;
mesg[2] = "<L>To get the contents of the matrix cell, you can";
mesg[3] = "<L>use getCDKMatrixCell():";
mesg[4] = getCDKMatrixCell(form_prov, form_prov->crow, form_prov->ccol);
mesg[5] = "";
mesg[6] = "<C>Press any key to continue.";
popupLabel (cdkscreen, mesg, 7);
}
/* Clean up. */
destroyCDKMatrix (form_prov);
destroyCDKScreen (cdkscreen);
endCDK();
exit (EXIT_SUCCESS);
}
/***************************************************************
* void malloc_command
* Calcula la cantidad de RAM que tiene la máquina.
*
****************************************************************/
void malloc_command(char* params) {
char option[20];
unsigned int size = 0;
unsigned int i = 0;
memset_custom(option, sizeof(option), 0);
sscanf_custom(params, "%s %d", option, &size);
if( strcmp(option, "get") == 0 ) {
if( size > minimum_segment_size ) {
void* ptr = malloc_custom(size, 0);
newline();
if(ptr == 0) {
vprintf_custom("No hay espacio para alocar %d bytes.", size);
} else {
for(i = 0; info[i].ptr != 0; i++) {}
if( i < INFO_TABLE_SIZE ) {
vprintf_custom("Se aloco la memoria. Esta en la direccion %x", ptr);
info[i].ptr = ptr;
info[i].size = size;
} else {
newline();
vprintf_custom("No puedes alocar mas memoria porque llenaste la"
"tabla que tiene las referencias de las alocaciones, y que"
"usa el comando malloc info.");
}
}
} else {
newline();
vprintf_custom("Debes pedir %d bytes o mas.", minimum_segment_size);
}
} else if( strcmp(option, "info") == 0) {
clear_vscreen();
set_col(2); set_row(1);
vprintf_custom("Los valores de la izquierda son la direcciones "
"en donde empieza cada segmento, los de la derecha son los "
"tamanos de los mismos.");
set_col(2); set_row(3);
for(i = 0; i < INFO_TABLE_SIZE; i++) {
if( info[i].ptr != 0 ) {
set_col(2);
vprintf_custom("%x %d", info[i].ptr, info[i].size);
newline();
if( i > SCREEN_WIDTH - 3 ) {
set_col(20); set_row(3);
}
}
}
if (i == 0) {
vprintf_custom("Aun no alocaste ninguna porcion de memoria.");
}
} else if( strcmp(option, "") == 0 ) {
newline();
vprintf_custom("Debes ingresar al menos una opcion para"
" ser usada por esta funcion.");
} else if(strcmp(option, "test") == 0) {
malloc_tests();
q_to_continue(SCREEN_LENGTH - 1, 2, 1);
} else {
newline();
vprintf_custom("El comando que ingresaste no es valido.");
}
print_vscreen();
}
