PmaNode3D.cxx

Go to the documentation of this file.

#include "larreco/RecoAlg/PMAlg/PmaSegment3D.h"
#include "larreco/RecoAlg/PMAlg/PmaTrack3D.h"
#include "larreco/RecoAlg/PMAlg/Utilities.h"

#include "larcore/Geometry/Geometry.h"
#include "larcore/Geometry/WireReadout.h"
#include "larcorealg/Geometry/PlaneGeo.h"
#include "larcorealg/Geometry/TPCGeo.h"
#include "lardataalg/DetectorInfo/DetectorPropertiesData.h"

#include "art/Framework/Services/Registry/ServiceHandle.h"
#include "messagefacility/MessageLogger/MessageLogger.h"

// Fixed optimization directions:     X      Y      Z
bool pma::Node3D::fGradFixed[3] = {false, false, false};

double pma::Node3D::fMargin = 3.0;

pma::Node3D::Node3D()
  : fTpcGeo(art::ServiceHandle<geo::Geometry const>()->TPC())
  , fChannelMap(art::ServiceHandle<geo::WireReadout>()->Get())
  , fMinX(0)
  , fMaxX(0)
  , fMinY(0)
  , fMaxY(0)
  , fMinZ(0)
  , fMaxZ(0)
  , fPoint3D(0, 0, 0)
  , fDriftOffset(0)
  , fIsVertex(false)
{
  fTPC = 0;
  fCryo = 0;

  fProj2D[0].Set(0);
  fProj2D[1].Set(0);
  fProj2D[2].Set(0);
}

pma::Node3D::Node3D(detinfo::DetectorPropertiesData const& detProp,
                    const TVector3& p3d,
                    unsigned int tpc,
                    unsigned int cryo,
                    bool vtx,
                    double xshift)
  : fTpcGeo(art::ServiceHandle<geo::Geometry const>()->TPC(geo::TPCID(cryo, tpc)))
  , fChannelMap(art::ServiceHandle<geo::WireReadout>()->Get())
  , fDriftOffset(xshift)
  , fIsVertex(vtx)
{
  fTPC = tpc;
  fCryo = cryo;

  unsigned int lastPlane = geo::kZ;
  while ((lastPlane > 0) && !fChannelMap.HasPlane(geo::PlaneID{fTpcGeo.ID(), lastPlane}))
    lastPlane--;

  fMinX = detProp.ConvertTicksToX(0, lastPlane, tpc, cryo);
  fMaxX = detProp.ConvertTicksToX(detProp.NumberTimeSamples() - 1, lastPlane, tpc, cryo);
  if (fMaxX < fMinX) {
    double tmp = fMaxX;
    fMaxX = fMinX;
    fMinX = tmp;
  }

  fMinY = fTpcGeo.MinY();
  fMaxY = fTpcGeo.MaxY();
  fMinZ = fTpcGeo.MinZ();
  fMaxZ = fTpcGeo.MaxZ();

  SetPoint3D(p3d);
}

double pma::Node3D::GetDistToWall() const
{
  double d, dmin = fPoint3D.X() - fMinX;
  d = fMaxX - fPoint3D.X();
  if (d < dmin) dmin = d;

  d = fPoint3D.Y() - fMinY;
  if (d < dmin) dmin = d;
  d = fMaxY - fPoint3D.Y();
  if (d < dmin) dmin = d;

  d = fPoint3D.Z() - fMinZ;
  if (d < dmin) dmin = d;
  d = fMaxZ - fPoint3D.Z();
  if (d < dmin) dmin = d;

  return dmin;
}

bool pma::Node3D::SameTPC(const TVector3& p3d, float margin) const
{
  if (((fMinX - margin) <= p3d.X()) && (p3d.X() <= (fMaxX + margin)) &&
      ((fMinY - margin) <= p3d.Y()) && (p3d.Y() <= (fMaxY + margin)) &&
      ((fMinZ - margin) <= p3d.Z()) && (p3d.Z() <= (fMaxZ + margin)))
    return true;
  else
    return false;
}

bool pma::Node3D::SameTPC(const pma::Vector3D& p3d, float margin) const
{
  if (((fMinX - margin) <= p3d.X()) && (p3d.X() <= (fMaxX + margin)) &&
      ((fMinY - margin) <= p3d.Y()) && (p3d.Y() <= (fMaxY + margin)) &&
      ((fMinZ - margin) <= p3d.Z()) && (p3d.Z() <= (fMaxZ + margin)))
    return true;
  else
    return false;
}

