CbmKFSecondaryVertexFinder.cxx

Go to the documentation of this file.

 
#include "CbmKFSecondaryVertexFinder.h"
 
#include "CbmKF.h"
#include "CbmKFTrack.h"
 
#include <cmath>
 
using std::fabs;
using std::vector;
 
ClassImp(CbmKFSecondaryVertexFinder)
 
  void CbmKFSecondaryVertexFinder::Clear(Option_t* /*opt*/) {
  vTracks.clear();
  VGuess         = 0;
  VParent        = 0;
  MassConstraint = -1;
}
 
void CbmKFSecondaryVertexFinder::ClearTracks() { vTracks.clear(); }
 
void CbmKFSecondaryVertexFinder::AddTrack(CbmKFTrackInterface* Track) {
  vTracks.push_back(Track);
}
 
void CbmKFSecondaryVertexFinder::SetTracks(vector<CbmKFTrackInterface*>& vTr) {
  vTracks = vTr;
}
 
void CbmKFSecondaryVertexFinder::SetApproximation(CbmKFVertexInterface* Guess) {
  VGuess = Guess;
}
 
void CbmKFSecondaryVertexFinder::SetMassConstraint(Double_t MotherMass) {
  MassConstraint = MotherMass;
}
 
void CbmKFSecondaryVertexFinder::SetTopoConstraint(CbmKFVertexInterface* VP) {
  VParent = VP;
}
 
