static void v_networkReaderEntryInit( v_networkReaderEntry entry, v_networkReader reader, v_group group, v_networkId networkId, c_ulong channelsToConnect, v_networkPartitionId networkPartitionId, c_bool isRouting) { v_networkReaderEntry found; v_entryInit(v_entry(entry),v_reader(reader)); entry->group = c_keep(group); entry->networkId = networkId; entry->channelCountdown = channelsToConnect; c_mutexInit(&entry->channelCountdownMutex, SHARED_MUTEX); entry->networkPartitionId = networkPartitionId; entry->hashValue = v_networkReaderEntryCalculateHashValue(entry); entry->isRouting = isRouting; found = v_networkReaderEntry(v_readerAddEntry(v_reader(reader), v_entry(entry))); assert(found == entry); c_free(found); }
static void deliveryServiceUnSubscribe( void *o, void *arg) { v_partition p = v_partition(o); v_deliveryServiceEntry e = v_deliveryServiceEntry(arg); v_kernel kernel; v_group g; c_value params[2]; c_iter list; assert(C_TYPECHECK(e,v_deliveryServiceEntry)); assert(C_TYPECHECK(p,v_partition)); params[0] = c_objectValue(p); params[1] = c_objectValue(e->topic); kernel = v_objectKernel(e); list = v_groupSetSelect(kernel->groupSet, "partition = %0 and topic = %1", params); while ((g = c_iterTakeFirst(list)) != NULL) { v_groupRemoveEntry(g,v_entry(e)); c_free(g); } c_iterFree(list); }
/* * executes kalman predict and correct step */ void kalman_run(kalman_out_t *out, kalman_t *kalman, const kalman_in_t *in) { /* A = | init dt | | init init | */ m_set_val(kalman->A, 0, 1, in->dt); /* B = | 0.5 * dt ^ 2 | | dt | */ m_set_val(kalman->B, 0, 0, 0.5 * in->dt * in->dt); m_set_val(kalman->B, 1, 0, in->dt); kalman_predict(kalman, in->acc); kalman_correct(kalman, in->pos, 0.0); out->pos = v_entry(kalman->x, 0); out->speed = v_entry(kalman->x, 1); }
static c_bool getHistoricalData( c_object o, c_voidp arg) { v_entry entry; c_iter proxies; v_proxy proxy; v_group group; assert(o != NULL); entry = v_entry(o); assert(entry != NULL); assert(C_TYPECHECK(entry,v_entry)); proxies = c_select(entry->groups, 0); proxy = c_iterTakeFirst(proxies); while (proxy != NULL) { group = v_group(v_proxyClaim(proxy)); if (group) { if(arg == NULL){ v_groupGetHistoricalData(group, entry); } else { v_groupGetHistoricalDataWithCondition(group, entry, (v_historicalDataRequest)arg); } v_proxyRelease(proxy); } c_free(proxy); proxy = c_iterTakeFirst(proxies); } c_iterFree(proxies); return TRUE; }
/* rot_vec -- apply Givens rotation to x's i & k components */ extern VEC *rot_vec(VEC *x, u_int i, u_int k, double c, double s, VEC *out) { Real temp; if ( x==VNULL ) error(E_NULL,"rot_vec"); if ( i >= x->dim || k >= x->dim ) error(E_RANGE,"rot_vec"); out = v_copy(x,out); /* temp = c*out->ve[i] + s*out->ve[k]; */ temp = c*v_entry(out,i) + s*v_entry(out,k); /* out->ve[k] = -s*out->ve[i] + c*out->ve[k]; */ v_set_val(out,k,-s*v_entry(out,i)+c*v_entry(out,k)); /* out->ve[i] = temp; */ v_set_val(out,i,temp); return (out); }
MAT *Hfactor(MAT *A, VEC *diag, VEC *beta) #endif { STATIC VEC *hh = VNULL, *w = VNULL; int k, limit; if ( ! A || ! diag || ! beta ) error(E_NULL,"Hfactor"); if ( diag->dim < A->m - 1 || beta->dim < A->m - 1 ) error(E_SIZES,"Hfactor"); if ( A->m != A->n ) error(E_SQUARE,"Hfactor"); limit = A->m - 1; hh = v_resize(hh,A->m); w = v_resize(w,A->n); MEM_STAT_REG(hh,TYPE_VEC); MEM_STAT_REG(w, TYPE_VEC); for ( k = 0; k < limit; k++ ) { /* compute the Householder vector hh */ get_col(A,(unsigned int)k,hh); /* printf("the %d'th column = "); v_output(hh); */ hhvec(hh,k+1,&beta->ve[k],hh,&A->me[k+1][k]); /* diag->ve[k] = hh->ve[k+1]; */ v_set_val(diag,k,v_entry(hh,k+1)); /* printf("H/h vector = "); v_output(hh); */ /* printf("from the %d'th entry\n",k+1); */ /* printf("beta = %g\n",beta->ve[k]); */ /* apply Householder operation symmetrically to A */ _hhtrcols(A,k+1,k+1,hh,v_entry(beta,k),w); hhtrrows(A,0 ,k+1,hh,v_entry(beta,k)); /* printf("A = "); m_output(A); */ } #ifdef THREADSAFE V_FREE(hh); V_FREE(w); #endif return (A); }
v_writeResult v_networkReaderEntryWrite( v_networkReaderEntry entry, v_message message, v_networkId writingNetworkId) { v_writeResult result = V_WRITE_SUCCESS; c_bool writeSucceeded; static v_gid zeroAddressee = {0,0,0}; assert(C_TYPECHECK(entry, v_networkReaderEntry)); assert(message != NULL); /* First check if there is any remote interest at all */ if (v_networkReader(v_entry(entry)->reader)->remoteActivity) { /* Only forward messages that come from this kernel */ if (writingNetworkId == V_NETWORKID_LOCAL || entry->isRouting) { /* OK, message is from this kernel or this is a routing entry. Now * attach the correct fields if needed */ /* TODO: For networking services that support routing perhaps * messages shouldn't be forwarded to 'self' (e.g., echo cancellation * may be needed). For R&R this modus is fine. */ writeSucceeded = v_networkReaderWrite( v_networkReader(v_entry(entry)->reader), message, entry, 0, message->writerGID, FALSE /* no p2p */, zeroAddressee); if (writeSucceeded) { result = V_WRITE_SUCCESS; } else { result = V_WRITE_REJECTED; } } } return result; }
void v_networkReaderEntryNotifyConnected( v_networkReaderEntry entry, const c_char* serviceName) { c_bool allChannelsConnected = FALSE; c_mutexLock(&entry->channelCountdownMutex); assert (entry->channelCountdown > 0); entry->channelCountdown--; if (entry->channelCountdown == 0) { allChannelsConnected = TRUE; } c_mutexUnlock(&entry->channelCountdownMutex); if (allChannelsConnected) { v_groupAddEntry(v_group(entry->group), v_entry(entry)); v_groupNotifyAwareness(v_group(entry->group),serviceName,TRUE); v_groupGetHistoricalData(v_group(entry->group), v_entry(entry)); } }
static c_bool v_networkReaderEntryUnSubscribeGroup( v_networkReaderEntry entry, v_group group) { assert(C_TYPECHECK(entry, v_networkReaderEntry)); assert(C_TYPECHECK(group, v_group)); if (v_group(entry->group) == group) { v_groupRemoveEntry(entry->group, v_entry(entry)); } return TRUE; }
static c_bool v_networkReaderEntryUnSubscribe( v_networkReaderEntry entry, v_partition partition) { assert(C_TYPECHECK(entry, v_networkReaderEntry)); assert(C_TYPECHECK(partition, v_partition)); if (v_group(entry->group)->partition == partition) { v_groupRemoveEntry(entry->group, v_entry(entry)); } return TRUE; }
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); }
static c_bool entryFree( c_object o, c_voidp arg) { v_entry entry; assert(o != NULL); assert(arg == NULL); entry = v_entry(o); assert(entry != NULL); v_entryFree(entry); return TRUE; }
v_deliveryServiceEntry v_deliveryServiceEntryNew( v_deliveryService deliveryService, v_topic topic) { v_kernel kernel; v_deliveryServiceEntry e; assert(C_TYPECHECK(deliveryService,v_deliveryService)); assert(C_TYPECHECK(topic,v_topic)); kernel = v_objectKernel(deliveryService); e = v_deliveryServiceEntry(v_objectNew(kernel,K_DELIVERYSERVICEENTRY)); v_entryInit(v_entry(e), v_reader(deliveryService)); e->topic = c_keep(topic); return e; }
static c_bool waitForHistoricalData( c_object o, c_voidp arg) { v_entry entry; c_iter proxies; v_proxy proxy; v_group group; c_time waitTime; struct historicalWaitArg *parms = (struct historicalWaitArg *)arg; assert(o != NULL); assert(arg != NULL); entry = v_entry(o); assert(entry != NULL); assert(C_TYPECHECK(entry,v_entry)); proxies = c_select(entry->groups, 0); proxy = c_iterTakeFirst(proxies); while ((proxy != NULL) && (parms->_status == TRUE)) { group = v_group(v_proxyClaim(proxy)); if (group) { if (group->complete == FALSE) { waitTime = c_timeSub(parms->_expire_time, v_timeGet()); if (c_timeCompare(waitTime, C_TIME_ZERO) == C_GT) { parms->_status = v_groupWaitForComplete(group, waitTime); } else { parms->_status = FALSE; /* time out */ } } v_proxyRelease(proxy); } c_free(proxy); proxy = c_iterTakeFirst(proxies); } c_iterFree(proxies); return parms->_status; }
static c_bool v_networkReaderFindEntry( c_object o, void * walkArg) { struct v_findEntryArg *findEntryArg; v_entry entry; c_bool result = TRUE; entry = v_entry(o); assert(entry); assert(walkArg); if (v_objectKind(entry) == K_NETWORKREADERENTRY) { findEntryArg = (struct v_findEntryArg *)walkArg; if (v_networkReaderEntry(entry)->group == findEntryArg->group) { result = FALSE; findEntryArg->found = v_networkReaderEntry(c_keep(entry)); } } return result; }
static void deliveryServiceSubscribe( void *o, void *arg) { v_partition p = v_partition(o); v_deliveryServiceEntry e = v_deliveryServiceEntry(arg); v_kernel kernel; v_group g; assert(C_TYPECHECK(e,v_deliveryServiceEntry)); assert(C_TYPECHECK(p,v_partition)); kernel = v_objectKernel(e); g = v_groupSetCreate(kernel->groupSet,p,e->topic); if(v_groupPartitionAccessMode(g) == V_ACCESS_MODE_READ_WRITE || v_groupPartitionAccessMode(g) == V_ACCESS_MODE_READ) { v_groupAddEntry(g,v_entry(e)); } c_free(g); }
MAT *makeHQ(MAT *H, VEC *diag, VEC *beta, MAT *Qout) #endif { unsigned int i,/* j,*/ limit; int j; char MatrixTempBuffer[ 2000 ]; /*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); if( SET_VEC_SIZE( H->m ) < 1000 ) { vec_get( &tmp1, (void *)MatrixTempBuffer, H->m ); } else { tmp1 = v_get( H->m ); } if( SET_VEC_SIZE( H->m ) < 1000 ) { vec_get( &tmp2, (void *)(MatrixTempBuffer + 1000), H->m ); } else { tmp2 = v_get( 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 < (int)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 */ if( tmp1 != (VEC *)(MatrixTempBuffer ) ) // память выделялась, надо освободить V_FREE(tmp1); if( tmp2 != (VEC *)(MatrixTempBuffer + 1000) ) // память выделялась, надо освободить V_FREE(tmp2); return (Qout); }
v_deliveryService v_deliveryServiceNew ( v_subscriber subscriber, const c_char *name) { v_kernel kernel; v_deliveryService _this; v_readerQos q; v_topic topic; c_base base; c_type type; v_entry entry, found; assert(C_TYPECHECK(subscriber,v_subscriber)); base = c_getBase(subscriber); kernel = v_objectKernel(subscriber); topic = v_lookupTopic (kernel, V_DELIVERYINFO_NAME); /* ES, dds1576: Before creating the ackreader we have to verify that read * access to the topic is allowed. We can accomplish this by checking the * access mode of the topic. */ if(!topic) { OS_REPORT(OS_ERROR, "v_deliveryServiceNew",0, "DeliveryService not created: " "Could not locate topic with name DCPS_Delivery."); return NULL; } if(v_topicAccessMode(topic) == V_ACCESS_MODE_READ || v_topicAccessMode(topic) == V_ACCESS_MODE_READ_WRITE) { q = v_readerQosNew(kernel,NULL); if (q == NULL) { OS_REPORT(OS_ERROR, "v_deliveryServiceNew", 0, "DeliveryService not created: inconsistent qos"); return NULL; } _this = v_deliveryService(v_objectNew(kernel,K_DELIVERYSERVICE)); type = c_resolve(base, "kernelModule::v_deliveryGuard"); _this->guards = c_tableNew(type, "writerGID.localId"); c_free(type); type = c_resolve(base, "kernelModule::v_subscriptionInfoTemplate"); _this->subscriptions = c_tableNew(type, "userData.key.systemId,userData.key.localId"); c_free(type); q->userKey.enable = TRUE; q->userKey.expression = NULL; v_readerInit(v_reader(_this),name, subscriber,q, NULL, TRUE); c_free(q); entry = v_entry(v_deliveryServiceEntryNew(_this,topic)); found = v_readerAddEntry(v_reader(_this),v_entry(entry)); c_free(entry); c_free(found); v_deliveryServiceEnable(_this); } else { OS_REPORT_1(OS_ERROR, "v_deliveryServiceNew", 0, "Creation of DeliveryService <%s> failed. Topic DCPS_Delivery." "does not have read access rights.", name); _this = NULL; } return _this; }
/* BKPsolve -- solves A.x = b where A has been factored a la BKPfactor() -- returns x, which is created if NULL */ extern VEC *BKPsolve(MAT *A, PERM *pivot, PERM *block, VEC *b, VEC *x) { static VEC *tmp=VNULL; /* dummy storage needed */ int i, j, n, onebyone; Real **A_me, a11, a12, a22, b1, b2, det, sum, *tmp_ve, tmp_diag; if ( ! A || ! pivot || ! block || ! b ) error(E_NULL,"BKPsolve"); if ( A->m != A->n ) error(E_SQUARE,"BKPsolve"); n = A->n; if ( b->dim != n || pivot->size != n || block->size != n ) error(E_SIZES,"BKPsolve"); x = v_resize(x,n); tmp = v_resize(tmp,n); MEM_STAT_REG(tmp,TYPE_VEC); A_me = A->me; tmp_ve = tmp->ve; px_vec(pivot,b,tmp); /* solve for lower triangular part */ for ( i = 0; i < n; i++ ) { sum = v_entry(tmp,i); if ( block->pe[i] < i ) for ( j = 0; j < i-1; j++ ) sum -= m_entry(A,i,j)*v_entry(tmp,j); else for ( j = 0; j < i; j++ ) sum -= m_entry(A,i,j)*v_entry(tmp,j); v_set_val(tmp,i,sum); } /* printf("# BKPsolve: solving L part: tmp =\n"); v_output(tmp); */ /* solve for diagonal part */ for ( i = 0; i < n; i = onebyone ? i+1 : i+2 ) { onebyone = ( block->pe[i] == i ); if ( onebyone ) { tmp_diag = m_entry(A,i,i); if ( tmp_diag == 0.0 ) error(E_SING,"BKPsolve"); /* tmp_ve[i] /= tmp_diag; */ v_set_val(tmp,i,v_entry(tmp,i) / tmp_diag); } else { a11 = m_entry(A,i,i); a22 = m_entry(A,i+1,i+1); a12 = m_entry(A,i+1,i); b1 = v_entry(tmp,i); b2 = v_entry(tmp,i+1); det = a11*a22-a12*a12; /* < 0 : see BKPfactor() */ if ( det == 0.0 ) error(E_SING,"BKPsolve"); det = 1/det; v_set_val(tmp,i,det*(a22*b1-a12*b2)); v_set_val(tmp,i+1,det*(a11*b2-a12*b1)); } } /* printf("# BKPsolve: solving D part: tmp =\n"); v_output(tmp); */ /* solve for transpose of lower traingular part */ for ( i = n-1; i >= 0; i-- ) { /* use symmetry of factored form to get stride 1 */ sum = v_entry(tmp,i); if ( block->pe[i] > i ) for ( j = i+2; j < n; j++ ) sum -= m_entry(A,i,j)*v_entry(tmp,j); else for ( j = i+1; j < n; j++ ) sum -= m_entry(A,i,j)*v_entry(tmp,j); v_set_val(tmp,i,sum); } /* printf("# BKPsolve: solving L^T part: tmp =\n");v_output(tmp); */ /* and do final permutation */ x = pxinv_vec(pivot,tmp,x); return x; }
MAT *Hfactor(MAT *A, VEC *diag, VEC *beta) #endif { char MatrixTempBuffer[ 2000 ]; /*STATIC */VEC *hh = VNULL, *w = VNULL; int k, limit; if ( ! A || ! diag || ! beta ) error(E_NULL,"Hfactor"); if ( diag->dim < A->m - 1 || beta->dim < A->m - 1 ) error(E_SIZES,"Hfactor"); if ( A->m != A->n ) error(E_SQUARE,"Hfactor"); limit = A->m - 1; if( SET_VEC_SIZE( A->m ) < 1000 ) { vec_get( &hh, (void *)MatrixTempBuffer, A->m ); } else { hh = v_get( A->m ); } if( SET_VEC_SIZE( A->n ) < 1000 ) { vec_get( &w, (void *)(MatrixTempBuffer + 1000), A->n ); } else { w = v_get( A->n ); } /*hh = v_resize(hh,A->m); w = v_resize(w,A->n); MEM_STAT_REG(hh,TYPE_VEC); MEM_STAT_REG(w, TYPE_VEC);*/ for ( k = 0; k < limit; k++ ) { /* compute the Householder vector hh */ get_col(A,(unsigned int)k,hh); /* printf("the %d'th column = "); v_output(hh); */ hhvec(hh,k+1,&beta->ve[k],hh,&A->me[k+1][k]); /* diag->ve[k] = hh->ve[k+1]; */ v_set_val(diag,k,v_entry(hh,k+1)); /* printf("H/h vector = "); v_output(hh); */ /* printf("from the %d'th entry\n",k+1); */ /* printf("beta = %g\n",beta->ve[k]); */ /* apply Householder operation symmetrically to A */ _hhtrcols(A,k+1,k+1,hh,v_entry(beta,k),w); hhtrrows(A,0 ,k+1,hh,v_entry(beta,k)); /* printf("A = "); m_output(A); */ } /* #ifdef THREADSAFE V_FREE(hh); V_FREE(w); #endif */ if( hh != (VEC *)(MatrixTempBuffer ) ) // память выделялась, надо освободить V_FREE(hh); if( w != (VEC *)(MatrixTempBuffer + 1000) ) // память выделялась, надо освободить V_FREE(w); return (A); }