Sti/StiHitContainer.cxx

//StiHitContainer.cxx
//M.L. Miller (Yale Software)
//03/01

#include "Stiostream.h"
#include <fstream>
#include <math.h>
#include <algorithm>
#include "Sti/Base/Filter.h"
#include "StiKalmanTrackNode.h"
#include "StiHit.h"
#include "StiPlacement.h"
#include "StiDetector.h"
#include "StiHitContainer.h"
#include <float.h>

using std::sort;
using std::find;
using std::lower_bound;
using std::upper_bound;
using std::stable_partition;
ostream& operator<<(ostream& os, const StiHit& hit);
ostream& operator<<(ostream&, const HitMapKey&);

int VectorAndEnd::fIdCounter = 0;
//________________________________________
VectorAndEnd::VectorAndEnd(): fEffectiveEndValid(false)
{
   invalidateEnd();
   fId=fIdCounter++; 
   theEffectiveEnd=theHitVec.end();
   TestId(568);
}
   
//________________________________________
void VectorAndEnd::TestId(int id)
{
   if ( fId == id) {
       printf(" Id = %d \n",fId);
   }   
}
//________________________________________
void VectorAndEnd:: clear()
{
   theHitVec.clear();
   invalidateEnd();
}
//________________________________________
StiHitContainer::StiHitContainer(const string & name, 
                                 const string & description,
                                 Factory<StiHit> *hitFactory)
  : Named(name),
    Described(description),
    _hitFactory(hitFactory)
{
  cout <<"StiHitContainer::StiHitContainer() -I- Started with name:"<<name<<endl;
  //_minPoint = new StiHit();
  //_maxPoint = new StiHit();
}

StiHitContainer::~StiHitContainer()
{
  cout <<"StiHitContainer::~StiHitContainer()"<<endl;
}

//void StiHitContainer::update()
//{}

void StiHitContainer::add(StiHit* hit)
{
  const StiDetector* det = hit->detector();
  if (!det) 
    throw runtime_error("StiHitContainer::add() -E- Given hit has no associated detector");
  //_key.refangle = det->getPlacement()->getNormalRefAngle();
  _key.refangle = det->getPlacement()->getLayerAngle();
  //_key.position = det->getPlacement()->getNormalRadius();
  _key.position = det->getPlacement()->getLayerRadius();
  _map[_key].push_back(hit);
  return;
}

void StiHitContainer::reset()
{
   HitMapToVectorAndEndType::iterator it;
   vector<StiHit*>::iterator iter;
   //cout << "StiHitContainer::reset() -i- XXXXXXXXXXXXXXXXXXXXXXXX _map.size() ="<<_map.size()<<endl;
   for (it=_map.begin(); it!=_map.end(); it++) 
     {
       vector<StiHit*> &hits = (*it).second.hits();
       //cout << ":"<<hits.size();
       for (iter=hits.begin();iter!=hits.end();iter++)
         {
           (*iter)->setTimesUsed(0);
         }
     }
}

void StiHitContainer::clear()
{
  cout<<"StiHitContainer::clear() -I- Started"<<endl;
    HitMapToVectorAndEndType::iterator it;
    for (it=_map.begin(); it!=_map.end(); it++) 
                        {
             (*it).second.clear();
                        }
    cout<<"StiHitContainer::clear() -I- Done"<<endl;
}

unsigned int StiHitContainer::size() const
{
    unsigned int thesize = 0;
    HitMapToVectorAndEndType::const_iterator it;
    for (it=_map.begin(); it!=_map.end(); it++) {
        thesize+=(*it).second.size();
    }
    return thesize;
}


vector<StiHit*>::iterator StiHitContainer::hitsBegin(const StiDetector* layer)
{
  //_key.refangle = layer->getPlacement()->getNormalRefAngle();
    _key.refangle = layer->getPlacement()->getLayerAngle();
    //_key.position = layer->getPlacement()->getNormalRadius();
    _key.position = layer->getPlacement()->getLayerRadius();
    assert(_map.find(_key) != _map.end());
    return _map[_key].begin();
}

vector<StiHit*>::iterator StiHitContainer::hitsEnd(const StiDetector* layer)
{
  //_key.refangle = layer->getPlacement()->getNormalRefAngle();
  _key.refangle = layer->getPlacement()->getLayerAngle();
  //_key.position = layer->getPlacement()->getNormalRadius();
  _key.position = layer->getPlacement()->getLayerRadius();
  assert(_map.find(_key) != _map.end());
  return _map[_key].TheEffectiveEnd();
}