bool pma::Node3D::LimitPoint3D()
{
  bool trimmed = false;

  if (fPoint3D.X() < fMinX - fMargin) {
    fPoint3D.SetX(fMinX - fMargin);
    trimmed = true;
  }
  if (fPoint3D.X() > fMaxX + fMargin) {
    fPoint3D.SetX(fMaxX + fMargin);
    trimmed = true;
  }

  if (fPoint3D.Y() < fMinY - fMargin) {
    fPoint3D.SetY(fMinY - fMargin);
    trimmed = true;
  }
  if (fPoint3D.Y() > fMaxY + fMargin) {
    fPoint3D.SetY(fMaxY + fMargin);
    trimmed = true;
  }

  if (fPoint3D.Z() < fMinZ - fMargin) {
    fPoint3D.SetZ(fMinZ - fMargin);
    trimmed = true;
  }
  if (fPoint3D.Z() > fMaxZ + fMargin) {
    fPoint3D.SetZ(fMaxZ + fMargin);
    trimmed = true;
  }

  return trimmed;
}

void pma::Node3D::UpdateProj2D()
{
  unsigned int i = 0;
  for (auto const& plane : fChannelMap.Iterate<geo::PlaneGeo>(fTpcGeo.ID())) {
    fProj2D[i++].Set(plane.PlaneCoordinate(geo::vect::toPoint(fPoint3D)),
                     fPoint3D.X() - fDriftOffset);
  }
}

bool pma::Node3D::SetPoint3D(const TVector3& p3d)
{
  fPoint3D = p3d;

  bool accepted = !LimitPoint3D();
  UpdateProj2D();

  return accepted;
}

double pma::Node3D::GetDistance2To(const TVector3& p3d) const
{
  return pma::Dist2(fPoint3D, p3d);
}

double pma::Node3D::GetDistance2To(const TVector2& p2d, unsigned int view) const
{
  return pma::Dist2(fProj2D[view], p2d);
}

double pma::Node3D::SumDist2Hits() const
{
  double sum = 0.0F;
  for (auto h : fAssignedHits) {
    if (h->IsEnabled()) {
      unsigned int view = h->View2D();

      sum += OptFactor(view) *     // alpha_i
             h->GetSigmaFactor() * // hit_amp / hit_max_amp
             pma::Dist2(h->Point2D(), fProj2D[view]);
    }
  }
  return sum;
}

pma::Vector3D pma::Node3D::GetDirection3D() const
{
  pma::Element3D* seg = 0;
  if (next) { seg = dynamic_cast<pma::Element3D*>(next); }
  else if (prev) {
    seg = dynamic_cast<pma::Element3D*>(prev);
  }
  else {
    throw cet::exception("Node3D") << "Isolated vertex." << std::endl;
  }

  if (seg) { return seg->GetDirection3D(); }
  else {
    throw cet::exception("Node3D") << "Corrupted vertex." << std::endl;
  }
}

