// $Id: Similarity6.cc,v 1.1 2003/07/02 16:21:08 sachs Exp $
//
// David Sachs - Fermilab - July 2003

#include "CovMatrices/Similarity6.h"

void CovMatrices::similarityPAPt(
	const CovMatrix6& a,
	const Vector6 p[],
	CovMatrix6& result)
{
  // Pointers to use internal triangular representation
  // of CovMatrix classes
  const double * const ap = &a(0,0);
  double *rp = &result(0,0);

  for (int i = 0; i < 6; ++i)
  {
    const double *pi = &p[i][0];
    for (int j = 0; j <= i; ++j)
    {
      const double *pj = &p[j][0];

      // Unfolded form of inner matrix multiply loops
      *rp++ =	ap[0] * (pi[0]*pj[0]) +
		ap[1] * (pi[0]*pj[1] + pi[1]*pj[0]) +
		ap[2] * (pi[1]*pj[1]) +
		ap[3] * (pi[0]*pj[2] + pi[2]*pj[0]) +
		ap[4] * (pi[1]*pj[2] + pi[2]*pj[1]) +
		ap[5] * (pi[2]*pj[2]) +
		ap[6] * (pi[0]*pj[3] + pi[3]*pj[0]) +
		ap[7] * (pi[1]*pj[3] + pi[3]*pj[1]) +
		ap[8] * (pi[2]*pj[3] + pi[3]*pj[2]) +
		ap[9] * (pi[3]*pj[3]) +
		ap[10]* (pi[0]*pj[4] + pi[4]*pj[0]) +
		ap[11]* (pi[1]*pj[4] + pi[4]*pj[1]) +
		ap[12]* (pi[2]*pj[4] + pi[4]*pj[2]) +
		ap[13]* (pi[3]*pj[4] + pi[4]*pj[3]) +
		ap[14]* (pi[4]*pj[4]) +
		ap[15]* (pi[0]*pj[5] + pi[5]*pj[0]) +
		ap[16]* (pi[1]*pj[5] + pi[5]*pj[1]) +
		ap[17]* (pi[2]*pj[5] + pi[5]*pj[2]) +
		ap[18]* (pi[3]*pj[5] + pi[5]*pj[3]) +
		ap[19]* (pi[4]*pj[5] + pi[5]*pj[4]) +
		ap[20]* (pi[5]*pj[5]);
    }
  }
}


void CovMatrices::similarityPtAP(
	const CovMatrix6& a,
	const Vector6 p[],
	CovMatrix6& result)
{
  // Pointers to use internal triangular representation
  // of CovMatrix classes
  const double * const ap = &a(0,0);
  double *rp = &result(0,0);

  for (int i = 0; i < 6; ++i)
  {
    for (int j = 0; j <= i; ++j)
    {
      *rp++ =	ap[0] * (p[0][i]*p[0][j]) +
		ap[1] * (p[0][i]*p[1][j] + p[1][i]*p[0][j]) +
		ap[2] * (p[1][i]*p[1][j]) +
		ap[3] * (p[0][i]*p[2][j] + p[2][i]*p[0][j]) +
		ap[4] * (p[1][i]*p[2][j] + p[2][i]*p[1][j]) +
		ap[5] * (p[2][i]*p[2][j]) +
		ap[6] * (p[0][i]*p[3][j] + p[3][i]*p[0][j]) +
		ap[7] * (p[1][i]*p[3][j] + p[3][i]*p[1][j]) +
		ap[8] * (p[2][i]*p[3][j] + p[3][i]*p[2][j]) +
		ap[9] * (p[3][i]*p[3][j]) +
		ap[10]* (p[0][i]*p[4][j] + p[4][i]*p[0][j]) +
		ap[11]* (p[1][i]*p[4][j] + p[4][i]*p[1][j]) +
		ap[12]* (p[2][i]*p[4][j] + p[4][i]*p[2][j]) +
		ap[13]* (p[3][i]*p[4][j] + p[4][i]*p[3][j]) +
		ap[14]* (p[4][i]*p[4][j]) +
		ap[15]* (p[0][i]*p[5][j] + p[5][i]*p[0][j]) +
		ap[16]* (p[1][i]*p[5][j] + p[5][i]*p[1][j]) +
		ap[17]* (p[2][i]*p[5][j] + p[5][i]*p[2][j]) +
		ap[18]* (p[3][i]*p[5][j] + p[5][i]*p[3][j]) +
		ap[19]* (p[4][i]*p[5][j] + p[5][i]*p[4][j]) +
		ap[20]* (p[5][i]*p[5][j]);
    }
  }
}