vector<StiHit*> & StiHitContainer::getHits(StiHit& ref, double dY, double dZ, bool fetchAll)
{
  _selectedHits.clear();
  _key.refangle = ref.refangle();
  _key.position = ref.position();
        // cout << "StiHitContainer::getHits(StiHit& ref, double dY, double dZ, bool fetchAll) -I- " << endl;
        //cout << "   dY:"<<dY<<endl
        //<< "   dZ:"<<dZ<<endl;
  _minPoint.set(ref.position(),ref.refangle(),ref.y()-dY,ref.z()-dZ );
  _maxPoint.set(ref.position(),ref.refangle(),ref.y()+dY,ref.z()+dZ );

  static int id = 0;
  if (_map.find(_key) != _map.end()) {
  vector<StiHit*>& tempvec = _map[_key].hits();
  if (tempvec.size())
      {
  id = _map[_key].fId;

  vector<StiHit*>::iterator tempend = _map[_key].TheEffectiveEnd();

#if 1
   //sanity check block
   vector<StiHit*>::iterator  tmptest = tempvec.begin();
   vector<StiHit*>::iterator  tmpend  = tempvec.end();
   if (!tempvec.size() ) {
       cout  << "-- Doing tmptest for id:" <<  id<< " " << ( tmpend != tmptest )
            << " cmp " << ( tempend !=  tmptest )
            << " " << tempvec.size() << " --> " << endl;
       assert(0);
   }
#endif   

   //Search first by distance along z
  _start = lower_bound(tempvec.begin(), tempend, &_minPoint, StizHitLessThan());
  if (_start!=tempend) 
    _stop = upper_bound(tempvec.begin(), tempend, &_maxPoint, StizHitLessThan());
  else 
    {
      _start = tempend;
      _stop  = _start;
    }
  //Now search over distance along d
  StiHit * hit;
  for (vector<StiHit*>::iterator cit=_start; cit!=_stop; cit++) 
    {
      hit = *cit;
      if (fabs( hit->y() - ref.y() ) < dY)
        {
          if (fetchAll || (hit->timesUsed()==0 && hit->detector()->isActive()) )
            _selectedHits.push_back(hit);
        }
    }
 }  else {  //  StiHit vector size ==0; This is workround. 
       //  we do not know why do we need this proptection  yet
       // cout << "Warning: Fix me, please !" << endl; 
    }
  }    //  NO _key was provided
#if 0
 else {
     cout << "-- Go ahead to test thing ------------------ last id =" << id << endl;
     id = _map[_key].fId;
     cout << "-- Go ahead to test thing ------------------ next id =" << id << endl;
  }
#endif  
  return _selectedHits;
}

void StiHitContainer::sortHits()
{
  HitMapToVectorAndEndType::iterator it;
  for (it=_map.begin(); it!=_map.end(); ++it) 
    {
      vector<StiHit*>& tempvec = (*it).second.hits();
      sort(tempvec.begin(), tempvec.end(), StizHitLessThan());
      (*it).second.invalidateEnd();
    }
  return;
}

void StiHitContainer::partitionUsedHits()
{
    for (HitMapToVectorAndEndType::iterator it=_map.begin(); it!=_map.end(); ++it)
      {
      vector<StiHit*>& tempvec = (*it).second.hits();
      vector<StiHit*>::iterator where =
        stable_partition(tempvec.begin(), tempvec.end(), StiHitIsUsed() );
      (*it).second.setEnd(where);
    }
}

ostream& operator<<(ostream& os, const vector<StiHit*>& vec)
{
    for (vector<StiHit*>::const_iterator vit=vec.begin(); vit!=vec.end(); vit++) {
        os<<*(*vit)<<endl;
    }
    return os;
}

ostream& operator<<(ostream& os, const StiHitContainer& store)
{
    for (HitMapToVectorAndEndType::const_iterator it=store._map.begin(); it!=store._map.end(); it++) {
        os <<endl;
        os <<(*it).second.hits();
    }
    return os;   
}


//  then return all hits.
vector<StiHit*> & StiHitContainer::getHits()
{
  _selectedHits.clear();
  for(HitMapToVectorAndEndType::const_iterator iter= _map.begin(); iter !=_map.end(); iter++)
   {
      const vector<StiHit*> & t_hits = (*iter).second.hits();
      for (vector<StiHit*>::const_iterator it=t_hits.begin();it!=t_hits.end();++it)
        _selectedHits.push_back(*it);
   }  
  return _selectedHits;
}

//  then return all hits.
vector<StiHit*> & StiHitContainer::getHits(Filter<StiHit> & filter)
{
  //cout << "StiHitContainer::getHits(Filter<StiHit> * filter) -I- Started"<<endl;
  _selectedHits.clear();
  StiHit * hit;
  for(HitMapToVectorAndEndType::const_iterator iter= _map.begin(); iter !=_map.end(); iter++)
    {
    const vector<StiHit*> & t_hits = (*iter).second.hits();
    for (vector<StiHit*>::const_iterator it=t_hits.begin();
         it!=t_hits.end();
         ++it)
      {
      hit = *it;
      if (filter.accept(hit)) _selectedHits.push_back(hit);
      }
    }
  return _selectedHits;
}


StiHit * StiHitContainer::getNearestHit(StiHit& ref, double dY, double dZ, bool fetchAll)
{ 
  StiHit* hit = 0;
  StiHit* closestHit = 0;
  double dMax = DBL_MAX;
  double dy, dz, d;
  vector<StiHit*> & hits = getHits(ref,dY,dZ,fetchAll);
  for (vector<StiHit*>::iterator iter=hits.begin();iter!=hits.end();++iter)
    {
      hit = *iter;
      dy  = hit->y() - ref.y();
      dz  = hit->z() - ref.z();
      d   = dy*dy + dz*dz;
      if ( d<dMax)
        {
        closestHit = hit;
        dMax       = d;
        }
    }
  return closestHit;
}

---