    // @(#)root/geom:$Name:  $:$Id: TGeoHelix.cxx,v 1.1 2004/05/26 15:11:13 brun Exp $
    // Author: Andrei Gheata   28/04/04

    /*************************************************************************
     * Copyright (C) 1995-2000, Rene Brun and Fons Rademakers.               *
     * All rights reserved.                                                  *
     *                                                                       *
     * For the licensing terms see $ROOTSYS/LICENSE.                         *
     * For the list of contributors see $ROOTSYS/README/CREDITS.             *
     *************************************************************************/

    ////////////////////////////////////////////////////////////////////////////////
    //   TGeoHelix - class representing a helix curve
    //
    //  A helix is a curve defined by the following equations:
    //     x = (1/c) * COS(q*phi)
    //     y = (1/c) * SIN(q*phi)
    //     z = s * alfa
    // where:
    //     c = 1/Rxy  - curvature in XY plane
    //     phi        - phi angle
    //     S = 2*PI*s - vertical separation between helix loops
    //     q = +/- 1  - (+)=left-handed, (-)=right-handed
    //
    //   In particular, a helix describes the trajectory of a charged particle in magnetic
    // field. In such case, the helix is right-handed for negative particle charge.
    // To define a helix, one must define:
    //   - the curvature - positive defined
    //   - the Z step made after one full turn of the helix
    //   - the particle charge sign
    //   - the initial particle position and direction (force normalization to unit)
    //   - the magnetic field direction
    //
    // A helix provides:
    //   - propagation to a given Z position (in global frame)
    //   Double_t *point = TGeoHelix::PropagateToZ(Double_t z);
    //   - propagation to an arbitrary plane, returning also the new point
    //   - propagation in a geometry until the next crossed surface
    //   - computation of the total track length along a helix

    #include "TGeoHelix.h"

    ClassImp(TGeoHelix)

    //_____________________________________________________________________________
     TGeoHelix::TGeoHelix()
    {
    // Dummy constructor
       fC    = 0.;
       fS    = 0.;
       fStep = 0.;
       fPhi  = 0.;
       fQ    = 0;
       fMatrix = 0;
       TObject::SetBit(kHelixNeedUpdate, kTRUE);
       TObject::SetBit(kHelixStraigth, kFALSE);
       TObject::SetBit(kHelixCircle, kFALSE);
    }

    //_____________________________________________________________________________
     TGeoHelix::TGeoHelix(Double_t curvature, Double_t hstep, Int_t charge)
    {
    // Normal constructor
       SetXYcurvature(curvature);
       SetHelixStep(hstep);
       SetCharge(charge);
       fStep = 0.;
       fPhi  = 0.;
       fMatrix    = new TGeoHMatrix();
       TObject::SetBit(kHelixNeedUpdate, kTRUE);
       TObject::SetBit(kHelixStraigth, kFALSE);
       TObject::SetBit(kHelixCircle, kFALSE);
    }

    //_____________________________________________________________________________
     TGeoHelix::~TGeoHelix()
    {
    // Destructor
       if (fMatrix)    delete fMatrix;
    }

    //_____________________________________________________________________________
     Double_t TGeoHelix::ComputeSafeStep(Double_t epsil) const
    {
    // Compute safe linear step that can be made such that the error
    // between linear-helix extrapolation is less than EPSIL.
       if (TestBit(kHelixStraigth) || fC==0.) return 1.E30;
       Double_t c = GetTotalCurvature();
       Double_t step = TMath::Sqrt(2.*epsil/c);
       return step;
    }

    //_____________________________________________________________________________
     void TGeoHelix::InitPoint(Double_t x0, Double_t y0, Double_t z0)
    {
    // Initialize coordinates of a point on the helix
       fPointInit[0] = x0;
       fPointInit[1] = y0;
       fPointInit[2] = z0;
       TObject::SetBit(kHelixNeedUpdate, kTRUE);
    }

    //_____________________________________________________________________________
     void TGeoHelix::InitPoint (Double_t *point)
    {
       InitPoint(point[0], point[1], point[2]);
    }

    //_____________________________________________________________________________
     void TGeoHelix::InitDirection(Double_t dirx, Double_t diry, Double_t dirz, Bool_t is_normalized)
    {
    // Initialize particle direction (tangent on the helix in initial point)
       fDirInit[0] = dirx;
       fDirInit[1] = diry;
       fDirInit[2] = dirz;
       TObject::SetBit(kHelixNeedUpdate, kTRUE);
       if (is_normalized) return;
       Double_t norm = 1./TMath::Sqrt(dirx*dirx+diry*diry+dirz*dirz);
       for (Int_t i=0; i<3; i++) fDirInit[i] *= norm;
    }

    //_____________________________________________________________________________
     void TGeoHelix::InitDirection(Double_t *dir, Bool_t is_normalized)
    {
       InitDirection(dir[0], dir[1], dir[2], is_normalized);
    }

    //_____________________________________________________________________________
     Double_t TGeoHelix::GetTotalCurvature() const
    {
    // Compute helix total curvature
       Double_t k = fC/(1.+fC*fC*fS*fS);
       return k;
    }

    //_____________________________________________________________________________
     void TGeoHelix::SetXYcurvature(Double_t curvature)
    {
    // Set XY curvature: c = 1/Rxy
       fC    = curvature;
       TObject::SetBit(kHelixNeedUpdate, kTRUE);
       if (fC < 0) {
          Error("SetXYcurvature", "Curvature %f not valid. Must be positive.", fC);
          return;
       }
       if (fC == 0) {
          Warning("SetXYcurvature", "Curvature is zero. Helix is a straigth line.");
          TObject::SetBit(kHelixStraigth, kTRUE);
       }
    }

    //_____________________________________________________________________________
     void TGeoHelix::SetCharge(Int_t charge)
    {
    // Positive charge means left-handed helix.
       if (charge==0) {
          Error("ctor", "charge cannot be 0 - define it positive for a left-handed helix, negative otherwise");
          return;
       }
       Int_t q = TMath::Sign(1, charge);
       if (q == fQ) return;
       fQ = q;
       TObject::SetBit(kHelixNeedUpdate, kTRUE);
    }

    //_____________________________________________________________________________
     void TGeoHelix::SetField(Double_t bx, Double_t by, Double_t bz, Bool_t is_normalized)
    {
    // Initialize particle direction (tangent on the helix in initial point)
       fB[0] = bx;
       fB[1] = by;
       fB[2] = bz;
       TObject::SetBit(kHelixNeedUpdate, kTRUE);
       if (is_normalized) return;
       Double_t norm = 1./TMath::Sqrt(bx*bx+by*by+bz*bz);
       for (Int_t i=0; i<3; i++) fB[i] *= norm;
    }

    //_____________________________________________________________________________
     void TGeoHelix::SetHelixStep(Double_t step)
    {
    // Set Z step of the helix on a complete turn. Positive or null.
       if (step < 0) {
          Error("ctor", "Z step %f not valid. Must be positive.", step);
          return;
       }
       TObject::SetBit(kHelixNeedUpdate, kTRUE);
       fS    = 0.5*step/TMath::Pi();
       if (fS == 0.) TObject::SetBit(kHelixCircle, kTRUE);
    }

    //_____________________________________________________________________________
     void TGeoHelix::ResetStep()
    {
    // Reset current point/direction to initial values
       fStep = 0.;
       memcpy(fPoint, fPointInit, 3*sizeof(Double_t));
       memcpy(fDir, fDirInit, 3*sizeof(Double_t));
    }

    //_____________________________________________________________________________
     void TGeoHelix::Step(Double_t step)
    {
    // Make a step from current point along the helix and compute new point, direction and angle
    // To reach a plane/ shape boundary, one has to:
    //  1. Compute the safety to the plane/boundary
    //  2. Define / update a helix according local field and particle state (position, direction, charge)
    //  3. Compute the magnetic safety (maximum distance for which the field can be considered constant)
    //  4. Call TGeoHelix::Step() having as argument the minimum between 1. and 3.
    //  5. Repeat from 1. until the step to be made is small enough.
    //  6. Add to the total step the distance along a straigth line from the last point
    //     to the plane/shape boundary
       Int_t i;
       fStep += step;
       if (TObject::TestBit(kHelixStraigth)) {
          for (i=0; i<3; i++) {
             fPoint[i] = fPointInit[i]+fStep*fDirInit[i];
             fDir[i] = fDirInit[i];
          }
          return;
       }
       if (TObject::TestBit(kHelixNeedUpdate)) UpdateHelix();
       Double_t r = 1./fC;
       fPhi = fStep/TMath::Sqrt(r*r+fS*fS);
       Double_t vect[3];
       vect[0] = r * TMath::Cos(fPhi);
       vect[1] = -fQ * r * TMath::Sin(fPhi);
       vect[2] = fS * fPhi;
       fMatrix->LocalToMaster(vect, fPoint);

       Double_t ddb = fDirInit[0]*fB[0]+fDirInit[1]*fB[1]+fDirInit[2]*fB[2];
       Double_t f = -TMath::Sqrt(1.-ddb*ddb);
       vect[0] = f*TMath::Sin(fPhi);
       vect[1] = fQ*f*TMath::Cos(fPhi);
       vect[2] = ddb;
       fMatrix->LocalToMasterVect(vect, fDir);
    }

    //_____________________________________________________________________________
     Double_t TGeoHelix::StepToPlane(Double_t *point, Double_t *norm)
    {
    // Propagate initial point up to a given Z position in MARS.
       Double_t step = 0.;
       Double_t snext = 1.E30;
       Double_t dx, dy, dz;
       Double_t ddn, pdn;
       if (TObject::TestBit(kHelixNeedUpdate)) UpdateHelix();
       dx = point[0] - fPoint[0];
       dy = point[1] - fPoint[1];
       dz = point[2] - fPoint[2];
       pdn = dx*norm[0]+dy*norm[1]+dz*norm[2];
       ddn = fDir[0]*norm[0]+fDir[1]*norm[1]+fDir[2]*norm[2];
       if (TObject::TestBit(kHelixStraigth)) {
          // propagate straigth line to plane
          if ((pdn*ddn) <= 0) return snext;
          snext = pdn/ddn;
          Step(snext);
          return snext;
       }

       Double_t r = 1./fC;
       Double_t dist;
       Double_t safety = TMath::Abs(pdn);
       Double_t safestep = ComputeSafeStep();
       snext = 1.E30;
       Bool_t approaching = (ddn*pdn>0)?kTRUE:kFALSE;
       if (approaching) snext = pdn/ddn;
       else if (safety > 2.*r) return snext;
       while (snext > safestep) {
          dist = TMath::Max(safety, safestep);
          Step(dist);
          step += dist;
          dx = point[0] - fPoint[0];
          dy = point[1] - fPoint[1];
          dz = point[2] - fPoint[2];
          pdn = dx*norm[0]+dy*norm[1]+dz*norm[2];
          ddn = fDir[0]*norm[0]+fDir[1]*norm[1]+fDir[2]*norm[2];
          safety = TMath::Abs(pdn);
          approaching = (ddn*pdn>0)?kTRUE:kFALSE;
          snext = 1.E30;
          if (approaching) snext = pdn/ddn;
          else if (safety > 2.*r) {
             ResetStep();
             return snext;
          }
       }
       step += snext;
       Step(snext);
       return step;
    }

    //_____________________________________________________________________________
     void TGeoHelix::UpdateHelix()
    {
    // Update the local helix matrix.
       TObject::SetBit(kHelixNeedUpdate, kFALSE);
       fStep = 0.;
       memcpy(fPoint, fPointInit, 3*sizeof(Double_t));
       memcpy(fDir, fDirInit, 3*sizeof(Double_t));
       Double_t rot[9];
       Double_t tr[3];
       Double_t ddb = fDirInit[0]*fB[0]+fDirInit[1]*fB[1]+fDirInit[2]*fB[2];
       if ((1.-TMath::Abs(ddb))<1E-10 || fC==0) {
          // helix is just a straigth line
          TObject::SetBit(kHelixStraigth, kTRUE);
          *fMatrix = gGeoIdentity;
          return;
       }
       rot[2] = fB[0];
       rot[5] = fB[1];
       rot[8] = fB[2];
       if (ddb < 0) fS = -TMath::Abs(fS);
       Double_t fy = - fQ*TMath::Sqrt(1.-ddb*ddb);
       fy = 1./fy;
       rot[1] = fy*(fDirInit[0]-fB[0]*ddb);
       rot[4] = fy*(fDirInit[1]-fB[1]*ddb);
       rot[7] = fy*(fDirInit[2]-fB[2]*ddb);

       rot[0] = rot[4]*rot[8] - rot[7]*rot[5];
       rot[3] = rot[7]*rot[2] - rot[1]*rot[8];
       rot[6] = rot[1]*rot[5] - rot[4]*rot[2];

       tr[0] = fPointInit[0] - rot[0]/fC;
       tr[1] = fPointInit[1] - rot[3]/fC;
       tr[2] = fPointInit[2] - rot[6]/fC;

       fMatrix->SetTranslation(tr);
       fMatrix->SetRotation(rot);

    }