Ejemplo n.º 1
0
void Stage::updateTeamVision(
    Team **teams, int numTeams, Ship** ships, int numShips, bool** teamVision) {
  for (int x = 0; x < numTeams; x++) {
    Team *team = teams[x];
    for (int y = 0; y < numShips; y++) {
      Ship *ship = ships[y];
      teamVision[x][y] =
          (x != ship->teamIndex) && (team->shipsAlive > 0) && (ship->alive);
    }
  }

  if (numInnerWallLines_ == 0) {
    return;
  }

  for (int x = 0; x < numTeams; x++) {
    Team *team = teams[x];
    for (int y = 0; y < numShips; y++) {
      Ship *visibleShip = ships[y];
      if (x != visibleShip->teamIndex && visibleShip->alive) {
        bool teamHasVision = false;
        for (int z = 0; z < team->numShips && !teamHasVision; z++) {
          int teamShipIndex = team->firstShipIndex + z;
          Ship *teamShip = ships[teamShipIndex];
          if (teamShip->alive) {
            Line2D visionLine(
                teamShip->x, teamShip->y, visibleShip->x, visibleShip->y);
            teamHasVision = hasVision(&visionLine);
          }
        }
        teamVision[x][y] = teamHasVision;
      }
    }
  }
}
void ObservationGenerator::generateLineObservations() {
    getSelf(gtSelf,obsSelf,player_);
    int seenLines = 0;

    for (int i = WO_OPP_GOAL_LINE; i <= WO_BOTTOM_SIDE_LINE; i++) {
        if (seenLines >= 4) break;
        WorldObject& truthWO = gt_object_->objects_[i];

        Line2D visionLine(gtSelf.loc, gtSelf.orientation);
        Point2D point = truthWO.lineLoc.getSegmentIntersection(visionLine);
        int idx = WO_UNKNOWN_FIELD_LINE_1+seenLines;
        if(i == WO_CENTER_LINE) idx = WO_CENTER_LINE;
        auto& obsWO = obs_object_->objects_[idx];
        float bearing = gtSelf.loc.getBearingTo(point, gtSelf.orientation);
        float distance = gtSelf.loc.getDistanceTo(point);
        float pan = joint_->values_[HeadPan];
        if (fabs(pan - bearing) > FOVx/2.0)
            continue;

        float missedObsRate = 1.0/3.0;
        float randPct = rand_.sampleU();
        // see lines up to 2.5 m
        if (randPct > (missedObsRate * MISSED_OBS_FACTOR) && distance < 2500) {
            // seen
            seenLines++;

            float maxleft = gtSelf.orientation + pan + FOVx/2;
            float maxright = gtSelf.orientation + pan - FOVx/2;
            LineSegment vline = truthWO.lineLoc.getVisiblePortion(gtSelf.loc, gtSelf.orientation + pan, FOVx);
            vline = vline.globalToRelative(gtSelf.loc, gtSelf.orientation);

            float xnoise = rand_.sampleU(.95,1.05);
            float ynoise = rand_.sampleU(.95,1.05);
            float tnoise = rand_.sampleU(-10 * DEG_T_RAD, 10 * DEG_T_RAD);
            vline.start.x *= xnoise;
            vline.end.x *= xnoise;
            vline.start.y *= ynoise;
            vline.end.y *= ynoise;
            vline.computeCenter();
            obsWO.visionPt1 = vline.start;
            obsWO.visionPt2 = vline.end;
            obsWO.visionLine = vline;
            Point2D robot(0,0);
            Point2D closest = vline.getPointOnSegmentClosestTo(robot);
            obsWO.visionDistance = closest.getMagnitude();
            obsWO.visionBearing = closest.getDirection();
            obsWO.visionConfidence = 1.0;
            float diff = joint_->values_[HeadPan] - bearing;
            obsWO.imageCenterX = iparams_.width/2.0 + (diff / (FOVx/2.0) * iparams_.width/2.0);
            obsWO.imageCenterY = iparams_.height/2.0;
            obsWO.seen = true;


            point = point.globalToRelative(gtSelf.loc, gtSelf.orientation);
        } // line was seen with random prob
    } // line loop
}