void CbmKFSecondaryVertexFinder::Fit() {
  const Int_t MaxIter = 3;
 
  if (VGuess) {
    r[0] = VGuess->GetRefX();
    r[1] = VGuess->GetRefY();
    r[2] = VGuess->GetRefZ();
  } else {
    if (CbmKF::Instance()->vTargets.empty()) {
      r[0] = r[1] = r[2] = 0.;
    } else {
      CbmKFTube& t = CbmKF::Instance()->vTargets[0];
      r[0] = r[1] = 0.;
      r[2]        = t.z;
    }
  }
 
  r[3] = 0;
  r[4] = 0;
  r[5] = 0;
  r[6] = 0;
  r[7] = 0;
 
  for (Int_t iteration = 0; iteration < MaxIter; ++iteration) {
 
    for (int i = 0; i < 8; i++)
      r0[i] = r[i];
 
    r[3] = 0;
    r[4] = 0;
    r[5] = 0;
    r[6] = 0;
 
    for (Int_t i = 0; i < 36; ++i)
      C[i] = 0.0;
 
    C[0] = C[2] = C[5] = 100.0;
    C[35]              = 1.E4;
    NDF                = -3;
    Chi2               = 0.;
 
    for (vector<CbmKFTrackInterface*>::iterator tr = vTracks.begin();
         tr != vTracks.end();
         ++tr) {
      CbmKFTrack T(**tr);
 
      T.Extrapolate(r0[2]);
 
      Double_t* m = T.GetTrack();
      Double_t* V = T.GetCovMatrix();
 
      //* Fit of vertex track slopes and momentum (a,b,qp) to r0 vertex
 
      Double_t a = m[2], b = m[3], qp = m[4];
      {
        Double_t s        = V[0] * V[2] - V[1] * V[1];
        s                 = (s > 1.E-20) ? 1. / s : 0;
        Double_t zetas[2] = {(r0[0] - m[0]) * s, (r0[1] - m[1]) * s};
        a += (V[3] * V[2] - V[4] * V[1]) * zetas[0]
             + (-V[3] * V[1] + V[4] * V[0]) * zetas[1];
        b += (V[6] * V[2] - V[7] * V[1]) * zetas[0]
             + (-V[6] * V[1] + V[7] * V[0]) * zetas[1];
        qp += (V[10] * V[2] - V[11] * V[1]) * zetas[0]
              + (-V[10] * V[1] + V[11] * V[0]) * zetas[1];
      }
 
      //* convert the track to (x,y,px,py,pz,e) parameterization
      double mm[6], VV[21];
      {
        double c2 = 1. / (1. + a * a + b * b);
        double p  = 1. / qp;
        double p2 = p * p;
        double pz = sqrt(p2 * c2);
        double px = a * pz;
        double py = b * pz;
        double E  = sqrt(T.GetMass() * T.GetMass() + p2);
 
        double da = (m[2] - a);
        double db = (m[3] - b);
        double dq = (m[4] - qp);
 
        double H[3] = {-px * c2, -py * c2, -pz * p};
        double HE   = -p * p2 / E;
 
        double dpz = H[0] * da + H[1] * db + H[2] * dq;
 
        mm[0] = m[0];
        mm[1] = m[1];
        mm[2] = px + da + a * dpz;
        mm[3] = py + db + b * dpz;
        mm[4] = pz + dpz;
        mm[5] = E + HE * dq;
 
        double cxpz  = H[0] * V[3] + H[1] * V[6] + H[2] * V[10];
        double cypz  = H[0] * V[4] + H[1] * V[7] + H[2] * V[11];
        double capz  = H[0] * V[5] + H[1] * V[8] + H[2] * V[12];
        double cbpz  = H[0] * V[8] + H[1] * V[9] + H[2] * V[13];
        double cqpz  = H[0] * V[12] + H[1] * V[13] + H[2] * V[14];
        double cpzpz = H[0] * H[0] * V[5] + H[1] * H[1] * V[9]
                       + H[2] * H[2] * V[14]
                       + 2
                           * (H[0] * H[1] * V[8] + H[0] * H[2] * V[12]
                              + H[1] * H[2] * V[13]);
 
        VV[0]  = V[0];
        VV[1]  = V[1];
        VV[2]  = V[2];
        VV[3]  = V[3] * pz + a * cxpz;
        VV[4]  = V[4] * pz + a * cypz;
        VV[5]  = V[5] * pz * pz + 2 * a * pz * capz + a * a * cpzpz;
        VV[6]  = V[6] * pz + b * cxpz;
        VV[7]  = V[7] * pz + b * cypz;
        VV[8]  = V[8] * pz * pz + a * pz * cbpz + b * pz * capz + a * b * cpzpz;
        VV[9]  = V[9] * pz * pz + 2 * b * pz * cbpz + b * b * cpzpz;
        VV[10] = cxpz;
        VV[11] = cypz;
        VV[12] = capz * pz + a * cpzpz;
        VV[13] = cbpz * pz + b * cpzpz;
        VV[14] = cpzpz;
        VV[15] = HE * V[10];
        VV[16] = HE * V[11];
        VV[17] = HE * (V[12] * pz + a * cqpz);
        VV[18] = HE * (V[13] * pz + b * cqpz);
        VV[19] = HE * cqpz;
        VV[20] = HE * HE * V[14];
      }
 
 
      //* Measure the state vector with the track estimate
 
      Chi2 += T.GetRefChi2();
      NDF += T.GetRefNDF() - 3;
 
      //* Update the state vector with the momentum part of the track estimate
 
      r[3] += mm[2];
      r[4] += mm[3];
      r[5] += mm[4];
      r[6] += mm[5];
 
      C[9] += VV[5];
      C[13] += VV[8];
      C[14] += VV[9];
      C[18] += VV[12];
      C[19] += VV[13];
      C[20] += VV[14];
      C[24] += VV[17];
      C[25] += VV[18];
      C[26] += VV[19];
      C[27] += VV[20];
 
      NDF += 3;
      Chi2 += 0.;
 
      //* Update the state vector with the coordinate part of the track estimate
 
      //* Residual (measured - estimated)
 
      Double_t zeta[2] = {mm[0] - (r[0] - a * (r[2] - r0[2])),
                          mm[1] - (r[1] - b * (r[2] - r0[2]))};
 
      //* Measurement matrix H= { { 1, 0, -a, 0..0}, { 0, 1, -b,0..0} };
 
      //* S = (H*C*H' + V )^{-1}
 
      Double_t S[3] = {VV[0] + C[0] - 2 * a * C[3] + a * a * C[5],
                       VV[1] + C[1] - b * C[3] - a * C[4] + a * b * C[5],
                       VV[2] + C[2] - 2 * b * C[4] + b * b * C[5]};
 
      //* Invert S
      {
        Double_t s = S[0] * S[2] - S[1] * S[1];
 
        if (s < 1.E-20) continue;
        s           = 1. / s;
        Double_t S0 = S[0];
        S[0]        = s * S[2];
        S[1]        = -s * S[1];
        S[2]        = s * S0;
      }
 
      //* CHt = CH' - D'
 
      Double_t CHt0[7], CHt1[7];
 
      CHt0[0] = C[0] - a * C[3];
      CHt1[0] = C[1] - b * C[3];
      CHt0[1] = C[1] - a * C[4];
      CHt1[1] = C[2] - b * C[4];
      CHt0[2] = C[3] - a * C[5];
      CHt1[2] = C[4] - b * C[5];
      CHt0[3] = C[6] - a * C[8] - VV[3];
      CHt1[3] = C[7] - b * C[8] - VV[4];
      CHt0[4] = C[10] - a * C[12] - VV[6];
      CHt1[4] = C[11] - b * C[12] - VV[7];
      CHt0[5] = C[15] - a * C[17] - VV[10];
      CHt1[5] = C[16] - b * C[17] - VV[11];
      CHt0[6] = C[21] - a * C[23] - VV[15];
      CHt1[6] = C[22] - b * C[23] - VV[16];
 
      //* Kalman gain K = CH'*S
 
      Double_t K0[7], K1[7];
 
      for (Int_t i = 0; i < 7; ++i) {
        K0[i] = CHt0[i] * S[0] + CHt1[i] * S[1];
        K1[i] = CHt0[i] * S[1] + CHt1[i] * S[2];
      }
 
      //* New estimation of the vertex position r += K*zeta
 
      for (Int_t i = 0; i < 7; ++i)
        r[i] += K0[i] * zeta[0] + K1[i] * zeta[1];
 
      //* New covariance matrix C -= K*(CH')'
 
      for (Int_t i = 0, k = 0; i < 7; ++i) {
        for (Int_t j = 0; j <= i; ++j, ++k)
          C[k] -= K0[i] * CHt0[j] + K1[i] * CHt1[j];
      }
 
      //* Calculate Chi^2 & NDF
 
      NDF += 2;
      Chi2 += zeta[0] * zeta[0] * S[0] + 2 * zeta[0] * zeta[1] * S[1]
              + zeta[1] * zeta[1] * S[2];
 
    }  // add tracks
 
    // Put constraints if they exist
 
    AddTopoConstraint();
    AddMassConstraint();
 
  }  // iterations
}
 
 
void CbmKFSecondaryVertexFinder::GetVertex(CbmKFVertexInterface& vtx) {
  vtx.GetRefX() = r[0];
  vtx.GetRefY() = r[1];
  vtx.GetRefZ() = r[2];
  for (int i = 0; i < 6; i++)
    vtx.GetCovMatrix()[i] = C[i];
  vtx.GetRefChi2() = Chi2;
  vtx.GetRefNDF()  = NDF;
}
 