void pma::Node3D::SetProjection(pma::Hit3D& h) const
{
  TVector2 gstart;
  TVector3 g3d;
  if (prev) {
    pma::Node3D* vtx = static_cast<pma::Node3D*>(prev->Prev());
    gstart = vtx->Projection2D(h.View2D());
    if (!next) g3d = vtx->Point3D();
  }
  else if (next) {
    pma::Node3D* vtx = static_cast<pma::Node3D*>(next->Next());
    gstart = Projection2D(h.View2D());
    gstart -= vtx->Projection2D(h.View2D()) - Projection2D(h.View2D());
    if (!prev) {
      g3d = fPoint3D;
      g3d -= vtx->Point3D() - fPoint3D;
    }
  }
  else {
    mf::LogError("pma::Node3D") << "Isolated vertex.";
    TVector2 p(Projection2D(h.View2D()));
    h.SetProjection(p, 0.0F);
    h.SetPoint3D(fPoint3D);
    return;
  }

  TVector2 v0(h.Point2D());
  v0 -= Projection2D(h.View2D());

  TVector2 v1(gstart);
  v1 -= Projection2D(h.View2D());

  double v0Norm = v0.Mod();
  double v1Norm = v1.Mod();
  double mag = v0Norm * v1Norm;
  double cosine = 0.0;
  if (mag != 0.0) cosine = v0 * v1 / mag;

  TVector2 p(Projection2D(h.View2D()));

  if (prev && next) {
    pma::Node3D* vNext = static_cast<pma::Node3D*>(next->Next());
    TVector2 vN(vNext->Projection2D(h.View2D()));
    vN -= Projection2D(h.View2D());

    mag = v0Norm * vN.Mod();
    double cosineN = 0.0;
    if (mag != 0.0) cosineN = v0 * vN / mag;

    // hit on the previous segment side, sorting on the -cosine(prev_seg, point)  /max.val. = 1/
    if (cosineN <= cosine) h.SetProjection(p, -(float)cosine);
    // hit on the next segment side, sorting on the 1+cosine(next_seg, point)  /min.val. = 1/
    else
      h.SetProjection(p, 2.0F + (float)cosineN);

    h.SetPoint3D(fPoint3D);
  }
  else {
    float b = (float)(v0Norm * cosine / v1Norm);
    if (fFrozen) // limit 3D positions to outermose node if frozen
    {
      h.SetPoint3D(fPoint3D);
    }
    else // or set 3D positions along the line of outermost segment
    {
      g3d -= fPoint3D;
      h.SetPoint3D(fPoint3D + (g3d * b));

      p += (v1 * b);
    }
    h.SetProjection(p, -b);
  }
}

double pma::Node3D::Length2() const
{
  double l = 0.0;
  if (next) l += (static_cast<pma::Segment3D*>(next))->Length();
  if (prev) l += (static_cast<pma::Segment3D*>(prev))->Length();

  if (next && prev)
    return 0.25 * l * l;
  else
    return l * l;
}

double pma::Node3D::SegmentCos() const
{
  if (prev && next) {
    auto const& vStop1 = static_cast<pma::Node3D*>(prev->Prev())->fPoint3D;
    auto const& vStop2 = static_cast<pma::Node3D*>(next->Next())->fPoint3D;
    pma::Vector3D v1(
      vStop1.X() - fPoint3D.X(), vStop1.Y() - fPoint3D.Y(), vStop1.Z() - fPoint3D.Z());
    pma::Vector3D v2(
      vStop2.X() - fPoint3D.X(), vStop2.Y() - fPoint3D.Y(), vStop2.Z() - fPoint3D.Z());
    double mag = sqrt(v1.Mag2() * v2.Mag2());
    double cosine = 0.0;
    if (mag != 0.0) cosine = v1.Dot(v2) / mag;
    return cosine;
  }
  else {
    mf::LogError("pma::Node3D") << "pma::Node3D::SegmentCos(): neighbours not initialized.";
    return -1.0;
  }
}

double pma::Node3D::SegmentCosWirePlane() const
{
  if (prev && next) {
    auto const& vStop1 = static_cast<pma::Node3D*>(prev->Prev())->fPoint3D;
    auto const& vStop2 = static_cast<pma::Node3D*>(next->Next())->fPoint3D;
    pma::Vector2D v1(vStop1.Y() - fPoint3D.Y(), vStop1.Z() - fPoint3D.Z());
    pma::Vector2D v2(vStop2.Y() - fPoint3D.Y(), vStop2.Z() - fPoint3D.Z());
    double mag = sqrt(v1.Mag2() * v2.Mag2());
    double cosine = 0.0;
    if (mag != 0.0) cosine = v1.Dot(v2) / mag;
    return cosine;
  }
  else {
    mf::LogError("pma::Node3D") << "pma::Node3D::SegmentCosZX(): neighbours not initialized.";
    return -1.0;
  }
}

double pma::Node3D::SegmentCosTransverse() const
{
  if (prev && next) {
    auto const& vStop1 = static_cast<pma::Node3D*>(prev->Prev())->fPoint3D;
    auto const& vStop2 = static_cast<pma::Node3D*>(next->Next())->fPoint3D;
    pma::Vector2D v1(vStop1.X() - fPoint3D.X(), vStop1.Z() - fPoint3D.Z());
    pma::Vector2D v2(vStop2.X() - fPoint3D.X(), vStop2.Z() - fPoint3D.Z());
    double mag = sqrt(v1.Mag2() * v2.Mag2());
    double cosine = 0.0;
    if (mag != 0.0) cosine = v1.Dot(v2) / mag;
    return cosine;
  }
  else {
    mf::LogError("pma::Node3D") << "pma::Node3D::SegmentCosZY(): neighbours not initialized.";
    return -1.0;
  }
}

