void NeuralNetwork::Propagate(FloatsVector& inputs, FloatsVector& outputs, FloatsVector& hiddenLayerValues)
{
assert(inputs.size() == inputNeuronCount);
outputs.resize(outputNeuronCount);
for (int j = 0; j < hiddenNeuronCount; j++) {
for (int i = 0; i < inputNeuronCount; i++)
{
hiddenLayerValues[j] += inputWeights[j * inputNeuronCount + i] * inputs[i];
}
}
for (int j = 0; j < hiddenNeuronCount; j++) {
hiddenLayerValues[j] = logistic(hiddenLayerValues[j]);
}
for (int k = 0; k < outputNeuronCount; k++)
{
for (int j = 0; j < hiddenNeuronCount; j++)
{
outputs[k] += outputWeights[k * hiddenNeuronCount + j] * hiddenLayerValues[j];
}
}
for (int j = 0; j < outputNeuronCount; j++)
{
outputs[j] = logistic(outputs[j]);
}
return;
}
void
compute_output(void)
{
int i, j;
float netinp, sum, omax;
for (i = NUM_IN; i < NUM_IN + NUM_HID; i++)
{
netinp = bias[i];
for (j = fwt_to[i]; j < lwt_to[i]; j++)
netinp += activation[j] * weight[i][j];
/* Trigger neuron */
activation[i] = logistic(netinp);
}
for (i = NUM_IN + NUM_HID; i < TOTAL; i++)
{
netinp = bias[i];
for (j = fwt_to[i]; j < lwt_to[i]; j++)
netinp += activation[j] * weight[i][j];
/* Trigger neuron */
activation[i] = logistic(netinp);
}
}
/*
* Penalizes max jerk.
*/
double max_jerk_cost(struct trajectory* traj, int8_t target_vehicle, struct state* delta, double T, std::map<int8_t, Vehicle>* predictions, bool verbose)
{
double cost = 0.0;
double* s = traj->s_coeffs;
double t = traj->T;
double s_dot[5];
differentiate(s_dot, s, 5);
double s_d_dot[4];
differentiate(s_d_dot, s_dot, 4);
double jerk[3];
differentiate(jerk, s_d_dot, 3);
std::vector<double> all_jerks;
for (uint8_t i = 0; i < 100; i++)
{
all_jerks.push_back(calculate(jerk, T/100 * i, 4));
}
double max_jerk = *std::max_element(all_jerks.begin(), all_jerks.end());
if (abs(max_jerk) > MAX_JERK)
{
cost = 1.0;
}
else
{
cost = logistic(MAX_JERK/(MAX_JERK-abs(max_jerk)));
}
if (verbose == true)
{
printf("[COST][INFO] %24s : %f\n", __func__, cost);
}
return cost;
}
/*
* Penalizes total jerk.
*/
double total_jerk_cost(struct trajectory* traj, int8_t target_vehicle, struct state* delta, double T, std::map<int8_t, Vehicle>* predictions, bool verbose)
{
double cost = 0.0;
double* s = traj->s_coeffs;
double t = traj->T;
double s_dot[5];
differentiate(s_dot, s, 5);
double s_d_dot[4];
differentiate(s_d_dot, s_dot, 4);
double jerk[3];
differentiate(jerk, s_d_dot, 3);
double total_jerk = 0.0;
double dt = T / 100.0;
double j = 0.0;
for (uint8_t i = 0; i < 100; i++)
{
t = dt * i;
j = calculate(jerk, t, 3);
total_jerk += abs(j*dt);
}
double jerk_per_second = total_jerk / T;
cost = logistic(jerk_per_second / EXPECTED_JERK_IN_ONE_SEC );
if (verbose == true)
{
printf("[COST][INFO] %24s : %f\n", __func__, cost);
}
return cost;
}
/*
* Penalizes trajectories whose d coordinate (and derivatives)
* differ from the goal.
*/
double d_diff_cost(struct trajectory* traj, int8_t target_vehicle, struct state* delta, double T, std::map<int8_t, Vehicle>* predictions, bool verbose)
{
double cost = 0.0;
double* d = traj->d_coeffs;
double t = traj->T;
struct state target;
if (target_vehicle > -1)
{
target = (*predictions)[target_vehicle].state_in(t);
}
target.add(delta);
double* d_targ = (double*)&target.d;
double D[3];
get_f_and_N_derivatives(D, d, 2, t);
for (uint8_t i = 0; i < 3; i++)
{
double actual = D[i];
double expected = d_targ[i];
double sigma = SIGMA_D[i];
double diff = double(abs(actual-expected));
cost += logistic(diff/sigma);
}
cost /= 3.0;
if (verbose == true)
{
printf("[COST][INFO] %24s : %f\n", __func__, cost);
}
return cost;
}
void refineBiasNegative(double *mPostB, double *vPostB, double vPriorB,
double mPriorB, double mPred, double vPred, int T, double rho,
int *nZerosSampling, double *e0) {
double eta1New, eta2New, eta1Prior, eta2Prior, eta1Post, eta2Post,
zeta, e;
zeta = sqrt(mPred * mPred + vPred);
e = logistic(mPred + log(*e0 / (1 - *e0)));
mPred -= *mPostB;
eta2Post = 1 / *vPostB;
eta1Post = *mPostB / *vPostB;
eta2Prior <- 1 / vPriorB;
eta1Prior <- mPriorB / vPriorB;
eta2New = eta2Prior - 4 * nZerosSampling[ T - 1 ] * lambda(zeta);
eta1New = -(1 - 2 * e) * nZerosSampling[ T - 1 ] + 4 * lambda(zeta) *
nZerosSampling[ T - 1 ] * mPred + eta1Prior;
/* We update the posterior mean and variance */
eta2Post = (1 - rho) * eta2Post + rho * eta2New;
eta1Post = (1 - rho) * eta1Post + rho * eta1New;
*vPostB = 1 / eta2Post;
*mPostB = eta1Post / eta2Post;
}
void Node::ComputeActivation(bool debugPrint)
{
// activation of bias nodes remains at the initialized constant
// of 1.
if (isBiasNode)
return;
std::vector<Node *> *inNodes = inLayer->GetNodes();
unsigned int numInNodes = inNodes->size();
linearWeightedSummation = 0;
//if (debugPrint) printf("\tweighted sum: logistic(\n");
for (unsigned int i=0; i<numInNodes; i++) {
/*if (debugPrint) {
if (i<20) {
printf("[%u] %f*%f%s\n", i, (*inNodes)[i]->GetActivationVal(),
synapseLinks[(*inNodes)[i]],
i==numInNodes-1?"":"+"
);
}
}*/
linearWeightedSummation +=
(*inNodes)[i]->GetActivationVal() *
synapseLinks[(*inNodes)[i]];
}
activationVal = logistic(linearWeightedSummation);
if (debugPrint) fprintf(stderr, " ... )=%f\n", activationVal);
}
void refineRowUNegative(double *mPostU, double *vPostU, double *mPostV,
double *vPostV, double *vPriorUi, double *vPriorUh, double *mPriorU,
double *mPred, double *vPred, int i, int j, int T, int P,
int k, double *e0, int *nOnesSampling, int *nZerosSampling,
int *nOnesAuxi, int *nZerosAuxi, double rho) {
int h;
double eta1New, eta2New, eta1Old, eta2Old, eta1Prior, eta2Prior,
eta1Post, eta2Post, zeta, reps, vNew, e;
/* We update the posterior approx. for the i-th row of U */
for (h = 0 ; h < k ; h++) {
zeta = sqrt((*mPred) * (*mPred) + *vPred);
e = logistic((*mPred) + log(*e0 / (1 - *e0)));
*mPred -= mPostU[ i + h * T ] * mPostV[ j + h * P ];
*vPred -= mPostU[ i + h * T ] * mPostU[ i + h * T ] *
vPostV[ j + h * P ] + vPostU[ i + h * T ] *
mPostV[ j + h * P ] * mPostV[ j + h * P ] +
vPostU[ i + h * T ] * vPostV[ j + h * P ];
eta2Prior = 1 / (vPriorUi[ i ] * vPriorUh[ h ]);
eta1Prior = mPriorU[ h ] / (vPriorUi[ i ] * vPriorUh[ h ]);
eta2Post = 1 / vPostU[ i + h * T ];
eta1Post = mPostU[ i + h * T ] / vPostU[ i + h * T ];
reps = nZerosAuxi[ i ] + nOnesAuxi[ i ] *
(double) nZerosSampling[ T - 1 ] /
nOnesSampling[ T - 1 ];
eta2New = eta2Prior - 2 * reps * lambda(zeta) *
(mPostV[ j + h * P ] * mPostV[ j + h * P ] +
vPostV[ j + h * P ]);
eta1New = -(1 - 2 * e) * mPostV[ j + h * P ] * reps / 2 + 2 *
lambda(zeta) * reps * mPostV[ j + h * P ] *
(*mPred) + eta1Prior;
/* We update the posterior mean and variance */
eta2Post = (1 - rho) * eta2Post + rho * eta2New;
eta1Post = (1 - rho) * eta1Post + rho * eta1New;
vPostU[ i + h * T ] = 1 / eta2Post;
mPostU[ i + h * T ] = eta1Post / eta2Post;
/* We update the predictive mean and variance */
*mPred += mPostU[ i + h * T ] * mPostV[ j + h * P ];
*vPred += mPostU[ i + h * T ] * mPostU[ i + h * T ] *
vPostV[ j + h * P ] + vPostU[ i + h * T ] *
mPostV[ j + h * P ] * mPostV[ j + h * P ] +
vPostU[ i + h * T ] * vPostV[ j + h * P ];
}
}
void update(const int max_iter) {
std::random_shuffle(examples.begin(), examples.end());
for (int iter = 0; iter < max_iter; iter++) {
std::vector<std::pair<fv_t, int> >::iterator it = examples.begin();
for(; it != examples.end(); it++) {
fv_t fv = it->first; int y = it->second;
logistic(fv, y, eta);
}
l1_regularize(iter);
std::cerr << w.size() << std::endl;
}
};
void ComputeFeedForwardSignals(double* MAT_INOUT,double* V_IN,double* V_OUT, double* V_BIAS,int size1,int size2,int layer)
{
int row,col;
for(row=0;row < size2; row++)
{
V_OUT[row]=0.0;
for(col=0;col<size1;col++)V_OUT[row]+=(*(MAT_INOUT+(row*size1)+col)*V_IN[col]);
V_OUT[row]+=V_BIAS[row];
if(layer==0) V_OUT[row] = exp(V_OUT[row]);
if(layer==1) V_OUT[row] = logistic(V_OUT[row]);
}
}
/*
* Penalizes getting close to other vehicles.
*/
double buffer_cost(struct trajectory* traj, int8_t target_vehicle, struct state* delta, double T, std::map<int8_t, Vehicle>* predictions, bool verbose)
{
double cost = 0.0;
double nearest = nearest_approach_to_any_vehicle(traj, predictions);
cost = logistic(2*VEHICLE_RADIUS / nearest);
if (verbose == true)
{
printf("[COST][INFO] %24s : %f\n", __func__, cost);
}
return cost;
}
/*
* Penalizes trajectories that span a duration which is longer or
* shorter than the duration requested.
*/
double time_diff_cost(struct trajectory* traj, int8_t target_vehicle, struct state* delta, double T, std::map<int8_t, Vehicle>* predictions, bool verbose)
{
double cost = 0.0;
double t = traj->T;
cost = logistic(double(abs(t-T)) / T);
if (verbose == true)
{
printf("[COST][INFO] %24s : %f\n", __func__, cost);
}
return cost;
}
void
ACS::dynamics(const FDouble & v, const double & t) const
{
(void)t;
mInf = pow(scaleLogistic(mCoef * exp((v + mOffsetF) / mScaleF),
-(v + mOffset2F)/ mScale2F), 1.0/3.0);
if(!(mInf < 1.0))
mInf = 1.0;
mTau = mTauAF + scaleLogistic(mTauBF, -(v + mTauOffsetF) / mTauVScaleF);
hInf = power<4>( logistic(-(v + hOffsetF) / hScaleF) );
hTau = hTauAF + scaleLogistic(hTauBF, -(v + hTauOffsetF) / hTauVScaleF);
}
/*
* Penalizes exceeds speed limit.
*/
double exceeds_speed_limit_cost(struct trajectory* traj, int8_t target_vehicle, struct state* delta, double T, std::map<int8_t, Vehicle>* predictions, bool verbose)
{
double cost = 0.0;
double* s = traj->s_coeffs;
double t = traj->T;
double s_dot[5];
differentiate(s_dot, s, 5);
double speed = calculate(s_dot, t, 5);
if (speed > SPEED_LIMIT/2.24)
{
cost = 1.0 + logistic((speed-SPEED_LIMIT/2.24)/(SPEED_LIMIT/2.24));;
}
else
{
cost = logistic((SPEED_LIMIT/2.24-speed)/(SPEED_LIMIT/2.24));
}
if (verbose == true)
{
printf("[COST][INFO] %24s : %f\n", __func__, cost);
}
return cost;
}
/*
* Rewards high average speeds.
*/
double efficiency_cost(struct trajectory* traj, int8_t target_vehicle, struct state* delta, double T, std::map<int8_t, Vehicle>* predictions, bool verbose)
{
double cost = 0.0;
double* s = traj->s_coeffs;
double t = traj->T;
double avg_v = double(calculate(s, t)) / t;
struct state curr_state = (*predictions)[target_vehicle].state_in(t);
double targ_s = curr_state.s.m;
double targ_v = double(targ_s) / t;
cost = logistic(2*double(targ_v - avg_v) / avg_v);
if (verbose == true)
{
printf("[COST][INFO] %24s : %f\n", __func__, cost);
}
return cost;
}
void Utility::bezierCurve(const std::vector<Ravelin::Vector3d>& control_points, int num_segments,
std::vector<Ravelin::Vector3d>& trajectory,
std::vector<Ravelin::Vector3d> & dtrajectory,
std::vector<Ravelin::Vector3d> & ddtrajectory){
trajectory.resize(num_segments);
dtrajectory.resize(num_segments);
ddtrajectory.resize(num_segments);
double u_last,t_last;
for(int t = 0;t < num_segments; t++) {
Ravelin::Vector3d u = logistic((double)t/(double)num_segments,10,0.5);
evalBernstein(control_points[0], control_points[1], control_points[2], control_points[3], u[0],trajectory[t],dtrajectory[t],ddtrajectory[t]);
for(int i=0;i<3;i++){
if(!std::isfinite(trajectory[t][i]))
trajectory[t][i] = trajectory[t-1][i];
if(!std::isfinite(dtrajectory[t][i]))
dtrajectory[t][i] = dtrajectory[t-1][i];
if(!std::isfinite(ddtrajectory[t][i]))
ddtrajectory[t][i] = ddtrajectory[t-1][i];
}
ddtrajectory[t] = ddtrajectory[t]*u[1]*u[1] + dtrajectory[t]*u[2];
dtrajectory[t] *= u[1];
}
}
//component-wise activation using logistic function
void activate_logistic(float * x, int dim){
for(int i=0; i<dim; i++)
x[i] = logistic(x[i]);
}
static double compute_ev_det_loglike(const Physics_t * physics,
int num_sta, const Station_t * sta_list,
const Event_t * event,
const Detection_t * det)
/* returns the log likelihood that the event generated that given detection */
{
/* first compute basic event-station attributes like distance,
* travel time, azimuth, and azimuth difference
*/
int staidx = det->staidx;
const Station_t * sta = sta_list + staidx;
double dist = compute_distance(sta->lon, sta->lat, event->lon, event->lat);
double ttime = compute_travel_time(dist);
double sta_to_ev_az = compute_azimuth(sta->lon, sta->lat, event->lon,
event->lat);
/* the azimuth difference of observed vs. theoretical */
double degdiff = compute_degdiff(sta_to_ev_az, det->azimuth);
double loglike = 0;
/* detection probability */
double detprob = logistic(physics->mu_d0[staidx]
+ physics->mu_d1[staidx] * event->mag
+ physics->mu_d2[staidx] * dist);
loglike += log(detprob);
/* detection time */
loglike += laplace_logpdf(det->time,
event->time + ttime + physics->mu_t[staidx],
physics->theta_t[staidx]);
/* detection azimuth */
loglike += laplace_logpdf(degdiff, physics->mu_z[staidx],
physics->theta_z[staidx]);
/* detection slowness */
loglike += laplace_logpdf(det->slowness,
compute_slowness(dist) + physics->mu_s[staidx],
physics->theta_s[staidx]);
/* detection amplitude */
loglike += norm_logpdf(det->logamp,
physics->mu_a0[staidx]
+ physics->mu_a1[staidx] * event->mag
+ physics->mu_a2[staidx] * dist,
physics->sigma_a[staidx]);
return loglike;
}
double lambda(double zeta) {
return (0.5 - logistic(zeta)) / (2 * zeta);
}
void Test :: run(){
//Test frand
// assert( frand()==0.1f );
// assert( frand()==0.7f );
// assert( frand()==0.7f );
// assert( frand()==0.1f );
// cout << "gismoManager::randmom() is OK."<<endl;
cout << "CLASS Sound is ok.(check the receive yourself.)" << endl;
//TestEventHandler
EventHandler eventHandler;
EvTest evTest;
eventHandler.eventAdd("/t01" , &evTest);
int args[] = {0,1,2};
assert ( eventHandler.bang("/t01", args) == 138 );
assert ( eventHandler.bang("/t01") == 137 );
cout << "GismoBundledClass::eventHandler is OK." << endl;
//Test EventHandler with Gismo
gismo.eventAdd("/t01" , &evTest);
assert ( gismo.bang("/t01" , args) == 138 );
//Sound Trigger
int snd_id = 0;
setSound(0);
setSound(2);
setSound(4);
cout << "GismoManager::eventHandler with Gismo is OK." << endl;
//Define an agent
ag_t ag;
//Test GismoManager.getAgents()
ag_t *agents = gismo.getAgents();
agents[0].posi.x = 0.13f;
agents[0].posi.y = 0.2f;
assert(agents[0].posi.x == 0.13f);
assert(agents[0].posi.y == 0.2f);
cout << "GismoManager:getAgent() is OK." << endl;
//Test GismoLibrary distance()
posi_t tmp1, tmp2;
tmp1.x = 0.0f;
tmp1.y = 0.0f;
tmp2.x = 3.0f;
tmp2.y = 4.0f;
assert(distance(tmp1, tmp2) == 5.0f);
tmp1.x = 0.5f;
tmp1.y = 0.5f;
tmp2.x = -0.5f;
tmp2.y = -0.5f;
assert(distance(tmp1, tmp2)==(float)sqrt(2.0f));
tmp1.x = 3.0f;
tmp1.y = 4.0f;
tmp2.x = 0.0f;
tmp2.y = 0.0f;
assert(distance(tmp1, tmp2) == 5.0f);
tmp1.x = 5.0f;
tmp1.y = 12.0f;
tmp2.x = 0.0f;
tmp2.y = 0.0f;
assert(distance(tmp1, tmp2) == 13.0f);
cout << "GismoLibrary:distance() is OK." << endl;
//Test Init AgentActive
initAgentActive(&ag);
assert(ag.size == AG_DEF_SIZE);
assert(ag.active==true);
cout << "GismoLibrary:initAgentActive() is OK." << endl;
//TestAgentAdd
initAgentActive(&ag);
ag.view = 0.23f;
ag.posi.x = 0.2f; ag.posi.y=0.2f;
gismo.addAgent(ag);
assert (gismo.add.buf[0].view == 0.23f);
assert (gismo.add.count == 1);
cout << "GismoManager:addAgent() is OK." << endl;
//TestSync
ag_t ag2;
initAgentActive(&ag2);
ag2.view = 0.34f;
gismo.addAgent(ag2);
gismo.addSync(); //Finally gismo requires sync to avoid direct agent addition when processing agents.
assert(gismo.add.count==0 && gismo.agents.count==2);
assert(agents[0].active && agents[1].active);
assert(agents[0].view==0.23f && agents[1].view==0.34f);
cout << "gismoLibrary:addSync() is OK." << endl;
//Test gismo Library seekNearest();
agents[0].posi.x = 0.0f;
agents[0].posi.y = 0.0f;
agents[1].posi.x = 0.5f;
agents[1].posi.y = 0.5f;
ag_t ag3;
initAgentActive(&ag3);
ag3.posi.x = 0.5f;
ag3.posi.y = 0.49f;
gismo.addAgent(ag3); //add the new agent to addBuffer
ag_t ag4;
ag4.posi.x = 0.7f;
ag4.posi.y = 0.49f;
gismo.addAgent(ag3); //add the new agent to addBuffer
gismo.addSync(); //refrect the add buffer to actual buffer
int nearest_agent = seekNearest(0, &gismo.agents); //seek the nearest agent of agent[0]
cout << nearest_agent << endl;
assert(nearest_agent==2);
cout << "gismoManager:seekNearest() is OK."<<endl;
//Test isViewRange
/* REST FOR VIEW/MOV RATE
ag_t ag5;
ag5.view = 0.5f;
assert( isViewRange(&ag5,0.3f)==true );
assert( isViewRange(&ag5,0.51f)==false );
cout << "gismoLibrary::isViewRange() is OK" << endl;
//Test isLarge
assert( isLarge(0.5 , 0.4)==true );
assert( isLarge(0.5, 0.501)==false);
cout << "gismoLibrary::isaLarge is OK" <<endl;
//Test Move
ag_t ag6;
posi_t tmp;
tmp.x=1.0; tmp.y=0.0;
initAgent(&ag6);
ag6.posi.x=0.5; ag6.posi.y=0.5;
move(&ag6,&tmp);
assert(ag6.posi.x >= 0.5f);
assert(ag6.posi.y <= 0.5f);
cout << "gismoLibrary::move() is OK." << endl;
//Test Run
ag_t tmpAg1, tmpAg2;
tmpAg1.posi.x = 0.75f;
tmpAg1.posi.y = 0.75f;
tmpAg1.mov = 0.001f;
tmpAg1.spd.x = 0.0f;
tmpAg1.spd.y = 0.0f;
tmpAg2.posi.x = 0.5f;
tmpAg2.posi.y = 0.5f;
running(&tmpAg1, &tmpAg2.posi);
assert(tmpAg1.posi.x >= 0.75f);
assert(tmpAg1.posi.y >= 0.75f);
tmpAg1.posi.x = 0.25f;
tmpAg1.posi.y = 0.75f;
tmpAg1.spd.x = 0.0f;
tmpAg1.spd.y = 0.0f;
running(&tmpAg1, &tmpAg2.posi);
assert(tmpAg1.posi.x <= 0.25f);
assert(tmpAg1.posi.y >= 0.75f);
tmpAg1.posi.x = 0.75f;
tmpAg1.posi.y = 0.45f;
tmpAg1.spd.x = 0.0f;
tmpAg1.spd.y = 0.0f;
running(&tmpAg1, &tmpAg2.posi);
assert(tmpAg1.posi.x >= 0.75f);
assert(tmpAg1.posi.y <= 0.45f);
tmpAg1.posi.x = 0.25f;
tmpAg1.posi.y = 0.25f;
tmpAg1.spd.x = 0.0f;
tmpAg1.spd.y = 0.0f;
running(&tmpAg1, &tmpAg2.posi);
//0115 assert(tmpAg1.posi.x < 0.25f);
//assert(tmpAg1.posi.y < 0.25f);
cout << "gismoLibrary::running() is OK." << endl;
//TestConditionCheck
condition_e cond1 = CALM;
condition_e cond2 = RUN;
assert ( conditionCheck(cond1, cond2) == false );
cond2 = CALM;
assert ( conditionCheck(cond1, cond2) == true );
//Test interactWith()
ag_t ag8 , ag9;
initAgent(&ag8);
initAgent(&ag9);
ag8.size = 1.0f;
ag8.posi.x = 0.0f; ag8.posi.y = 0.0f;
ag9.posi.x = 1.0f; ag9.posi.y = 1.0f;
ag8.view = 1.5;
interactWith(&ag8 , &ag9);
*/
//TestReset
agents[0].active=true;
agents[1].active=true;
agBuffReset(&gismo.agents);
assert(agents[0].active==false);
assert(agents[1].active==false);
assert(gismo.agents.count == 0);
//TestLogistic
float fval=0.5;
fval = logistic(fval);
assert(fval==0.75f);
fval = logistic(fval);
cout << "GismoLibrary::logistic() is OK." << endl;
//Test
//agBuffReset(&gismo.agents);
int val = 1;
gismo.bang("/gismo/reset" , &val);
ag_t ag1;
initAgent(&ag1);
ag1.view = 256.0f;
gismo.agents.buf[0] = ag1;
ag1.view = 356.0f;
gismo.agents.buf[1] = ag1;
ag_t *pAg1 = gismo.getAgent(0);
assert(pAg1->view == 256.0f);
ag_t *pAg2 = gismo.getAgent(1);
assert(pAg2->view == 356.0f);
//TestSpeedLimit
assert ( limitter(1.1f, 1.0f) == 1.0f );
assert ( limitter(-1.1f, 1.0f) == -1.0f );
assert ( limitter(0.49f, 0.5f) == 0.49f );
assert ( limitter(-0.49f, 0.5f) == -0.49f );
assert ( limitter(0.0051f, 0.005f) == 0.005f );
assert ( limitter(-0.00501f, 0.005f) == -0.005f );
cout << "speedLimitter is OK." << endl;
//Test positionLoop()
posi_t pos;
pos.x = 1.1; pos.y = -0.01;
positionLoop(&pos, 1.0f, 1.0f);
assert (pos.x == 0.0f);
assert (pos.y == 1.0f);
pos.x = -0.1; pos.y = 1.4;
positionLoop(&pos, 1.0f, 1.0f);
assert (pos.x == 1.0f);
assert (pos.y == 0.0f);
pos.x = 0.0f; pos.y = 1.0f;
positionLoop(&pos, 1.0f, 1.0f);
assert (pos.x == 1.0f);
assert (pos.y == 1.0f);
//Check result check
pos.x = 0.5f;
pos.y = 0.5f;
bool result = positionLoop(&pos , 1.0f, 1.0f);
assert (result==false);
pos.x = 1.0f;
pos.y = 1.0f;
result = positionLoop(&pos , 1.0f, 1.0f);
assert (result==false);
pos.x = 1.05f;
pos.y = 1.05f;
result = positionLoop(&pos , 1.0f, 1.0f);
assert (result==true);
pos.x = 1.05f;
pos.y = 1.00f;
result = positionLoop(&pos , 1.0f, 1.0f);
assert (result==true);
cout << "GismoLibrary::positionLoop() is OK" << endl;
//Test attackCheck
float fval2 = 0.0f;
float size2 = 1.0f;
attackCheck(fval2, &size2);
bool size_test2 = true;
if ( size2 != (1.0f-AG_DMG) ) size_test2 = false;
assert(size_test2);
assert(size2 == 1.0f-AG_DMG);
size2 = 1.0f;
fval2 = ATK_DIST+0.1;
attackCheck(fval2, &size2);
assert(size2 == 1.0f);
cout << "GismoLibrary::attackCheck() is OK" << endl;
size2 = 1.0f;
fval2 = ATK_DIST;
attackCheck(fval2, &size2);
assert(size2 == 1.0f-AG_DMG);
//Test deadCheck
float dummy_size = 0.0001f;
bool active = true;
deadCheck( &dummy_size , &active );
assert(active == false);
assert(dummy_size == 0.0f);
dummy_size = 1.0f;
active = true;
deadCheck( &dummy_size , &active );
assert(active == true);
cout << "GismoLibrary::deadCheck is OK" << endl;
//Test Shape2Agent
ag_shape_t shape;
shape.nodes[0].x = 0.5f;
shape.nodes[0].y = 0.5f;
shape.nodes[1].x = 1.0f;
shape.nodes[1].y = 1.0f;
shape.node_count = 2;
shape.edges[0].node_id_a = 0;
shape.edges[0].node_id_b = 1;
shape.edge_count = 1;
ag_t tmpAg = shape2Agent(shape);
assert(tmpAg.view == 0.005f);
//assert(tmpAg.size == 0.011f);
assert(tmpAg.mov == 0.35f);
ag_shape_t shape2;
shape2.node_count = 50000;
ag_t tmpAg3 = shape2Agent(shape2);
cout << tmpAg3.mov << endl;
assert(tmpAg3.mov == MOV_MINIMUM);
cout << "Shape2Agent.hpp::shape2Agent() is OK" << endl;
//Test moveOnLine()
posi_t posi = moveOnLine(0.5f, 0.0f, 0.0f, 1.0f, 1.0f);
assert(posi.x == 0.5f && posi.y == 0.5f);
posi = moveOnLine(1.0f, 0.0f, 0.0f, 1.0f, 1.0f);
assert(posi.x == 1.0f && posi.y == 1.0f);
posi = moveOnLine(0.5f, -1.0f, -1.0f, -2.0f, -2.0f);
assert(posi.x == -1.5f && posi.y == -1.5f);
posi = moveOnLine(0.0f, -1.0f, -1.0f, -2.0f, -2.0f);
assert(posi.x == -1.0f && posi.y == -1.0f);
posi = moveOnLine(0.5f, 0.0f, 0.0f, -1.0f, -2.0f);
assert(posi.x == -0.5f && posi.y == -1.0f);
//TestGetArraySize
int iArray[137];
posi_t posiArray[138];
assert(getArraySize(iArray)==137);
assert(getArraySize(posiArray)==138);
cout << "TestGetArraySize.h::getArraySize() is ok." << endl;
//Test lambda bang
int myArg[2];
myArg[0] = 12;
myArg[1] = 13;
gismo.bang("/lambdaTest", myArg);
//TestSoundTrigger
param_u params[4];
params[0].ival = 0; //Genre
params[1].ival = 1; //Song
params[2].ival = 2; //Slice
params[3].fval = 1.0f;//effect
gismo.bang("/soundTrg" , ¶ms);
cout << "sound trigger is ok. If you could listen the RM sound" << endl;
//Test shape2Sound
ag_shape_t shapeForSound;
shapeForSound.node_count = 12;
shapeForSound.color = 0.50f;
sound_t snd = shape2sound(shapeForSound,5);
assert(snd.genre == 2);
assert(snd.song == 5);
shapeForSound.node_count = 6;
shapeForSound.color = 1.0f;
snd = shape2sound(shapeForSound,1137);
assert(snd.genre == 0);
assert(snd.song == 137);
cout << "SoundTrigger::shape2Sound is OK" << endl;
ag_t test;
test.condition = CALM;
ag2sound(&test, &snd);
assert(snd.slice == 0);
assert(snd.effect_val == EF_VAL_CALM);
test.condition = RUN;
ag2sound(&test, &snd);
assert(snd.slice == 1);
assert(snd.effect_val == EF_VAL_RUN);
test.condition = CHASE;
ag2sound(&test, &snd);
assert(snd.slice == 2);
assert(snd.effect_val == EF_VAL_CHASE);
test.condition = DMG;
ag2sound(&test, &snd);
assert(snd.slice == 3);
assert(snd.effect_val == EF_VAL_DMG);
test.condition = DEATH;
ag2sound(&test, &snd);
assert(snd.slice == 4);
assert(snd.effect_val == EF_VAL_DEATH);
cout << "SoundTrigger::ag2Sound is OK" << endl;
//Test makePositionToAdd in Shape2Agent
// posi_t myPosi = makePositionToAdd();
// assert(myPosi.x == )
//
}
// compute P(H_{i} = 1 | v)
// wvc is the sum of wv and c - the visible bias
void compute_probability_hidden_given_visible(float * wvc, float * p, int n){
for (int i=0; i<n; i++){
p[i] = logistic(wvc[i]);
}
}
// compute P(V_{j} = 1 | h)
// dim(whb) = m x 1
void compute_probability_visible_given_hidden(float * whb, float * p, int m){
for (int j=0; j<m; j++){
p[j] = logistic(whb[j]);
}
}
void refineRowVNegative(double *mPostU, double *vPostU, double *mPostV,
double *vPostV, double *vPriorVj, double *vPriorVh, double *mPriorV,
double *mPred, double *vPred, int i, int j, int T, int P,
int k, double *e0, int *nOnesSampling, int *nZerosSampling,
int *nOnesAuxj, int *nZerosAuxj, double rho) {
int h;
double eta1New, eta2New, eta1Old, eta2Old, eta1Prior, eta2Prior,
eta1Post, eta2Post, zeta, reps, vNew, e;
/* We update the posterior approx. for the i-th row of U */
for (h = 0 ; h < k ; h++) {
zeta = sqrt((*mPred) * (*mPred) + *vPred);
e = logistic((*mPred) + log(*e0 / (1 - *e0)));
*mPred -= mPostU[ i + h * T ] * mPostV[ j + h * P ];
*vPred -= mPostU[ i + h * T ] * mPostU[ i + h * T ] *
vPostV[ j + h * P ] + vPostU[ i + h * T ] *
mPostV[ j + h * P ] * mPostV[ j + h * P ] +
vPostU[ i + h * T ] * vPostV[ j + h * P ];
eta2Prior = 1 / (vPriorVj[ j ] * vPriorVh[ h ]);
eta1Prior = mPriorV[ h ] / (vPriorVj[ j ] * vPriorVh[ h ]);
eta2Post = 1 / vPostV[ j + h * P ];
eta1Post = mPostV[ j + h * P ] / vPostV[ j + h * P ];
reps = (double) nZerosAuxj[ j ] + nOnesAuxj[ j ];
eta2New = eta2Prior - 2 * reps * lambda(zeta) *
(mPostU[ i + h * T ] * mPostU[ i + h * T ] +
vPostU[ i + h * T ]);
eta1New = -(1 - 2 * e) * mPostU[ i + h * T ] *
reps / 2 + 2 * lambda(zeta) * reps *
mPostU[ i + h * T ] * (*mPred) + eta1Prior;
/* We update the posterior mean and variance */
eta2Post = (1 - rho) * eta2Post + rho * eta2New;
eta1Post = (1 - rho) * eta1Post + rho * eta1New;
vPostV[ j + h * P ] = 1 / eta2Post;
mPostV[ j + h * P ] = eta1Post / eta2Post;
/* We update the predictive mean and variance */
*mPred += mPostU[ i + h * T ] * mPostV[ j + h * P ];
*vPred += mPostU[ i + h * T ] * mPostU[ i + h * T ] *
vPostV[ j + h * P ] + vPostU[ i + h * T ] *
mPostV[ j + h * P ] * mPostV[ j + h * P ] +
vPostU[ i + h * T ] * vPostV[ j + h * P ];
/* We update the prior variance */
if (h != k - 1 && h != k - 2) {
vNew = ((mPostV[ j + h * P ] - mPriorV[ h ]) *
(mPostV[ j + h * P ] - mPriorV[ h ]) +
vPostV[ j + h * P ]) / vPriorVj[ j ];
vPriorVh[ h ] = (1 - rho) * vPriorVh[ h ] + rho * vNew;
}
}
}