void CbmKFSecondaryVertexFinder::GetVertex(CbmVertex& vtx) {
  CbmKFVertexInterface vi;
  GetVertex(vi);
  vi.GetVertex(vtx);
}
 
 
void CbmKFSecondaryVertexFinder::GetMotherTrack(Double_t Par[],
                                                Double_t Cov[]) {
 
  if (!Par) return;
 
  Double_t px = r[3], py = r[4], pz = r[5];
 
  Double_t p = sqrt(px * px + py * py + pz * pz);
  double pzi = 1 / pz;
  double qp  = 1 / p;
  Par[0]     = r[0];
  Par[1]     = r[1];
  Par[2]     = px * pzi;
  Par[3]     = py * pzi;
  Par[4]     = qp;
  Par[5]     = r[2];
 
  if (!Cov) return;
 
  double qp3 = qp * qp * qp;
  Double_t J[5][6];
 
  for (Int_t i = 0; i < 5; i++)
    for (Int_t j = 0; j < 6; ++j)
      J[i][j] = 0.0;
 
 
  J[0][0] = 1;
  J[0][2] = -Par[2];
  J[1][1] = 1;
  J[1][2] = -Par[3];
  J[2][3] = pzi;
  J[2][5] = -Par[2] * pzi;
  J[3][4] = pzi;
  J[3][5] = -Par[3] * pzi;
  J[4][3] = -qp3 * px;
  J[4][4] = -qp3 * py;
  J[4][4] = -qp3 * pz;
 
  Double_t JC[5][6];
 
  for (Int_t i = 0; i < 5; i++)
    for (Int_t j = 0; j < 6; j++) {
      JC[i][j] = 0;
      for (Int_t k = 0; k < 6; k++)
        JC[i][j] += J[i][k] * Cij(k, j);
    }
 
  Int_t ii = 0;
  for (Int_t i = 0; i < 5; i++)
    for (Int_t j = i; j < 5; j++, ii++) {
      Cov[ii] = 0;
      for (Int_t k = 0; k < 6; k++)
        Cov[ii] += JC[i][k] * J[j][k];
    }
}
 
 
void CbmKFSecondaryVertexFinder::GetMass(Double_t* M, Double_t* Error_) {
  // S = sigma^2 of m2/2
 
  Double_t S =
    (r[3] * r[3] * C[9] + r[4] * r[4] * C[14] + r[5] * r[5] * C[20]
     + r[6] * r[6] * C[27]
     + 2
         * (+r[3] * r[4] * C[13] + r[5] * (r[3] * C[18] + r[4] * C[19])
            - r[6] * (r[3] * C[24] + r[4] * C[25] + r[5] * C[26])));
  Double_t m2 = r[6] * r[6] - r[3] * r[3] - r[4] * r[4] - r[5] * r[5];
 
  if (M) *M = (m2 > 1.e-20) ? sqrt(m2) : 0.;
  if (Error_) *Error_ = (S >= 0 && m2 > 1.e-20) ? sqrt(S / m2) : 1.e4;
}
 
 
void CbmKFSecondaryVertexFinder::AddMassConstraint() {
  if (MassConstraint < 0) return;
  double H[8];
  H[0] = H[1] = H[2] = 0.;
  H[3]               = -2 * r0[3];
  H[4]               = -2 * r0[4];
  H[5]               = -2 * r0[5];
  H[6]               = 2 * r0[6];
  H[7]               = 0;
  double m2 = r0[6] * r0[6] - r0[3] * r0[3] - r0[4] * r0[4] - r0[5] * r0[5];
 
  double zeta = MassConstraint * MassConstraint - m2;
  for (Int_t i = 0; i < 8; ++i)
    zeta -= H[i] * (r[i] - r0[i]);
 
  Double_t S = 0.;
  Double_t CHt[8];
  for (Int_t i = 0; i < 8; ++i) {
    CHt[i] = 0.0;
    for (Int_t j = 0; j < 8; ++j)
      CHt[i] += Cij(i, j) * H[j];
    S += H[i] * CHt[i];
  }
 
  if (S < 1.e-20) return;
  S = 1. / S;
  Chi2 += zeta * zeta * S;
  NDF += 1;
  for (Int_t i = 0, ii = 0; i < 8; ++i) {
    Double_t Ki = CHt[i] * S;
    r[i] += Ki * zeta;
    for (Int_t j = 0; j <= i; ++j)
      C[ii++] -= Ki * CHt[j];
  }
}
 
 
void CbmKFSecondaryVertexFinder::Extrapolate(double T) {
 
  r0[0] += T * r0[3];
  r0[1] += T * r0[4];
  r0[2] += T * r0[5];
 
  r[0] += T * r[3];
  r[1] += T * r[4];
  r[2] += T * r[5];
 
  double C30 = C[6] + T * C[9];
  double C41 = C[11] + T * C[14];
  double C52 = C[17] + T * C[20];
  double T54 = T * C[19];
 
  C[0] += T * (C[6] + C30);
  C[1] += T * (C[7] + C[10]);
  C[2] += T * (C[11] + C41);
  C[10] += T * C[13];
  C[6]  = C30;
  C[11] = C41;
 
  C[15] += T * C[18];
  C[16] += T54;
  C[3] += T * (C[8] + C[15]);
  C[4] += T * (C[12] + C[16]);
  C[5] += T * (C[17] + C52);
  C[17] = C52;
  C[12] += T54;
 
  C[7] += T * C[13];
  C[8] += T * C[18];
 
  C[21] += T * C[24];
  C[22] += T * C[25];
  C[23] += T * C[26];
  C[28] += T * C[31];
  C[29] += T * C[32];
  C[30] += T * C[33];
}
 
