//---------------------------------------------------------------------------------------------------------------------
qreal VSpline::LengthT(qreal t) const
{
if (t < 0 || t > 1)
{
qDebug()<<"Wrong value t.";
return 0;
}
QLineF seg1_2 ( GetP1 ().toQPointF(), GetP2 () );
seg1_2.setLength(seg1_2.length () * t);
QPointF p12 = seg1_2.p2();
QLineF seg2_3 ( GetP2 (), GetP3 () );
seg2_3.setLength(seg2_3.length () * t);
QPointF p23 = seg2_3.p2();
QLineF seg12_23 ( p12, p23 );
seg12_23.setLength(seg12_23.length () * t);
QPointF p123 = seg12_23.p2();
QLineF seg3_4 ( GetP3 (), GetP4 ().toQPointF() );
seg3_4.setLength(seg3_4.length () * t);
QPointF p34 = seg3_4.p2();
QLineF seg23_34 ( p23, p34 );
seg23_34.setLength(seg23_34.length () * t);
QPointF p234 = seg23_34.p2();
QLineF seg123_234 ( p123, p234 );
seg123_234.setLength(seg123_234.length () * t);
QPointF p1234 = seg123_234.p2();
return LengthBezier ( GetP1().toQPointF(), p12, p123, p1234);
}
/**
* @brief VSpline constructor.
* @param p1 first point spline.
* @param p4 last point spline.
* @param angle1 angle from first point to first control point.
* @param angle2 angle from second point to second control point.
* @param kCurve coefficient of curvature spline.
* @param kAsm1 coefficient of length first control line.
* @param kAsm2 coefficient of length second control line.
*/
VSpline::VSpline (VPointF p1, VPointF p4, qreal angle1, qreal angle2, qreal kAsm1, qreal kAsm2, qreal kCurve,
quint32 idObject, Draw mode)
:VAbstractCurve(GOType::Spline, idObject, mode), p1(p1), p2(QPointF()), p3(QPointF()), p4(p4), angle1(angle1),
angle2(angle2), kAsm1(kAsm1), kAsm2(kAsm2), kCurve(kCurve)
{
CreateName();
this->p1 = p1;
this->p4 = p4;
this->angle1 = angle1;
this->angle2 = angle2;
this->kAsm1 = kAsm1;
this->kAsm2 = kAsm2;
this->kCurve = kCurve;
qreal L = 0, radius = 0, angle = 90;
QPointF point1 = GetP1().toQPointF();
QPointF point4 = GetP4().toQPointF();
radius = QLineF(point1, point4).length()/1.414213;//1.414213=sqrt(2);
L = kCurve * radius * 4 / 3 * tan( angle * M_PI / 180.0 / 4 );
QLineF p1p2(GetP1().x(), GetP1().y(), GetP1().x() + L * kAsm1, GetP1().y());
p1p2.setAngle(angle1);
QLineF p4p3(GetP4().x(), GetP4().y(), GetP4().x() + L * kAsm2, GetP4().y());
p4p3.setAngle(angle2);
this->p2 = p1p2.p2();
this->p3 = p4p3.p2();
}
// cppcheck-suppress unusedFunction
QLineF::IntersectType VSpline::CrossingSplLine ( const QLineF &line, QPointF *intersectionPoint ) const
{
QVector<qreal> px;
QVector<qreal> py;
px.append ( GetP1 ().x () );
py.append ( GetP1 ().y () );
QVector<qreal>& wpx = px;
QVector<qreal>& wpy = py;
PointBezier_r ( GetP1 ().x (), GetP1 ().y (), GetP2 ().x (), GetP2 ().y (),
GetP3 ().x (), GetP3 ().y (), GetP4 ().x (), GetP4 ().y (),
0, wpx, wpy);
px.append ( GetP4 ().x () );
py.append ( GetP4 ().y () );
qint32 i = 0;
QPointF crosPoint;
QLineF::IntersectType type = QLineF::NoIntersection;
for ( i = 0; i < px.count()-1; ++i )
{
type = line.intersect(QLineF ( QPointF ( px.at(i), py.at(i) ),
QPointF ( px.at(i+1), py.at(i+1) )), &crosPoint);
if ( type == QLineF::BoundedIntersection )
{
*intersectionPoint = crosPoint;
return type;
}
}
throw "Can't found point of intersection spline and line.";
}
Vector ChBox::GetPn(int ipoint)
{
switch (ipoint)
{
case 1: return GetP1();
case 2: return GetP2();
case 3: return GetP3();
case 4: return GetP4();
case 5: return GetP5();
case 6: return GetP6();
case 7: return GetP7();
case 8: return GetP8();
default: return GetP1();
}
}
/**
* @brief CutArc cut arc into two arcs.
* @param length length first arc.
* @param arc1 first arc.
* @param arc2 second arc.
* @return point cutting
*/
QPointF VArc::CutArc(const qreal &length, VArc &arc1, VArc &arc2) const
{
//Always need return two arcs, so we must correct wrong length.
qreal len = 0;
if (length < this->GetLength()*0.02)
{
len = this->GetLength()*0.02;
}
else if ( length > this->GetLength()*0.98)
{
len = this->GetLength()*0.98;
}
else
{
len = length;
}
qreal n = (len*180)/(M_PI*d->radius);
QLineF line(GetCenter().toQPointF(), GetP1());
line.setAngle(line.angle()+n);
arc1 = VArc (d->center, d->radius, d->formulaRadius, d->f1, d->formulaF1, line.angle(),
QString().setNum(line.angle()), getIdObject(), getMode());
arc2 = VArc (d->center, d->radius, d->formulaRadius, line.angle(), QString().setNum(line.angle()), d->f2,
d->formulaF2, getIdObject(), getMode());
return line.p2();
}
TF1 *GausBF::GetDf(Int_t is1, Int_t is9, Double_t rgt) const
{
AMSPoint p1 = GetP1(is1);
AMSPoint p9 = GetP9(is9);
AMSPoint pnt = p1;
AMSDir dir = p9-p1;
TString str = "[1]+1/[2]*TMath::C()/1e12*(0";
for (Int_t i = 0; i < 12; i++) str += SV(i*3+3);
str += ")";
static TF1 *func = 0;
if (!func) func = new TF1("fdf", str);
func->FixParameter(0, 0);
func->FixParameter(1, dir.y()/dir.z());
func->FixParameter(2, rgt);
for (Int_t i = 0; i < Np; i++) {
Double_t par = GetPar(is1, is9, i);
if (par != 0 && (i < 3 || i%3 == 0)) func->SetParameter(i+3, par);
else func->FixParameter(i+3, par);
}
return func;
}
void GausBF::Print(Option_t *option) const
{
TString sopt = option; sopt.ToLower();
if (sopt == "dump") {
for (Int_t i = 0; i < Ns1*Ns9*Np; i++) {
cout << Form(" %10.3e,", _par[i]);
if (i%6 == 5) cout << endl;
}
cout << endl;
}
if (sopt.BeginsWith("p")) {
TString ss = sopt(1, 2);
if (ss.IsDigit()) {
Int_t ip = ss.Atoi();
if (0 <= ip && ip < Np)
for (Int_t i = 0; i < Ns1; i++) {
for (Int_t j = 0; j < Ns9; j++) {
AMSPoint p1 = GetP1(i);
AMSPoint p9 = GetP9(j);
cout << Form("%3d %3d %6.2f %6.2f %7.2f %7.2f %10.3e",
i, j, p1.x(), p1.y(),
p9.x(), p9.y(), _par[(i*Ns9+j)*Np+ip]) << endl;
}
}
}
}
}
/**
* @brief CutSpline cut spline. GetPointP1() of base spline will return first point for first spline, GetPointP4()
* of base spline will return forth point of second spline.
* @param length length first spline
* @param spl1p2 second point of first spline
* @param spl1p3 third point of first spline
* @param spl2p2 second point of second spline
* @param spl2p3 third point of second spline
* @return point of cutting. This point is forth point of first spline and first point of second spline.
*/
QPointF VSpline::CutSpline ( qreal length, QPointF &spl1p2, QPointF &spl1p3, QPointF &spl2p2, QPointF &spl2p3 ) const
{
//Always need return two splines, so we must correct wrong length.
if (length < GetLength()*0.02)
{
length = GetLength()*0.02;
}
else if ( length > GetLength()*0.98)
{
length = GetLength()*0.98;
}
// Very stupid way find correct value of t.
// Better first compare with t = 0.5. Find length of spline.
// If length larger, take t = 0.75 and so on.
// If length less, take t = 0.25 and so on.
qreal parT = 0;
qreal step = 0.001;
while (1)
{
parT = parT + step;
qreal splLength = LengthT(parT);
if (splLength >= length || parT > 1)
{
break;
}
}
QLineF seg1_2 ( GetP1 ().toQPointF(), GetP2 () );
seg1_2.setLength(seg1_2.length () * parT);
QPointF p12 = seg1_2.p2();
QLineF seg2_3 ( GetP2 (), GetP3 () );
seg2_3.setLength(seg2_3.length () * parT);
QPointF p23 = seg2_3.p2();
QLineF seg12_23 ( p12, p23 );
seg12_23.setLength(seg12_23.length () * parT);
QPointF p123 = seg12_23.p2();
QLineF seg3_4 ( GetP3 (), GetP4 ().toQPointF() );
seg3_4.setLength(seg3_4.length () * parT);
QPointF p34 = seg3_4.p2();
QLineF seg23_34 ( p23, p34 );
seg23_34.setLength(seg23_34.length () * parT);
QPointF p234 = seg23_34.p2();
QLineF seg123_234 ( p123, p234 );
seg123_234.setLength(seg123_234.length () * parT);
QPointF p1234 = seg123_234.p2();
spl1p2 = p12;
spl1p3 = p123;
spl2p2 = p234;
spl2p3 = p34;
return p1234;
}
/**
* @brief GetPoints return list of points needed for drawing arc.
* @return list of points
*/
QVector<QPointF> VArc::GetPoints() const
{
QVector<QPointF> points;
qreal i = 0;
qreal angle = AngleArc();
qint32 k = static_cast<qint32>(angle);
qreal s = angle/(k/4);
do
{
QLineF line(d->center.toQPointF(), GetP1());
line.setAngle(line.angle()+i);
points.append(line.p2());
i = i + s;
if (i > angle)
{
QLineF line(d->center.toQPointF(), GetP1());
line.setAngle(line.angle()+angle);
points.append(line.p2());
}
} while (i <= angle);
return points;
}
/**
* @brief VSpline constructor.
* @param p1 first point spline.
* @param p2 first control point.
* @param p3 second control point.
* @param p4 second point spline.
*/
VSpline::VSpline (VPointF p1, QPointF p2, QPointF p3, VPointF p4, qreal kCurve, quint32 idObject, Draw mode)
:VAbstractCurve(GOType::Spline, idObject, mode), p1(p1), p2(p2), p3(p3), p4(p4), angle1(0), angle2(0), kAsm1(1),
kAsm2(1), kCurve(1)
{
CreateName();
this->p1 = p1;
this->p2 = p2;
this->p3 = p3;
this->p4 = p4;
this->angle1 = QLineF ( GetP1().toQPointF(), p2 ).angle();
this->angle2 = QLineF ( GetP4().toQPointF(), p3 ).angle();
qreal L = 0, radius = 0, angle = 90;
QPointF point1 = GetP1().toQPointF();
QPointF point4 = GetP4().toQPointF();
radius = QLineF(point1, point4).length()/1.414213;//1.414213=sqrt(2);
L = kCurve * radius * 4 / 3 * tan( angle * M_PI / 180.0 / 4 );
this->kCurve = kCurve;
this->kAsm1 = QLineF ( GetP1().toQPointF(), p2 ).length()/L;
this->kAsm2 = QLineF ( GetP4().toQPointF(), p3 ).length()/L;
}
const BOX2I SHAPE_ARC::BBox( int aClearance ) const
{
BOX2I bbox;
std::vector<VECTOR2I> points;
points.push_back( m_pc );
points.push_back( m_p0 );
points.push_back( GetP1() );
bbox.Compute( points );
if( aClearance != 0 )
bbox.Inflate( aClearance );
return bbox;
}
bool SHAPE_ARC::Collide( const SEG& aSeg, int aClearance ) const
{
int minDist = aClearance + m_width / 2;
auto centerDist = aSeg.Distance( m_pc );
auto p1 = GetP1();
if( centerDist < minDist )
return true;
auto ab = (aSeg.B - aSeg.A );
auto ac = ( m_pc - aSeg.A );
auto lenAbSq = ab.SquaredEuclideanNorm();
auto lambda = (double) ac.Dot( ab ) / (double) lenAbSq;
if( lambda >= 0.0 && lambda <= 1.0 )
{
VECTOR2I p;
p.x = (double) aSeg.A.x * lambda + (double) aSeg.B.x * (1.0 - lambda);
p.y = (double) aSeg.A.y * lambda + (double) aSeg.B.y * (1.0 - lambda);
auto p0pdist = ( m_p0 - p ).EuclideanNorm();
if( p0pdist < minDist )
return true;
auto p1pdist = ( p1 - p ).EuclideanNorm();
if( p1pdist < minDist )
return true;
}
auto p0dist = aSeg.Distance( m_p0 );
if( p0dist > minDist )
return true;
auto p1dist = aSeg.Distance( p1 );
if( p1dist > minDist )
return false;
return true;
}
TF1 *GausBF::GetPr(Int_t is1, Int_t is9, Double_t rgt) const
{
AMSPoint p1 = GetP1(is1);
AMSPoint p9 = GetP9(is9);
AMSPoint pnt = p1;
AMSDir dir = p9-p1;
TF1 *fdr = GetDf(is1, is9, rgt);
Double_t dy0 = fdr->Eval(p1.z())-dir.y()/dir.z();
Int_t ip = 11;
TString str = Form("[0]+[1]*(x-%.4f)-(%f)*(x-27)-[3]", p1.z(), dy0);
str += SE(5)+SE(8);
str += "+[4]*[2]*TMath::C()/1e12*(0";
for (Int_t i = 0; i < 7; i++) str += SE(i*3+ip);
str += ")";
static TF1 *func = 0;
if (!func) func = new TF1("fpr", str);
func->SetParameter(0, pnt.y());
func->SetParameter(1, dir.y()/dir.z());
func->SetParameter(2, 1/rgt);
func->SetParameter(3, 0);
func->FixParameter(4, TMath::Sqrt(1.+(dir[0]*dir[0]
+dir[1]*dir[1])/dir[2]/dir[2])*
(1.+dir[1]*dir[1] /dir[2]/dir[2]));
func->SetParameter(5, 0); func->SetParameter( 8, 0);
func->FixParameter(6, 65); func->FixParameter( 9, -65);
func->FixParameter(7, 0.1); func->FixParameter(10, 0.1);
for (Int_t i = 0; i < 21; i++) func->FixParameter(i+ip, GetPar(is1, is9, i));
func->FixParameter(3, func->Eval(pnt.z())-p1.y());
return func;
}
Double_t GausBF::Gfit(Int_t is1, Int_t is9, Double_t rgt, TGraph *gr,
TGraph *gb)
{
if ((is1 == 59 && is9 == 98) || (is1 == 59 && is9 == 109) ||
(is1 == 73 && is9 == 98) || (is1 == 86 && is9 == 97) ||
(is1 == 86 && is9 == 98) || (is1 == 97 && is9 == 98)) return -1;
AMSPoint p1 = GetP1(is1);
AMSPoint p9 = GetP9(is9);
if (rgt == 0) {
TGraph grb; if (!gb) gb = &grb;
TrProp trp(p1, p9-p1, rgt);
for (Int_t i = 0; i < 200; i++) {
Double_t z = -200+(i+0.5)*2;
trp.Propagate(z);
gb->SetPoint(i, z, trp.GuFld(trp.GetP0()).x());
}
TF1 *func = Bfit(gb);
Double_t *par = GausBF::Head()->GetPar(is1, is9);
for (Int_t i = 0; i < func->GetNpar() && i < Np; i++)
par[i] = func->GetParameter(i);
if (par[0] == 0) cout << "Failed: " << is1 << " " << is9 << endl;
for (Int_t i = 0; i < Np/3; i++)
if (par[i*3] != 0) {
if (par[i*3+2] < 0) par[i*3+2] *= -1;
if (par[i*3+2] < 1 || TMath::Abs(par[i*3+1]) > 200) {
par[i*3] = par[i*3+1] = 0;
par[i*3+2] = 1;
}
}
Double_t err = 1e-3;
Double_t csq = 0;
for (Int_t i = 0; i < gb->GetN(); i++) {
Double_t d = gb->GetY()[i]-func->Eval(gb->GetX()[i]);
csq += d*d/err/err;
}
return csq;
}
TGraph grp; if (!gr) gr = &grp;
TrProp trp(p1, p9-p1, rgt);
AMSPoint pnt;
AMSDir dir;
if (_Zref == 0) _Zref = TkDBc()->GetZlayerAJ(1);
for (Int_t i = 0; i < 9; i++) {
Double_t z = TkDBc()->GetZlayerAJ(i+1);
trp.Propagate(z);
gr->SetPoint(i, z, trp.GetP0y());
if (i == 0) { pnt = trp.GetP0(); dir = trp.GetDir(); }
}
TF1 *func = GausBF::Head()->GetPr(is1, is9, rgt);
gr->Fit(func, "q0");
for (Int_t i = 0; i < Np; i++)
GausBF::Head()->SetPar(is1, is9, i, func->GetParameter(i+5));
Double_t csq = 0;
for (Int_t i = 0; i < gr->GetN(); i++) {
Double_t res = (gr->GetY()[i]-func->Eval(gr->GetX()[i]))*1e4;
// cout << Form("%6.1f %7.3f %7.3f %6.1f", gr->GetX()[i], gr->GetY()[i],
// func->Eval(gr->GetX()[i]), res) << endl;
gr->GetY()[i] = res;
csq += res*res;
}
delete func;
return csq/gr->GetN();
}
/**
* @brief GetPoints return list with spline points.
* @return list of points.
*/
QVector<QPointF> VSpline::GetPoints () const
{
return GetPoints(GetP1().toQPointF(), p2, p3, GetP4().toQPointF());
}
void ChBox::CovarianceMatrix(ChMatrix33<>& C)
{
Vector p1,p2,p3,p4,p5,p6,p7,p8;
p1 = GetP1();
p2 = GetP2();
p3 = GetP3();
p4 = GetP4();
p5 = GetP5();
p6 = GetP6();
p7 = GetP7();
p8 = GetP8();
C(0,0)= p1.x*p1.x +
p2.x*p2.x +
p3.x*p3.x +
p4.x*p4.x +
p5.x*p5.x +
p6.x*p6.x +
p7.x*p7.x +
p8.x*p8.x;
C(1,1)= p1.y*p1.y +
p2.y*p2.y +
p3.y*p3.y +
p4.y*p4.y +
p5.y*p5.y +
p6.y*p6.y +
p7.y*p7.y +
p8.y*p8.y;
C(2,2)= p1.z*p1.z +
p2.z*p2.z +
p3.z*p3.z +
p4.z*p4.z +
p5.z*p5.z +
p6.z*p6.z +
p7.z*p7.z +
p8.z*p8.z;
C(0,1)= p1.x*p1.y +
p2.x*p2.y +
p3.x*p3.y +
p4.x*p4.y +
p5.x*p5.y +
p6.x*p6.y +
p7.x*p7.y +
p8.x*p8.y;
C(0,2)= p1.x*p1.z +
p2.x*p2.z +
p3.x*p3.z +
p4.x*p4.z +
p5.x*p5.z +
p6.x*p6.z +
p7.x*p7.z +
p8.x*p8.z;
C(1,2)= p1.y*p1.z +
p2.y*p2.z +
p3.y*p3.z +
p4.y*p4.z +
p5.y*p5.z +
p6.y*p6.z +
p7.y*p7.z +
p8.y*p8.z;
};
void ChBox::GetBoundingBox(double& xmin, double& xmax,
double& ymin, double& ymax,
double& zmin, double& zmax,
ChMatrix33<>* bbRot)
{
// TO OPTIMIZE for speed
Vector p1,p2,p3,p4,p5,p6,p7,p8;
xmax = ymax = zmax = -10e20;
xmin = ymin = zmin = +10e20;
if (bbRot==NULL)
{
p1 = GetP1();
p2 = GetP2();
p3 = GetP3();
p4 = GetP4();
p5 = GetP5();
p6 = GetP6();
p7 = GetP7();
p8 = GetP8();
}
else
{
p1 = bbRot->MatrT_x_Vect(GetP1());
p2 = bbRot->MatrT_x_Vect(GetP2());
p3 = bbRot->MatrT_x_Vect(GetP3());
p4 = bbRot->MatrT_x_Vect(GetP4());
p5 = bbRot->MatrT_x_Vect(GetP5());
p6 = bbRot->MatrT_x_Vect(GetP6());
p7 = bbRot->MatrT_x_Vect(GetP7());
p8 = bbRot->MatrT_x_Vect(GetP8());
}
if (p1.x > xmax) xmax = p1.x;
if (p1.y > ymax) ymax = p1.y;
if (p1.z > zmax) zmax = p1.z;
if (p2.x > xmax) xmax = p2.x;
if (p2.y > ymax) ymax = p2.y;
if (p2.z > zmax) zmax = p2.z;
if (p3.x > xmax) xmax = p3.x;
if (p3.y > ymax) ymax = p3.y;
if (p3.z > zmax) zmax = p3.z;
if (p4.x > xmax) xmax = p4.x;
if (p4.y > ymax) ymax = p4.y;
if (p4.z > zmax) zmax = p4.z;
if (p5.x > xmax) xmax = p5.x;
if (p5.y > ymax) ymax = p5.y;
if (p5.z > zmax) zmax = p5.z;
if (p6.x > xmax) xmax = p6.x;
if (p6.y > ymax) ymax = p6.y;
if (p6.z > zmax) zmax = p6.z;
if (p7.x > xmax) xmax = p7.x;
if (p7.y > ymax) ymax = p7.y;
if (p7.z > zmax) zmax = p7.z;
if (p8.x > xmax) xmax = p8.x;
if (p8.y > ymax) ymax = p8.y;
if (p8.z > zmax) zmax = p8.z;
if (p1.x < xmin) xmin = p1.x;
if (p1.y < ymin) ymin = p1.y;
if (p1.z < zmin) zmin = p1.z;
if (p2.x < xmin) xmin = p2.x;
if (p2.y < ymin) ymin = p2.y;
if (p2.z < zmin) zmin = p2.z;
if (p3.x < xmin) xmin = p3.x;
if (p3.y < ymin) ymin = p3.y;
if (p3.z < zmin) zmin = p3.z;
if (p4.x < xmin) xmin = p4.x;
if (p4.y < ymin) ymin = p4.y;
if (p4.z < zmin) zmin = p4.z;
if (p5.x < xmin) xmin = p5.x;
if (p5.y < ymin) ymin = p5.y;
if (p5.z < zmin) zmin = p5.z;
if (p6.x < xmin) xmin = p6.x;
if (p6.y < ymin) ymin = p6.y;
if (p6.z < zmin) zmin = p6.z;
if (p7.x < xmin) xmin = p7.x;
if (p7.y < ymin) ymin = p7.y;
if (p7.z < zmin) zmin = p7.z;
if (p8.x < xmin) xmin = p8.x;
if (p8.y < ymin) ymin = p8.y;
if (p8.z < zmin) zmin = p8.z;
}
/**
* @brief GetLength return length of spline.
* @return length.
*/
qreal VSpline::GetLength () const
{
return LengthBezier ( GetP1().toQPointF(), this->p2, this->p3, GetP4().toQPointF());
}
