//---------------------------------------------------------------------------------------------------------------------
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);
}
// 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.";
}
/**
* @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;
}
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();
}
}
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;
}