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);
}