// *** Note: should be changed / generalized for horizontal wire planes (e.g. 2-phase LAr). ***
double pma::Node3D::EndPtCos2Transverse() const
{
  if (prev && next) {
    auto const& vStart = static_cast<pma::Node3D*>(prev->Prev())->fPoint3D;
    auto const& vStop = static_cast<pma::Node3D*>(next->Next())->fPoint3D;

    double dy = vStop.X() - vStart.X();
    double dz = vStop.Z() - vStart.Z();
    double len2 = dy * dy + dz * dz;
    double cosine2 = 0.0;
    if (len2 > 0.0) cosine2 = dz * dz / len2;
    return cosine2;
  }
  else
    return 0.0;
}

double pma::Node3D::PiInWirePlane() const
{
  if (prev && NextCount()) {
    pma::Segment3D* seg0 = dynamic_cast<pma::Segment3D*>(prev);
    pma::Segment3D* seg1 = dynamic_cast<pma::Segment3D*>(Next(0));
    unsigned int nInd1 = NHits(geo::kU) + seg0->NHits(geo::kU) + seg1->NHits(geo::kU);

    if (fHitsRadius > 0.0F)
      return (1.0 + SegmentCosWirePlane()) * fHitsRadius * fHitsRadius / (4 * nInd1 + 1.0);
    else
      return (1.0 + SegmentCosWirePlane()) * Length2() / (4 * nInd1 + 1.0);
  }
  else
    return 0.0;
}

// Constraint on two segments angle in projection to plane parallel to wire plane, suppressed by
// the orientation in the plane transverse to wire plane (only sections with low variation of
// drift time are penalized with this constraint); PiInWirePlane() components are reduced if
// there are Ind1 / geo::kU hits which add information to the object shape.
// *** Note: should be changed / generalized for horizontal wire planes (e.g. 2-phase LAr). ***
double pma::Node3D::PenaltyInWirePlane() const
{
  if (fIsVertex) return 0.0;

  unsigned int nseg = 1;
  double penalty = PiInWirePlane();
  pma::Node3D* v;
  if (next) {
    v = static_cast<pma::Node3D*>(next->Next());
    penalty += v->PiInWirePlane();
    nseg++;
  }
  if (prev) {
    v = static_cast<pma::Node3D*>(prev->Prev());
    penalty += v->PiInWirePlane();
    nseg++;
  }
  if (penalty > 0.0)
    return pow(EndPtCos2Transverse(), 10) * penalty / nseg;
  else
    return 0.0;
}

bool pma::Node3D::IsBranching() const
{
  size_t nnext = NextCount();
  if (nnext > 1) return true; // 1 trk -> vtx -> n*trk

  if (prev && nnext) {
    pma::Segment3D* segPrev = static_cast<pma::Segment3D*>(prev);
    pma::Segment3D* segNext = static_cast<pma::Segment3D*>(Next(0));
    if (segNext->Parent() != segPrev->Parent()) // 1 trk -> vtx -> 1 trk
      return true;
  }
  return false;
}

bool pma::Node3D::IsTPCEdge() const
{
  if (prev && (NextCount() == 1)) {
    pma::Segment3D* segPrev = static_cast<pma::Segment3D*>(prev);
    pma::Segment3D* segNext = static_cast<pma::Segment3D*>(Next(0));

    if ((segPrev->TPC() < 0) || (segNext->TPC() < 0)) return true;
  }
  return false;
}

std::vector<pma::Track3D*> pma::Node3D::GetBranches() const
{
  std::vector<pma::Track3D*> branches;
  if (NextCount()) {
    branches.reserve(NextCount());
    for (size_t i = 0; i < NextCount(); ++i) {
      pma::Segment3D* seg = static_cast<pma::Segment3D*>(Next(i));
      branches.push_back(seg->Parent());
    }
  }
  return branches;
}

