#include "tmachine.h"
#include "tcurves.h"
#include "tcurveutil.h"
#include "tmathutil.h"
#include "tbezier.h"
using namespace std;
//=============================================================================
ostream &operator<<(ostream &out, const TSegment &segment)
{
return out << "S{" << segment.getP0() << ", " << segment.getP1() << "}";
}
//=============================================================================
void TCubic::split(double t, TCubic &first, TCubic &second) const
{
double s = 1.0 - t;
TPointD H = s * m_p1 + t * m_p2;
first.m_p0 = m_p0;
first.m_p1 = s * m_p0 + t * m_p1;
first.m_p2 = s * first.m_p1 + t * H;
second.m_p3 = m_p3;
second.m_p2 = s * m_p2 + t * m_p3;
second.m_p1 = s * H + t * second.m_p2;
first.m_p3 = s * first.m_p2 + t * second.m_p1;
second.m_p0 = first.m_p3;
}
double TCubic::getLength(double t0, double t1) const
{
return -1;
}
//=============================================================================
TPointD TQuadratic::getPoint(double t) const
{
double s = 1 - t;
return m_p0 * s * s + 2 * t * s * m_p1 + t * t * m_p2;
}
//-----------------------------------------------------------------------------
double TQuadratic::getX(double t) const
{
double s = 1 - t;
return m_p0.x * s * s + 2 * t * s * m_p1.x + t * t * m_p2.x;
}
//-----------------------------------------------------------------------------
double TQuadratic::getY(double t) const
{
double s = 1 - t;
return m_p0.y * s * s + 2 * t * s * m_p1.y + t * t * m_p2.y;
}
//-----------------------------------------------------------------------------
double TQuadratic::getT(const TPointD &p) const
{
// risolvo l'equazione min|| b(t) - p ||
// esprimo b in forma di polinomio ed ottengo
//
// || 2 ||
//min || a t + b t + c - p ||
// || ||
//
// il tutto si riconduce a cercare le radici
// di un'equazione del tipo
// 2 3 2 2
// 2·a ·t + 3·a·b·t + t·(2·a·v + b ) + b·v
// dove v e' pari a c - p
vector<TPointD>
bez(3),
poly(3);
bez[0] = m_p0;
bez[1] = m_p1;
bez[2] = m_p2;
bezier2poly(bez, poly);
TPointD v = poly[0] - p;
vector<double> toSolve(4);
vector<double> sol;
toSolve[3] = 2.0 * norm2(poly[2]);
toSolve[2] = 3.0 * (poly[2].x * poly[1].x + poly[2].y * poly[1].y);
toSolve[1] = 2.0 * (poly[2].x * v.x + poly[2].y * v.y) + norm2(poly[1]);
toSolve[0] = (poly[1].x * v.x + poly[1].y * v.y);
int nSol = rootFinding(toSolve, sol);
if (-1 == nSol) // infinite soluzioni
return 0;
int minParameter = -1;
double minDist = (std::numeric_limits<double>::max)();
for (int i = 0; i < nSol; ++i) {
if (sol[i] < 0.0)
sol[i] = 0.0;
else if (sol[i] > 1.0)
sol[i] = 1.0;
double tmpDist = tdistance2(p, getPoint(sol[i]));
if (tmpDist < minDist) {
minDist = tmpDist;
minParameter = i;
}
}
if (minParameter != -1)
return sol[minParameter];
return tdistance2(m_p0, p) < tdistance2(m_p2, p) ? 0 : 1;
}
//-----------------------------------------------------------------------------
void TQuadratic::split(double t, TQuadratic &left, TQuadratic &right) const
{
double dt;
TPointD p;
dt = 1.0 - t;
left.m_p0 = m_p0;
right.m_p2 = m_p2;
left.m_p1 = dt * m_p0 + t * m_p1;
right.m_p1 = dt * m_p1 + t * m_p2;
p = dt * left.m_p1 + t * right.m_p1;
left.m_p2 = right.m_p0 = p;
}
//-----------------------------------------------------------------------------
TRectD TQuadratic::getBBox() const
{
TRectD bBox;
if (m_p0.x < m_p2.x)
bBox.x0 = m_p0.x, bBox.x1 = m_p2.x;
else
bBox.x0 = m_p2.x, bBox.x1 = m_p0.x;
if (m_p0.y < m_p2.y)
bBox.y0 = m_p0.y, bBox.y1 = m_p2.y;
else
bBox.y0 = m_p2.y, bBox.y1 = m_p0.y;
TPointD denom = 2 * m_p1 - m_p0 - m_p2;
if (denom.x != 0) {
double tx = (m_p1.x - m_p0.x) / denom.x;
if (tx >= 0 && tx <= 1) {
double x = getPoint(tx).x;
if (x < bBox.x0)
bBox.x0 = x;
else if (x > bBox.x1)
bBox.x1 = x;
}
}
if (denom.y != 0) {
double ty = (m_p1.y - m_p0.y) / denom.y;
if (ty >= 0 && ty <= 1) {
double y = getPoint(ty).y;
if (y < bBox.y0)
bBox.y0 = y;
else if (y > bBox.y1)
bBox.y1 = y;
}
}
return bBox;
}
/*!
Calcolo della curvatura per una Quadratica.
Vedi Farin pag.176 per la spiegazione della formula
usata.
*/
double TQuadratic::getCurvature(double t) const
{
assert(0 <= t && t <= 1.0);
TQuadratic q1, q2;
split(t, q1, q2);
double signum = 1.0;
if (areAlmostEqual(t, 1.0)) {
signum *= -1.0;
std::swap(q1, q2);
std::swap(q2.m_p0, q2.m_p2);
}
TPointD v_1_0(q2.m_p1 - q2.m_p0);
double
a = norm2(v_1_0);
if (isAlmostZero(a))
return (std::numeric_limits<double>::max)();
a = 1.0 / sqrt(a);
double
b = cross(v_1_0 * a, q2.m_p2 - q2.m_p0);
return 0.5 * signum * b / a;
}
//-----------------------------------------------------------------------------
double TQuadratic::getLength(double t0, double t1) const
{
TQuadraticLengthEvaluator lengthEval(*this);
t0 = min(max(0.0, t0), 1.0); // backward compatibility
t1 = min(max(0.0, t1), 1.0); // backward compatibility
if (t0 > t1)
std::swap(t0, t1);
if (t0 > 0.0)
return lengthEval.getLengthAt(t1) - lengthEval.getLengthAt(t0);
return lengthEval.getLengthAt(t1);
}
double TQuadratic::getApproximateLength(double t0, double t1, double error) const
{
if (t0 == t1)
return 0;
t0 = min(max(0.0, t0), 1.0);
t1 = min(max(0.0, t1), 1.0);
if (t0 > t1)
std::swap(t0, t1);
TQuadratic q;
if (t0 == 0.0 && t1 == 1.0)
q = *this;
else {
TQuadratic q1;
split(t0, q, q1);
assert(t0 != 1.0);
double newPar = (t1 - t0) / (1.0 - t0);
q1.split(newPar, q, q1);
}
double step = computeStep(q, error);
double length = 0.0;
TPointD p1 = q.getP0();
TPointD p2;
for (double t = step; t < 1.0; t += step) {
p2 = q.getPoint(t);
length += tdistance(p1, p2);
p1 = p2;
}
length += tdistance(p1, q.getP2());
return length;
}
//-----------------------------------------------------------------------------
int TQuadratic::getX(double y, double &x0, double &x1) const
{
int ret = 0;
double t;
if (y > getBBox().y1 || y < getBBox().y0)
return 0;
double a = getP0().y - 2 * getP1().y + getP2().y;
double half_b = getP1().y - getP0().y;
double c = getP0().y - y;
if (a == 0) //segment
{
if (half_b == 0) //horizontal segment, or point
{
if (c == 0) {
x0 = getP0().x;
x1 = getP2().x;
return 2;
} else
return 0;
} else {
t = -c / (2 * half_b);
if (t >= 0 && t <= 1) {
x0 = getPoint(t).x;
return 1;
}
}
}
double discr = half_b * half_b - a * c;
if (discr < 0)
return 0;
double coeff = 1.0 / a;
double coeff1 = -half_b * coeff;
if (discr == 0) {
t = coeff1;
if (t >= 0 && t <= 1) {
ret = 2;
x0 = x1 = getPoint(t).x;
}
} else {
discr = sqrt(discr) * coeff;
t = coeff1 + discr;
if (t >= 0 && t <= 1) {
ret++;
x0 = getPoint(t).x;
}
t = coeff1 - discr;
if (t >= 0 && t <= 1) {
ret++;
if (ret == 2)
x1 = getPoint(t).x;
else
x0 = getPoint(t).x;
}
}
return ret;
}
int TQuadratic::getY(double y, double &y0, double &y1) const
{
TQuadratic temp(*this);
swap(temp.m_p0.x, temp.m_p0.y);
swap(temp.m_p1.x, temp.m_p1.y);
swap(temp.m_p2.x, temp.m_p2.y);
return temp.getX(y, y0, y1);
}
//=============================================================================
TPointD TCubic::getPoint(double t) const
{
double s = 1 - t;
return m_p0 * s * s * s + 3 * t * s * (s * m_p1 + t * m_p2) + t * t * t * m_p3;
}
//-----------------------------------------------------------------------------
TPointD TCubic::getSpeed(double t) const
{
double s = 1 - t;
return 3.0 * ((m_p1 - m_p0) * s * s + 2 * (m_p2 - m_p0) * s * t + (m_p3 - m_p2) * t * t);
}
//=============================================================================
TThickQuadratic::TThickQuadratic()
: TQuadratic(), m_thickP0(0), m_thickP1(0), m_thickP2(0)
{
}
//-----------------------------------------------------------------------------
TThickQuadratic::TThickQuadratic(const TQuadratic &q)
: TQuadratic(q), m_thickP0(0.0), m_thickP1(0.0), m_thickP2(0.0)
{
}
//-----------------------------------------------------------------------------
TThickQuadratic::TThickQuadratic(const TPointD &p0, double thickP0,
const TPointD &p1, double thickP1,
const TPointD &p2, double thickP2)
: TQuadratic(p0, p1, p2), m_thickP0(thickP0), m_thickP1(thickP1), m_thickP2(thickP2)
{
}
//-----------------------------------------------------------------------------
TThickQuadratic::TThickQuadratic(const TThickPoint &p0,
const TThickPoint &p1,
const TThickPoint &p2)
: TQuadratic(TPointD(p0.x, p0.y), TPointD(p1.x, p1.y), TPointD(p2.x, p2.y)), m_thickP0(p0.thick), m_thickP1(p1.thick), m_thickP2(p2.thick)
{
}
//-----------------------------------------------------------------------------
TThickQuadratic::TThickQuadratic(const TThickQuadratic &thickQuadratic)
: TQuadratic(thickQuadratic), m_thickP0(thickQuadratic.m_thickP0), m_thickP1(thickQuadratic.m_thickP1), m_thickP2(thickQuadratic.m_thickP2)
{
}
//-----------------------------------------------------------------------------
void TThickQuadratic::setThickP0(const TThickPoint &p)
{
m_p0 = p;
m_thickP0 = p.thick;
}
//-----------------------------------------------------------------------------
void TThickQuadratic::setThickP1(const TThickPoint &p)
{
m_p1 = p;
m_thickP1 = p.thick;
}
//-----------------------------------------------------------------------------
void TThickQuadratic::setThickP2(const TThickPoint &p)
{
m_p2 = p;
m_thickP2 = p.thick;
}
//-----------------------------------------------------------------------------
TThickPoint TThickQuadratic::getThickPoint(double t) const
{
double s = 1 - t;
return TThickPoint(m_p0 * s * s + 2 * t * s * m_p1 + t * t * m_p2, m_thickP0 * s * s + 2 * t * s * m_thickP1 + t * t * m_thickP2);
}
//-----------------------------------------------------------------------------
void TThickQuadratic::split(double t, TThickQuadratic &left, TThickQuadratic &right) const
{
double dt;
TPointD p;
dt = 1.0 - t;
// control points
left.m_p0 = m_p0;
right.m_p2 = m_p2;
left.m_p1 = dt * m_p0 + t * m_p1;
right.m_p1 = dt * m_p1 + t * m_p2;
p = dt * left.m_p1 + t * right.m_p1;
left.m_p2 = right.m_p0 = p;
// thick points
left.m_thickP0 = m_thickP0;
right.m_thickP2 = m_thickP2;
left.m_thickP1 = dt * m_thickP0 + t * m_thickP1;
right.m_thickP1 = dt * m_thickP1 + t * m_thickP2;
// store thickness of intermediary point
p.x = dt * left.m_thickP1 + t * right.m_thickP1;
left.m_thickP2 = right.m_thickP0 = p.x;
}
//-----------------------------------------------------------------------------
TRectD TThickQuadratic::getBBox() const
{
TRectD bBox = TQuadratic::getBBox();
double maxRadius = tmax(m_thickP0, m_thickP1, m_thickP2);
if (maxRadius > 0) {
// bBox.enlarge(maxRadius) si comporta male nel caso bBox.isEmpty()
bBox.x0 -= maxRadius;
bBox.y0 -= maxRadius;
bBox.x1 += maxRadius;
bBox.y1 += maxRadius;
}
return bBox;
}
// ============================================================================
// Methods of the class TThickCubic
// ============================================================================
TThickCubic::TThickCubic()
: TCubic(), m_thickP0(0), m_thickP1(0), m_thickP2(0), m_thickP3(0)
{
}
//-----------------------------------------------------------------------------
TThickCubic::TThickCubic(const TPointD &p0, double thickP0,
const TPointD &p1, double thickP1,
const TPointD &p2, double thickP2,
const TPointD &p3, double thickP3)
: TCubic(p0, p1, p2, p3), m_thickP0(thickP0), m_thickP1(thickP1), m_thickP2(thickP2), m_thickP3(thickP3)
{
}
//-----------------------------------------------------------------------------
TThickCubic::TThickCubic(const TThickPoint &p0,
const TThickPoint &p1,
const TThickPoint &p2,
const TThickPoint &p3)
: TCubic(TPointD(p0.x, p0.y), TPointD(p1.x, p1.y), TPointD(p2.x, p2.y), TPointD(p3.x, p3.y)), m_thickP0(p0.thick), m_thickP1(p1.thick), m_thickP2(p2.thick), m_thickP3(p3.thick)
{
}
// tonino ***************************************************************
TThickCubic::TThickCubic(const T3DPointD &p0,
const T3DPointD &p1,
const T3DPointD &p2,
const T3DPointD &p3)
: TCubic(TPointD(p0.x, p0.y), TPointD(p1.x, p1.y), TPointD(p2.x, p2.y), TPointD(p3.x, p3.y)), m_thickP0(p0.z), m_thickP1(p1.z), m_thickP2(p2.z), m_thickP3(p3.z)
{
}
// tonino ***************************************************************
//-----------------------------------------------------------------------------
TThickCubic::TThickCubic(const TThickCubic &thickCubic)
: TCubic(thickCubic), m_thickP0(thickCubic.m_thickP0), m_thickP1(thickCubic.m_thickP1), m_thickP2(thickCubic.m_thickP2), m_thickP3(thickCubic.m_thickP3)
{
}
//-----------------------------------------------------------------------------
void TThickCubic::setThickP0(const TThickPoint &p)
{
m_p0.x = p.x;
m_p0.y = p.y;
m_thickP0 = p.thick;
}
//-----------------------------------------------------------------------------
void TThickCubic::setThickP1(const TThickPoint &p)
{
m_p1.x = p.x;
m_p1.y = p.y;
m_thickP1 = p.thick;
}
//-----------------------------------------------------------------------------
void TThickCubic::setThickP2(const TThickPoint &p)
{
m_p2.x = p.x;
m_p2.y = p.y;
m_thickP2 = p.thick;
}
//-----------------------------------------------------------------------------
void TThickCubic::setThickP3(const TThickPoint &p)
{
m_p3.x = p.x;
m_p3.y = p.y;
m_thickP3 = p.thick;
}
//-----------------------------------------------------------------------------
TThickPoint TThickCubic::getThickPoint(double t) const
{
double
thick_l1,
thick_h,
thick_r3;
double s = 1.0 - t;
TPointD l1(m_p0 * s + m_p1 * t);
thick_l1 = m_thickP0 * s + m_thickP1 * t;
TPointD h(m_p1 * s + m_p2 * t);
thick_h = m_thickP1 * s + m_thickP2 * t;
TPointD r3(m_p2 * s + m_p3 * t);
thick_r3 = m_thickP2 * s + m_thickP3 * t;
// adesso riutilizzo le variabili gia' utilizzate
// l2
l1 = l1 * s + h * t;
thick_l1 = thick_l1 * s + thick_h * t;
// r1
r3 = h * s + r3 * t;
thick_r3 = thick_h * s + thick_r3 * t;
// l3-r0
h = l1 * s + r3 * t;
thick_h = thick_l1 * s + thick_r3 * t;
return TThickPoint(h, thick_h);
}
//-----------------------------------------------------------------------------
void TThickCubic::split(double t, TThickCubic &first, TThickCubic &second) const
{
double s = 1.0 - t;
TPointD H(m_p1 * s + m_p2 * t);
double thick_h = m_thickP1 * s + m_thickP2 * t;
first.m_p0 = m_p0;
first.m_thickP0 = m_thickP0;
first.m_p1 = m_p0 * s + m_p1 * t;
first.m_thickP1 = m_thickP0 * s + m_thickP1 * t;
first.m_p2 = first.m_p1 * s + H * t;
first.m_thickP2 = first.m_thickP1 * s + thick_h * t;
second.m_p3 = m_p3;
second.m_thickP3 = m_thickP3;
second.m_p2 = m_p2 * s + m_p3 * t;
second.m_thickP2 = m_thickP2 * s + m_thickP3 * t;
second.m_p1 = H * s + second.m_p2 * t;
second.m_thickP1 = thick_h * s + second.m_thickP2 * t;
first.m_p3 = first.m_p2 * s + second.m_p1 * t;
first.m_thickP3 = first.m_thickP2 * s + second.m_thickP1 * t;
second.m_p0 = first.m_p3;
second.m_thickP0 = first.m_thickP3;
}
//-----------------------------------------------------------------------------
ostream &operator<<(ostream &out, const TQuadratic &curve)
{
return out << "Q{" << curve.getP0() << ", " << curve.getP1() << ", "
<< curve.getP2() << "}";
}
ostream &operator<<(ostream &out, const TCubic &curve)
{
return out << "C{" << curve.getP0() << ", " << curve.getP1() << ", "
<< curve.getP2() << ", " << curve.getP3() << "}";
}
ostream &operator<<(ostream &out, const TThickSegment &segment)
{
return out << "TS{" << segment.getThickP0() << ", " << segment.getThickP1() << "}";
}
ostream &operator<<(ostream &out, const TThickQuadratic &tq)
{
return out << "TQ{" << tq.getThickP0() << ", " << tq.getThickP1() << ", " << tq.getThickP2() << "}";
}
ostream &operator<<(ostream &out, const TThickCubic &tc)
{
return out << "TC{" << tc.getThickP0() << ", "
<< tc.getThickP1() << ", "
<< tc.getThickP2() << ", "
<< tc.getThickP3() << "}";
}
//-----------------------------------------------------------------------------
// End Of File
//-----------------------------------------------------------------------------