830 - Haplotyper::Transpose()

void Haplotyper::Transpose(float * source, float * dest, float theta)
   {
   // *source and *dest store a vector with likelihood information for each possible state
   // Because probabilies are symmetric, only one half of the matrix is stored

   if (theta == 0.0)
      {
      for (int i = 0; i < states; i++)
         for (int j = 0; j <= i; j++, dest++, source++)
            *dest = *source;

      return;
      }

   float sum = 0.0;
   float * probability = source;
   float * output = dest;

   for (int i = 0; i < states; i++)
      marginals[i] = 0.0;

   // Loop through the matrix...
   // Notice how i < j throughout.

   for (int i = 0; i < states; i++)
      {
      for (int j = 0; j < i; j++)
         {
         sum += *probability;
         marginals[i] += *probability;
         marginals[j] += *probability;
         probability++;
         }

      sum += *probability;
      marginals[i] += (*probability) * 2.0;
      probability++;
      }

   probability = source;

   float no_change = (1.0 - theta) * (1.0 - theta);
   float one_change = (1.0 - theta) * theta / states;
   float two_changes = sum * theta * theta / (states * states);

   // Automatically rescale likelihoods when they get too small
   if (sum < 1e-15)
      {
      no_change *= 1e30;
      one_change *= 1e30;
      two_changes *= 1e30;
      }

   // This final loop actually transposes the probabilities for each state
   for (int i = 0; i < states; i++)
      {
      for (int j = 0; j < i; j++)
         {
         *output = *probability * no_change +
                    marginals[i] * one_change +
                    marginals[j] * one_change +
                    2 * two_changes;

          probability++;
          output++;
          }

      *output = *probability * no_change +
                 marginals[i] * one_change +
                 two_changes;

      probability++;
      output++;
      }
   }