SSL_WrapperPacket* SSLWorld::generatePacket()
{
SSL_WrapperPacket* packet = new SSL_WrapperPacket;
float x,y,z,dir;
ball->getBodyPosition(x,y,z);
packet->mutable_detection()->set_camera_id(0);
packet->mutable_detection()->set_frame_number(framenum);
double t_elapsed = timer->elapsed()/1000.0;
packet->mutable_detection()->set_t_capture(t_elapsed);
packet->mutable_detection()->set_t_sent(t_elapsed);
float dev_x = cfg->noiseDeviation_x();
float dev_y = cfg->noiseDeviation_y();
float dev_a = cfg->noiseDeviation_angle();
if (cfg->noise()==false) {dev_x = 0;dev_y = 0;dev_a = 0;}
if ((cfg->vanishing()==false) || (rand0_1() > cfg->ball_vanishing()))
{
SSL_DetectionBall* vball = packet->mutable_detection()->add_balls();
vball->set_x(randn_notrig(x*1000.0f,dev_x));
vball->set_y(randn_notrig(y*1000.0f,dev_y));
vball->set_z(z*1000.0f);
vball->set_pixel_x(x*1000.0f);
vball->set_pixel_y(y*1000.0f);
vball->set_confidence(0.9 + rand0_1()*0.1);
}
for(int i = 0; i < ROBOT_COUNT; i++){
if ((cfg->vanishing()==false) || (rand0_1() > cfg->blue_team_vanishing()))
{
if (!robots[i]->on) continue;
SSL_DetectionRobot* rob = packet->mutable_detection()->add_robots_blue();
robots[i]->getXY(x,y);
dir = robots[i]->getDir();
rob->set_robot_id(i);
rob->set_pixel_x(x*1000.0f);
rob->set_pixel_y(y*1000.0f);
rob->set_confidence(1);
rob->set_x(randn_notrig(x*1000.0f,dev_x));
rob->set_y(randn_notrig(y*1000.0f,dev_y));
rob->set_orientation(normalizeAngle(randn_notrig(dir,dev_a))*M_PI/180.0f);
}
}
for(int i = ROBOT_COUNT; i < ROBOT_COUNT*2; i++){
if ((cfg->vanishing()==false) || (rand0_1() > cfg->yellow_team_vanishing()))
{
if (!robots[i]->on) continue;
SSL_DetectionRobot* rob = packet->mutable_detection()->add_robots_yellow();
robots[i]->getXY(x,y);
dir = robots[i]->getDir();
rob->set_robot_id(i-ROBOT_COUNT);
rob->set_pixel_x(x*1000.0f);
rob->set_pixel_y(y*1000.0f);
rob->set_confidence(1);
rob->set_x(randn_notrig(x*1000.0f,dev_x));
rob->set_y(randn_notrig(y*1000.0f,dev_y));
rob->set_orientation(normalizeAngle(randn_notrig(dir,dev_a))*M_PI/180.0f);
}
}
return packet;
}
int ModelDefinition::Iterate0(double* X) { // Gillespie's Langevin equation
Propensity(X);
double dx = S[0][0]*a[0] + S[0][1]*a[1] + S[0][2]*a[2] + S[0][3]*a[3];
//double dy = S[1][0]*a[0] + S[1][1]*a[1] + S[1][2]*a[2] + S[1][3]*a[3];
//double nx, ny;
double nx;
while(true){
for (int i=0; i<m; i++) {
N[i] = randn_notrig();
}
nx = X[0] + dx*dt + (S[0][0]*sqrt_a[0]*N[0] + S[0][1]*sqrt_a[1]*N[1] + S[0][2]*sqrt_a[2]*N[2] + S[0][3]*sqrt_a[3]*N[3])*sqrt_dt;
//ny = X[1] + dy*dt + (S[1][0]*sqrt_a[0]*N[0] + S[1][1]*sqrt_a[1]*N[1] + S[1][2]*sqrt_a[2]*N[2] + S[1][3]*sqrt_a[3]*N[3])*sqrt_dt;
if (nx >= 0) {
//if (nx >= 0 && ny >= 0) {
break;
}
}
X[0] = nx;
//X[1] = ny;
return 0;
}
int ModelDefinition::Iterate1(double* X) {
Propensity(X);
double B[m][m];
for (int i=0; i<m; i++) {
for (int j=0; j<m; j++) {
if (j<i) {
B[i][j] = B[j][i];
}
else {
B[i][j] = sqrt_a[i] * sqrt_a[j] * Rcorr[i][j];
}
}
}
for (int i=0; i<n; i++) {
for (int j=0; j<n; j++) {
if (j<i) {
CovR[i][j] = CovR[j][i];
}
else{
CovR[i][j] = 0.0;
for (int r=0; r<m; r++) {
for (int s=0; s<m; s++) {
CovR[i][j] += S[i][r] * B[r][s] * S[j][s];
}
}
}
}
}
double dx = S[0][0]*a[0] + S[0][1]*a[1] + S[0][2]*a[2] + S[0][3]*a[3];
//double dy = S[1][0]*a[0] + S[1][1]*a[1] + S[1][2]*a[2] + S[1][3]*a[3];
// A is a lower-triangular matrix
double A11 = sqrt(CovR[0][0]);//
double A12 = 0.0;
double A21 = CovR[0][1]/A11;
double A22 = sqrt(CovR[1][1] - A21*A21);//
if (isnan(A11) || isnan(A21) || isnan(A22)) {
cout << "oops" << endl;
return -1;
}
double nx;
//double nx,ny;
while(true){
for (int i=0; i<n; i++) {
N[i] = randn_notrig();
}
//ny = X[1] + dy*dt + (A21*N[0] + A22*N[1])*sqrt_dt;
nx = X[0] + dx*dt + (A11*N[0] + A12*N[1])*sqrt_dt;
//if(nx>=0 && ny >=0){
if(nx>=0){
break;
}
}
X[0] = nx;
//X[1] = ny;
return 0;
}
SSL_WrapperPacket* SSLWorld::generatePacket()
{
SSL_WrapperPacket* packet = new SSL_WrapperPacket;
dReal x,y,z,dir;
ball->getBodyPosition(x,y,z);
packet->mutable_detection()->set_camera_id(0);
packet->mutable_detection()->set_frame_number(framenum);
dReal t_elapsed = timer->elapsed()/1000.0;
packet->mutable_detection()->set_t_capture(t_elapsed);
packet->mutable_detection()->set_t_sent(t_elapsed);
dReal dev_x = cfg->noiseDeviation_x();
dReal dev_y = cfg->noiseDeviation_y();
dReal dev_a = cfg->noiseDeviation_angle();
if (sendGeomCount++ % cfg->sendGeometryEvery() == 0)
{
SSL_GeometryData* geom = packet->mutable_geometry();
SSL_GeometryFieldSize* field = geom->mutable_field();
field->set_line_width(CONVUNIT(cfg->Field_Line_Width()));
field->set_field_length(CONVUNIT(cfg->Field_Length()));
field->set_field_width(CONVUNIT(cfg->Field_Width()));
field->set_boundary_width(CONVUNIT(cfg->Field_Margin()));
field->set_referee_width(CONVUNIT(cfg->Field_Referee_Margin()));
field->set_goal_width(CONVUNIT(cfg->Goal_Width()));
field->set_goal_depth(CONVUNIT(cfg->Goal_Depth()));
field->set_goal_wall_width(CONVUNIT(cfg->Goal_Thickness()));
field->set_center_circle_radius(CONVUNIT(cfg->Field_Rad()));
field->set_defense_radius(CONVUNIT(cfg->Field_Defense_Rad()));
field->set_defense_stretch(CONVUNIT(cfg->Field_Defense_Stretch()));
field->set_free_kick_from_defense_dist(CONVUNIT(cfg->Field_Free_Kick()));
//TODO: verify if these fields are correct:
field->set_penalty_line_from_spot_dist(CONVUNIT(cfg->Field_Penalty_Line()));
field->set_penalty_spot_from_field_line_dist(CONVUNIT(cfg->Field_Penalty_Point()));
}
if (cfg->noise()==false) {dev_x = 0;dev_y = 0;dev_a = 0;}
if ((cfg->vanishing()==false) || (rand0_1() > cfg->ball_vanishing()))
{
SSL_DetectionBall* vball = packet->mutable_detection()->add_balls();
vball->set_x(randn_notrig(x*1000.0f,dev_x));
vball->set_y(randn_notrig(y*1000.0f,dev_y));
vball->set_z(z*1000.0f);
vball->set_pixel_x(x*1000.0f);
vball->set_pixel_y(y*1000.0f);
vball->set_confidence(0.9 + rand0_1()*0.1);
}
for(int i = 0; i < ROBOT_COUNT; i++){
if ((cfg->vanishing()==false) || (rand0_1() > cfg->blue_team_vanishing()))
{
if (!robots[i]->on) continue;
SSL_DetectionRobot* rob = packet->mutable_detection()->add_robots_blue();
robots[i]->getXY(x,y);
dir = robots[i]->getDir();
rob->set_robot_id(i);
rob->set_pixel_x(x*1000.0f);
rob->set_pixel_y(y*1000.0f);
rob->set_confidence(1);
rob->set_x(randn_notrig(x*1000.0f,dev_x));
rob->set_y(randn_notrig(y*1000.0f,dev_y));
rob->set_orientation(normalizeAngle(randn_notrig(dir,dev_a))*M_PI/180.0f);
}
}
for(int i = ROBOT_COUNT; i < ROBOT_COUNT*2; i++){
if ((cfg->vanishing()==false) || (rand0_1() > cfg->yellow_team_vanishing()))
{
if (!robots[i]->on) continue;
SSL_DetectionRobot* rob = packet->mutable_detection()->add_robots_yellow();
robots[i]->getXY(x,y);
dir = robots[i]->getDir();
rob->set_robot_id(i-ROBOT_COUNT);
rob->set_pixel_x(x*1000.0f);
rob->set_pixel_y(y*1000.0f);
rob->set_confidence(1);
rob->set_x(randn_notrig(x*1000.0f,dev_x));
rob->set_y(randn_notrig(y*1000.0f,dev_y));
rob->set_orientation(normalizeAngle(randn_notrig(dir,dev_a))*M_PI/180.0f);
}
}
return packet;
}