double pma::Node3D::Pi(float endSegWeight, bool doAsymm) const
{
  if (fIsVertex) return 0.0;

  if (prev && NextCount()) {
    pma::Segment3D* segPrev = static_cast<pma::Segment3D*>(prev);
    pma::Segment3D* segNext = static_cast<pma::Segment3D*>(Next(0));

    double scale = 1.0;
    if ((segPrev->TPC() < 0) || (segNext->TPC() < 0))
      scale = 0.5; // lower penalty on segments between tpc's

    double segCos = SegmentCos();

    double lAsymmFactor = 0.0;
    if (doAsymm) {
      double lPrev = segPrev->Length();
      double lNext = segNext->Length();
      double lSum = lPrev + lNext;
      if (lSum > 0.1) {
        double lAsymm = (1.0 - segCos) * (lPrev - lNext) / lSum;
        lAsymmFactor = 0.05 * lAsymm * lAsymm;
      }
    }

    if (fHitsRadius > 0.0F)
      return scale * (1.0 + segCos + lAsymmFactor) * fHitsRadius * fHitsRadius;
    else
      return scale * (1.0 + segCos + lAsymmFactor) * Length2();
  }
  else {
    double pi_result = 0.0;
    unsigned int nSeg = 0;
    pma::Segment3D* seg = 0;
    if (prev) {
      seg = static_cast<pma::Segment3D*>(prev);

      SortedObjectBase* prevVtx = seg->Prev();
      if (prevVtx->Prev()) nSeg++;
      nSeg += prevVtx->NextCount();
    }
    else if (next) {
      seg = static_cast<pma::Segment3D*>(next);

      SortedObjectBase* nextVtx = seg->Next();
      nSeg += nextVtx->NextCount() + 1;
    }
    else {
      mf::LogWarning("pma::Node3D") << "pma::Node3D::Pi(): an isolated vertex?";
      return 0.0;
    }
    if (nSeg == 1) pi_result = endSegWeight * seg->Length2();
    return pi_result;
  }
}

double pma::Node3D::Penalty(float endSegWeight) const
{
  unsigned int nseg = 1;
  double penalty = Pi(endSegWeight, true);

  pma::Node3D* v;
  for (unsigned int i = 0; i < NextCount(); i++) {
    v = static_cast<pma::Node3D*>(Next(i)->Next());
    penalty += v->Pi(endSegWeight, false);
    nseg++;
  }
  if (prev) {
    v = static_cast<pma::Node3D*>(prev->Prev());
    penalty += v->Pi(endSegWeight, false);
    nseg++;
  }
  return penalty / nseg;
}

double pma::Node3D::Mse() const
{
  unsigned int nhits = NPrecalcEnabledHits(); //NEnabledHits();
  double mse = SumDist2();

  pma::Segment3D* seg;
  for (unsigned int i = 0; i < NextCount(); i++) {
    seg = static_cast<pma::Segment3D*>(Next(i));
    nhits += seg->NPrecalcEnabledHits(); //NEnabledHits();
    mse += seg->SumDist2();
  }
  if (prev) {
    seg = static_cast<pma::Segment3D*>(prev);
    nhits += seg->NPrecalcEnabledHits(); //NEnabledHits();
    mse += seg->SumDist2();
  }
  if (!nhits)
    return 0.0;
  else
    return mse / nhits;
}

double pma::Node3D::GetObjFunction(float penaltyValue, float endSegWeight) const
{
  return Mse() + penaltyValue * (Penalty(endSegWeight) + PenaltyInWirePlane());
}