void CbmKFSecondaryVertexFinder::AddTopoConstraint() {
  if (!VParent) return;
 
  double m[3], *V;
  {
    m[0] = VParent->GetRefX();
    m[1] = VParent->GetRefY();
    m[2] = VParent->GetRefZ();
    V    = VParent->GetCovMatrix();
  }
 
  Extrapolate(-r0[7]);
 
  {
    double inf  = 1.e8;
    double xinf = r0[0] * inf;
    double yinf = r0[1] * inf;
    double zinf = r0[2] * inf;
    C[0] += r0[0] * xinf;
    C[1] += r0[0] * yinf;
    C[2] += r0[1] * yinf;
    C[3] += r0[0] * zinf;
    C[4] += r0[1] * zinf;
    C[5] += r0[2] * zinf;
    C[28] = -2 * xinf;
    C[29] = -2 * yinf;
    C[30] = -2 * zinf;
    C[31] = 0;
    C[32] = 0;
    C[33] = 0;
    C[34] = 0;
    C[35] = inf;
  }
  r[7] = r0[7];
 
  double Ai[6];
  Ai[0]      = C[4] * C[4] - C[2] * C[5];
  Ai[1]      = C[1] * C[5] - C[3] * C[4];
  Ai[3]      = C[2] * C[3] - C[1] * C[4];
  double det = (C[0] * Ai[0] + C[1] * Ai[1] + C[3] * Ai[3]);
  det        = (det > 1.e-8) ? 1. / det : 0;
  Ai[0] *= det;
  Ai[1] *= det;
  Ai[3] *= det;
  Ai[2] = (C[3] * C[3] - C[0] * C[5]) * det;
  Ai[4] = (C[0] * C[4] - C[1] * C[3]) * det;
  Ai[5] = (C[1] * C[1] - C[0] * C[2]) * det;
 
  double B[5][3];
  B[0][0] = C[6] * Ai[0] + C[7] * Ai[1] + C[8] * Ai[3];
  B[0][1] = C[6] * Ai[1] + C[7] * Ai[2] + C[8] * Ai[4];
  B[0][2] = C[6] * Ai[3] + C[7] * Ai[4] + C[8] * Ai[5];
 
  B[1][0] = C[10] * Ai[0] + C[11] * Ai[1] + C[12] * Ai[3];
  B[1][1] = C[10] * Ai[1] + C[11] * Ai[2] + C[12] * Ai[4];
  B[1][2] = C[10] * Ai[3] + C[11] * Ai[4] + C[12] * Ai[5];
 
  B[2][0] = C[15] * Ai[0] + C[16] * Ai[1] + C[17] * Ai[3];
  B[2][1] = C[15] * Ai[1] + C[16] * Ai[2] + C[17] * Ai[4];
  B[2][2] = C[15] * Ai[3] + C[16] * Ai[4] + C[17] * Ai[5];
 
  B[3][0] = C[21] * Ai[0] + C[22] * Ai[1] + C[23] * Ai[3];
  B[3][1] = C[21] * Ai[1] + C[22] * Ai[2] + C[23] * Ai[4];
  B[3][2] = C[21] * Ai[3] + C[22] * Ai[4] + C[23] * Ai[5];
 
  B[4][0] = C[28] * Ai[0] + C[29] * Ai[1] + C[30] * Ai[3];
  B[4][1] = C[28] * Ai[1] + C[29] * Ai[2] + C[30] * Ai[4];
  B[4][2] = C[28] * Ai[3] + C[29] * Ai[4] + C[30] * Ai[5];
 
  double z[3] = {m[0] - r[0], m[1] - r[1], m[2] - r[2]};
  r[0]        = m[0];
  r[1]        = m[1];
  r[2]        = m[2];
  r[3] += B[0][0] * z[0] + B[0][1] * z[1] + B[0][2] * z[2];
  r[4] += B[1][0] * z[0] + B[1][1] * z[1] + B[1][2] * z[2];
  r[5] += B[2][0] * z[0] + B[2][1] * z[1] + B[2][2] * z[2];
  r[6] += B[3][0] * z[0] + B[3][1] * z[1] + B[3][2] * z[2];
  r[7] += B[4][0] * z[0] + B[4][1] * z[1] + B[4][2] * z[2];
 
  {
    double AV[6] = {C[0] - V[0],
                    C[1] - V[1],
                    C[2] - V[2],
                    C[3] - V[3],
                    C[4] - V[4],
                    C[5] - V[5]};
    double AVi[6];
    AVi[0] = AV[4] * AV[4] - AV[2] * AV[5];
    AVi[1] = AV[1] * AV[5] - AV[3] * AV[4];
    AVi[2] = AV[3] * AV[3] - AV[0] * AV[5];
    AVi[3] = AV[2] * AV[3] - AV[1] * AV[4];
    AVi[4] = AV[0] * AV[4] - AV[1] * AV[3];
    AVi[5] = -123;  // FIXME assing correct value
    det    = (AV[0] * AVi[0] + AV[1] * AVi[1] + AV[3] * AVi[3]);
    det    = (det > 1.e-8) ? 1. / det : 0;
    NDF += 2;
    Chi2 += (+(AVi[0] * z[0] + AVi[1] * z[1] + AVi[3] * z[2]) * z[0]
             + (AVi[1] * z[0] + AVi[2] * z[1] + AVi[4] * z[2]) * z[1]
             + (AVi[3] * z[0] + AVi[4] * z[1] + AVi[5] * z[2]) * z[2])
            * det;
  }
 
  double d0, d1, d2;
  C[0] = V[0];
  C[1] = V[1];
  C[2] = V[2];
  C[3] = V[3];
  C[4] = V[4];
  C[5] = V[5];
 
  d0 = B[0][0] * V[0] + B[0][1] * V[1] + B[0][2] * V[3] - C[6];
  d1 = B[0][0] * V[1] + B[0][1] * V[2] + B[0][2] * V[4] - C[7];
  d2 = B[0][0] * V[3] + B[0][1] * V[4] + B[0][2] * V[5] - C[8];
 
  C[6] += d0;
  C[7] += d1;
  C[8] += d2;
  C[9] += d0 * B[0][0] + d1 * B[0][1] + d2 * B[0][2];
 
  d0 = B[1][0] * V[0] + B[1][1] * V[1] + B[1][2] * V[3] - C[10];
  d1 = B[1][0] * V[1] + B[1][1] * V[2] + B[1][2] * V[4] - C[11];
  d2 = B[1][0] * V[3] + B[1][1] * V[4] + B[1][2] * V[5] - C[12];
 
  C[10] += d0;
  C[11] += d1;
  C[12] += d2;
  C[13] += d0 * B[0][0] + d1 * B[0][1] + d2 * B[0][2];
  C[14] += d0 * B[1][0] + d1 * B[1][1] + d2 * B[1][2];
 
  d0 = B[2][0] * V[0] + B[2][1] * V[1] + B[2][2] * V[3] - C[15];
  d1 = B[2][0] * V[1] + B[2][1] * V[2] + B[2][2] * V[4] - C[16];
  d2 = B[2][0] * V[3] + B[2][1] * V[4] + B[2][2] * V[5] - C[17];
 
  C[15] += d0;
  C[16] += d1;
  C[17] += d2;
  C[18] += d0 * B[0][0] + d1 * B[0][1] + d2 * B[0][2];
  C[19] += d0 * B[1][0] + d1 * B[1][1] + d2 * B[1][2];
  C[20] += d0 * B[2][0] + d1 * B[2][1] + d2 * B[2][2];
 
  d0 = B[3][0] * V[0] + B[3][1] * V[1] + B[3][2] * V[3] - C[21];
  d1 = B[3][0] * V[1] + B[3][1] * V[2] + B[3][2] * V[4] - C[22];
  d2 = B[3][0] * V[3] + B[3][1] * V[4] + B[3][2] * V[5] - C[23];
 
  C[21] += d0;
  C[22] += d1;
  C[23] += d2;
  C[24] += d0 * B[0][0] + d1 * B[0][1] + d2 * B[0][2];
  C[25] += d0 * B[1][0] + d1 * B[1][1] + d2 * B[1][2];
  C[26] += d0 * B[2][0] + d1 * B[2][1] + d2 * B[2][2];
  C[27] += d0 * B[3][0] + d1 * B[3][1] + d2 * B[3][2];
 
  d0 = B[4][0] * V[0] + B[4][1] * V[1] + B[4][2] * V[3] - C[28];
  d1 = B[4][0] * V[1] + B[4][1] * V[2] + B[4][2] * V[4] - C[29];
  d2 = B[4][0] * V[3] + B[4][1] * V[4] + B[4][2] * V[5] - C[30];
 
  C[28] += d0;
  C[29] += d1;
  C[30] += d2;
  C[31] += d0 * B[0][0] + d1 * B[0][1] + d2 * B[0][2];
  C[32] += d0 * B[1][0] + d1 * B[1][1] + d2 * B[1][2];
  C[33] += d0 * B[2][0] + d1 * B[2][1] + d2 * B[2][2];
  C[34] += d0 * B[3][0] + d1 * B[3][1] + d2 * B[3][2];
  C[35] += d0 * B[4][0] + d1 * B[4][1] + d2 * B[4][2];
 
  {
    Extrapolate(r[7]);
 
    double CH00 = C[0] + r0[0] * C[28];
    double CH10 = C[1] + r0[0] * C[29];
    double CH11 = C[2] + r0[1] * C[29];
    double CH20 = C[3] + r0[0] * C[30];
    double CH21 = C[4] + r0[1] * C[30];
    double CH22 = C[5] + r0[2] * C[30];
 
    C[28] += r0[0] * C[35];
    C[29] += r0[1] * C[35];
    C[30] += r0[2] * C[35];
 
    C[0] = CH00 + r0[0] * C[28];
    C[1] = CH10 + r0[1] * C[28];
    C[2] = CH11 + r0[1] * C[29];
    C[3] = CH20 + r0[2] * C[28];
    C[4] = CH21 + r0[2] * C[29];
    C[5] = CH22 + r0[2] * C[30];
 
    C[6] += r0[0] * C[31];
    C[7] += r0[1] * C[31];
    C[8] += r0[2] * C[31];
    C[10] += r0[0] * C[32];
    C[11] += r0[1] * C[32];
    C[12] += r0[2] * C[32];
    C[15] += r0[0] * C[33];
    C[16] += r0[1] * C[33];
    C[17] += r0[2] * C[33];
    C[21] += r0[0] * C[34];
    C[22] += r0[1] * C[34];
    C[23] += r0[2] * C[34];
  }
}