Приближенный алгоритм поиска p-медиан

[surrexi]

Здравствуйте уважаемые форумчане. Нужна помощь в написании приближенного алгоритма поиска p-медиан.
Я описал ввод графа через файл (описание списков смежности), занесение его в матрицу смежности и с помощью алгоритма Флойда расчет матрицы расстояний.
Алгоритм поиска p-медиан таков нашел на вики: https://ru.wikipedia.org/wiki/Алгоритм_поиска_p_медиан
Реализовал первый шаг, случайный выбор медиан (в условии указано две вершины)
Полученный код на C++

const int INF = 100000;
int _tmain() {
  int i, j, k; // индексы
  std::fstream file("graph.txt"); // открытие файла
	
  // Проверка открытия файла
  if (!file.is_open()) { 
    cout << "Unable to open file";
    getchar();
    exit(1);
  }

  int N; // количество вершин
  file >> N; // считывание с файла количества вершин

  // Создание матрицы весов и заполнение ее начальными значениями
  int **weights_matrix = new int *[N];
  weights_matrix++;
  for (i = 1; i <= N; i++) {
    weights_matrix[i] = new int[N];
    weights_matrix[i]++;
    for (j = 1; j <= N; j++)
      if (i == j) 
        weights_matrix[i][j] = 0;
      else 
        weights_matrix[i][j] = INF;
      }

  // Запись координат ребер с их весами в матрицу весов
  int ves;
  while (!file.eof()) {
    file >> i >> j >> ves;
    weights_matrix[i][j] = ves;
  }

  // Вывод матрицы весов
  printf("\nOutput weights matrix:\n\n");
  for (i = 1; i <= N; i++) {
    for (j = 1; j <= N; j++) {
      if (weights_matrix[i][j] == INF)
        printf("  inf");
      else
        printf("%5d", weights_matrix[i][j]);
    }
    printf("\n");
  }

  // Создание матрицы расстояний
  int **distance_matrix = new int *[N];
  distance_matrix++;
  for (i = 1; i <= N; i++) {
    distance_matrix[i] = new int[N];
    distance_matrix[i]++;
  }

  // заполнение матрицы расстояний значениями матрицы весов
  for (i = 1; i <= N; i++)
    for (j = 1; j <= N; j++)
      distance_matrix[i][j] = weights_matrix[i][j];

  // Формирование матрицы расстояний алгоритмом Флойда 
  for (k = 1; k <= N; k++) 
    for (i = 1; i <= N; i++) 
      for (j = 1; j <= N; j++) 
	if (distance_matrix[i][k] < INF && distance_matrix[k][j] < INF)
	  if (distance_matrix[i][k] + distance_matrix[k][j] < distance_matrix[i][j])
	    distance_matrix[i][j] = distance_matrix[i][k] + distance_matrix[k][j];

  // Вывод матрицы расстояний
  printf("\nOutput distance matrix:\n\n");
  for (i = 1; i <= N; i++) {
    for (j = 1; j <= N; j++) {
      printf("%4d", distance_matrix[i][j]);
    }
    printf("\n");
  }

  // Подготовительный этап для приближенного алгоритма поиска медиан
  int p = 2; // количество медиан
  int *Sol = new int [p]; // массив медиан
  Sol++;
  bool *Dop = new bool [N]; // логический массив проверки вершин на допустимость
  Dop++;
  for (i = 1; i <= p; i++)
    Sol[i] = 0; // инициализация решения, склады не выбраны
  for (i = 1; i <= N; i++)
    Dop[i] = true; //все вершины допустимы

  // Шаг 1. Рандомный выбор p-вершин
  bool stop;
  srand(time(0));
  for (i = 1; i <= p; i++) {
    stop = true;
    while (stop) {
      k = 1 + rand() % N;
      if (Dop[i]) {
        Sol[i] = k;
        Dop[k] = false;
        stop = false;
      }
    }
  }
  getchar();
  return 0;
}

Помогите с дальнейшей реализацией данного алгоритма, можно на любом языке, главное реализация.
Ссылки на подобные источники как вики и книга Кристофера не кидайте, уже все пролистал.
Нужно просто реализовать выполнение шагов.