double pma::Node3D::MakeGradient(float penaltyValue, float endSegWeight)
{
  double l1 = 0.0, l2 = 0.0, minLength2 = 0.0;
  TVector3 tmp(fPoint3D), gpoint(fPoint3D);

  pma::Segment3D* seg;
  if (prev) {
    seg = static_cast<pma::Segment3D*>(prev);
    l1 = seg->Length2();
  }
  if (next) {
    seg = static_cast<pma::Segment3D*>(next);
    l2 = seg->Length2();
  }
  if ((l1 > 0.01) && (l1 < l2))
    minLength2 = l1;
  else if ((l2 > 0.01) && (l2 < l1))
    minLength2 = l2;
  else
    minLength2 = 0.01;

  double dxi = 0.001 * sqrt(minLength2);

  if (dxi < 6.0E-37) return 0.0;

  double gi, g0, gz;
  gz = g0 = GetObjFunction(penaltyValue, endSegWeight);

  if (!fGradFixed[0]) // gradX
  {
    gpoint[0] = tmp[0] + dxi;
    SetPoint3D(gpoint);
    gi = GetObjFunction(penaltyValue, endSegWeight);
    fGradient[0] = (g0 - gi) / dxi;

    gpoint[0] = tmp[0] - dxi;
    SetPoint3D(gpoint);
    gi = GetObjFunction(penaltyValue, endSegWeight);
    fGradient[0] = 0.5 * (fGradient[0] + (gi - g0) / dxi);

    gpoint[0] = tmp[0];
  }

  if (!fGradFixed[1]) // gradY
  {
    gpoint[1] = tmp[1] + dxi;
    SetPoint3D(gpoint);
    gi = GetObjFunction(penaltyValue, endSegWeight);
    fGradient[1] = (g0 - gi) / dxi;

    gpoint[1] = tmp[1] - dxi;
    SetPoint3D(gpoint);
    gi = GetObjFunction(penaltyValue, endSegWeight);
    fGradient[1] = 0.5 * (fGradient[1] + (gi - g0) / dxi);

    gpoint[1] = tmp[1];
  }

  if (!fGradFixed[2]) // gradZ
  {
    gpoint[2] = tmp[2] + dxi;
    SetPoint3D(gpoint);
    gi = GetObjFunction(penaltyValue, endSegWeight);
    fGradient[2] = (gz - gi) / dxi;

    gpoint[2] = tmp[2] - dxi;
    SetPoint3D(gpoint);
    gi = GetObjFunction(penaltyValue, endSegWeight);
    fGradient[2] = 0.5 * (fGradient[2] + (gi - gz) / dxi);

    gpoint[2] = tmp[2];
  }

  SetPoint3D(tmp);
  if (fGradient.Mag2() < 6.0E-37) return 0.0;

  return g0;
}

double pma::Node3D::StepWithGradient(float alfa, float tol, float penalty, float weight)
{
  unsigned int steps = 0;
  double t, t1, t2, t3, g, g0, g1, g2, g3, p1, p2;
  double eps = 6.0E-37, zero_tol = 1.0E-15;
  TVector3 tmp(fPoint3D), gpoint(fPoint3D);

  g = MakeGradient(penalty, weight);
  if (g < zero_tol) return 0.0;
  g0 = g;

  //**** first three points ****//
  alfa *= 0.8F;
  t2 = 0.0;
  g2 = g;
  t3 = 0.0;
  g3 = g;
  do {
    t1 = t2;
    g1 = g2;
    t2 = t3;
    g2 = g3;

    alfa *= 1.25F;
    t3 += alfa;
    gpoint = tmp;
    gpoint += (fGradient * t3);
    if (!SetPoint3D(gpoint)) // stepped out of allowed volume
    {
      //std::cout << "****  SetPoint trimmed 1 ****" << std::endl;
      g3 = GetObjFunction(penalty, weight);
      if (g3 < g2)
        return (g0 - g3) / g3; // exit with the node at the border
      else {
        SetPoint3D(tmp);
        return 0.0;
      } // exit with restored original position
    }

    g3 = GetObjFunction(penalty, weight);

    if (g3 < zero_tol) return 0.0;

    if (++steps > 1000) {
      SetPoint3D(tmp);
      return 0.0;
    }

  } while (g3 < g2);
  //****************************//

  //*** first step overshoot ***//
  if (steps == 1) {
    t2 = t3;
    g2 = g3;
    do {
      t3 = t2;
      g3 = g2;
      t2 = (t1 * g3 + t3 * g1) / (g1 + g3);

      // small shift...
      t2 = 0.05 * t3 + 0.95 * t2;

      // break: starting point is very close to the minimum
      if (fabs(t2 - t1) < tol) {
        SetPoint3D(tmp);
        return 0.0;
      }

      gpoint = tmp;
      gpoint += (fGradient * t2);
      if (!SetPoint3D(gpoint)) // select the best point to exit
      {
        g2 = GetObjFunction(penalty, weight);
        if (g2 < g0)
          return (g0 - g2) / g2; // exit with the node at the border
        else if (g1 < g0) {
          gpoint = tmp;
          gpoint += (fGradient * t1);
          return (g0 - g1) / g1;
        }
        else if (g3 < g0) {
          gpoint = tmp;
          gpoint += (fGradient * t3);
          return (g0 - g3) / g3;
        }
        else {
          SetPoint3D(tmp);
          return 0.0;
        }
      }
      g2 = GetObjFunction(penalty, weight);

      if (g2 < zero_tol) return 0.0;
      steps++;

    } while (g2 >= g1);
  }
  //****************************//

  while (fabs(t1 - t3) > tol) {
    //*** 3-point minimization ***//
    if ((fabs(t2 - t1) < eps) || (fabs(t2 - t3) < eps)) break; // minimum on the edge
    if ((fabs(g2 - g1) < eps) && (fabs(g2 - g3) < eps)) break; // ~singularity

    p1 = (t2 - t1) * (g2 - g3);
    p2 = (t2 - t3) * (g2 - g1);
    if (fabs(p1 - p2) < eps) break; // ~linearity

    t = t2 + ((t2 - t1) * p1 - (t2 - t3) * p2) / (2 * (p2 - p1));
    if ((t <= t1) || (t >= t3)) t = (t1 * g3 + t3 * g1) / (g1 + g3);

    gpoint = tmp;
    gpoint += (fGradient * t);
    if (!SetPoint3D(gpoint)) // select the best point to exit
    {
      g = GetObjFunction(penalty, weight);
      if ((g < g0) && (g < g1) && (g < g3))
        return (g0 - g) / g; // exit with the node at the border
      else if ((g1 < g0) && (g1 < g3)) {
        gpoint = tmp;
        gpoint += (fGradient * t1);
        return (g0 - g1) / g1;
      }
      else if (g3 < g0) {
        gpoint = tmp;
        gpoint += (fGradient * t3);
        return (g0 - g3) / g3;
      }
      else {
        SetPoint3D(tmp);
        return 0.0;
      }
    }

    g = GetObjFunction(penalty, weight);
    if (g < zero_tol) return 0.0;
    steps++;
    //****************************//

    //*** select next 3 points ***//
    if (fabs(t - t2) < 0.2 * tol) break; // start in a new direction
    if (g < g2) {
      if (t < t2) {
        t3 = t2;
        g3 = g2;
      }
      else {
        t1 = t2;
        g1 = g2;
      }
      t2 = t;
      g2 = g;
    }
    else {
      if (t < t2) {
        t1 = t;
        g1 = g;
      }
      else {
        t3 = t;
        g3 = g;
      }
    }
    //****************************//
  }

  return (g0 - g) / g;
}

void pma::Node3D::Optimize(float penaltyValue, float endSegWeight)
{
  if (!fFrozen) {
    double dg = StepWithGradient(0.1F, 0.002F, penaltyValue, endSegWeight);
    if (dg > 0.01) dg = StepWithGradient(0.03F, 0.0001F, penaltyValue, endSegWeight);
    if (dg > 0.0) dg = StepWithGradient(0.03F, 0.0001F, penaltyValue, endSegWeight);
  }
}

void pma::Node3D::ClearAssigned(pma::Track3D* trk)
{
  if (!trk) {
    // like in the base class:
    fAssignedPoints.clear();
    fAssignedHits.clear();
  }
  else {
    std::vector<pma::Track3D*> to_check;
    pma::Segment3D* seg;
    if (Prev()) {
      seg = static_cast<pma::Segment3D*>(Prev());
      if (seg->Parent() != trk) to_check.push_back(seg->Parent());
    }
    for (unsigned int i = 0; i < NextCount(); i++) {
      seg = static_cast<pma::Segment3D*>(Next(i));
      if (seg->Parent() != trk) to_check.push_back(seg->Parent());
    }

    unsigned int p = 0;
    while (p < fAssignedPoints.size()) {
      bool found = false;
      for (size_t t = 0; t < to_check.size(); t++)
        if (to_check[t]->HasRefPoint(fAssignedPoints[p])) {
          found = true;
          break;
        }

      if (!found)
        fAssignedPoints.erase(fAssignedPoints.begin() + p);
      else
        p++;
    }

    unsigned int h = 0;
    while (h < fAssignedHits.size()) {
      bool found = false;
      pma::Hit3D* hit = fAssignedHits[h];

      for (size_t t = 0; (t < to_check.size()) && !found; t++)
        for (size_t i = 0; i < to_check[t]->size(); i++) {
          pma::Hit3D* pmaHit = static_cast<pma::Hit3D*>((*(to_check[t]))[i]);
          if (hit == pmaHit) {
            found = true;
            break;
          }
        }

      if (!found)
        fAssignedHits.erase(fAssignedHits.begin() + h);
      else
        h++;
    }
  }

  fHitsRadius = 0.0F;
}