aswi2021virtual-surreality-gitlab/deltarobot/element/trajectory.cpp @ c137512e

#include "trajectory.hpp"

const float Trajectory::MAX_RADIUS = 120.0f;
const float Trajectory::MIN_RADIUS = 50.0f;
const int Trajectory::SMOOTHING = 50;

Trajectory::Trajectory(): Element(Element::TRAJECTORY)
{
    set = false;
}

bool Trajectory::isSet()
{
    return set;
}

void Trajectory::setNewCurve(QList<QVector3D> data, const float minimumZ, const float maximumZ)
{
    // Uvolneni pameti od stareho tubusu.
    clear();

    // Vyhozeni zbytecnych bodu.
    QList<QVector3D> mData;
    QVector3D lastPoint;
    for (int i = 0; i < data.count(); i++)
    {
        if (i > 0 && (data.at(i) - lastPoint).length() < 50)
        {
            continue;
        }
        else
        {
            mData.append(data.at(i));
            lastPoint = data.at(i);
        }
    }

    data = mData;
    int lastIndex = data.count() - 1;

    // Vytvoreni vektoru mezi predchozim a nasledujicim bodem.
    QVector3D dv[data.count()];
    for (int i = 1; i < lastIndex; i++)
    {
        dv[i] = data.at(i + 1) - data.at(i - 1);
    }
    dv[0] = data.at(1) - data.at(0);
    //pokud maji prvni dva body stejnou y osu - krivka se nevykresli (nevim proc) - pro jistotu pridame 1 ke kazde ose
    dv[0] += QVector3D(1,1,1);
    dv[lastIndex] = data.last() - data.at(data.count() - 2);
    
    // Inicializace prvniho normaloveho vektoru pro urceni bodu na kruznici.
    QVector3D a(0,0,0);
    int index = 0;
    float smallest = dv[0].x();
    if (dv[0].y() < smallest)
    {
        smallest = dv[0].y();
        index = 1;
    }
    if (dv[0].z() < smallest)
    {
        index = 2;
    }
    switch (index)
    {
        case 0: a.setX(1.0f); break;
        case 1: a.setY(1.0f); break;
        default: a.setZ(1.0f); break;
    }
    
    // Vypocteni rozsahu souradnic a radiusu.
    float rangeZ = maximumZ - minimumZ;
    float rangeRadius = MAX_RADIUS - MIN_RADIUS;
    
    // Jestli jde o uzavrenou krivku.
    bool flSamePoints = data.at(0) == data.at(lastIndex);
    
    // Vytvoreni tubusu.
    QVector3D vertices[data.count()][SMOOTHING];
    for (int i = 0; i < data.count(); i++)
    {
        // a, b jsou normalove vektory definujici kruznici kolem daneho bodu.
        QVector3D b = QVector3D::crossProduct(a, dv[i]).normalized();
        a = QVector3D::crossProduct(dv[i], b).normalized();
        
        if (i != lastIndex || !flSamePoints)
        {
            float rTube = MIN_RADIUS + (data[i].z() - minimumZ) / rangeZ * rangeRadius;
            for (int j = 0; j < SMOOTHING; j++)
            {
                double th = j * 2 * M_PI / SMOOTHING;
                vertices[i][j] = data[i] + cos(th) * rTube * a + sin(th) * rTube * b;
            }
        }
    }
    
    if (flSamePoints)
    {
        // Pokud jde o uzavrenou krivku, spojit prvni a posledni kruznici.
        for (int i = 0; i < SMOOTHING; i++)
        {
            vertices[lastIndex][i] = vertices[0][i];
        }
    }
    else
    {
        // Pokud nejde o uzavrenou krivku, vytvorit krytky koncu.
        for (int i = 0; i < SMOOTHING; i++)
        {
            addTriangle(vertices[0][i], vertices[0][(i + 1) % SMOOTHING], data.at(0));
            addTriangle(data.at(lastIndex), vertices[lastIndex][(i + 1) % SMOOTHING], vertices[lastIndex][i]);
        }
    }
    
    // Vytvoreni trojuhelnikove site.
    for (int i = 0; i < lastIndex; i++)
    {
        for (int j = 0; j < SMOOTHING; j++)
        {
            addTetragon(vertices[i][j], vertices[i][(j + 1) % SMOOTHING], vertices[i+1][(j + 1) % SMOOTHING], vertices[i+1][j]);
        }
    }
    // Nyni muze byt draha vykreslena.
    set = true;
}

void Trajectory::addNewPointToCurve(QVector3D point, const bool start, const bool finish)
{
   if (start) {
       clear();
       curve.clear();
       newCurvePointCount = 0;
   }

   if (curve.size() > 0 && (point - curve.last()).length() < 50)
       return;

   if (curve.size() > MAX_CURVE_SIZE)
       return;

   curve.append(point-QVector3D(0,0,4500));

   if (curve.size() < 4)
       return;

   /*setNewCurve(curve, -4500,4500);
   curve.clear();
   return;*/

   // Vytvoreni vektoru mezi predchozim a nasledujicim bodem.
   QVector3D dv, dv1, dv0;
   int pos = curve.count();

   dv0 = curve.at(pos-3) - curve.at(pos-4);
   dv0 += QVector3D(1,1,1);
   dv = curve.last() - curve.at(pos-3);
   dv1 = curve.at(pos-2) - curve.at(pos - 4);

   // Inicializace prvniho normaloveho vektoru pro urceni bodu na kruznici.
   QVector3D a(0,0,0), a1;
   int index = 0;
   float smallest = dv0.x();
   if (dv0.y() < smallest)
   {
       smallest = dv0.y();
       index = 1;
   }
   if (dv0.z() < smallest)
   {
       index = 2;
   }
   switch (index)
   {
   case 0: a.setX(1.0f); break;
   case 1: a.setY(1.0f); break;
   default: a.setZ(1.0f); break;
   }

   // Vypocteni rozsahu souradnic a radiusu.
   float rangeZ = 9000;
   float rangeRadius = MAX_RADIUS - MIN_RADIUS;

   // Vytvoreni tubusu.
   QVector3D vertices[2][SMOOTHING];
   // a, b jsou normalove vektory definujici kruznici kolem daneho bodu.
   a1 = a;
   QVector3D b = QVector3D::crossProduct(a, dv).normalized();
   QVector3D b1 = QVector3D::crossProduct(a, dv1).normalized();
   a = QVector3D::crossProduct(dv, b).normalized();
   a1 = QVector3D::crossProduct(dv1, b1).normalized();

   float rTube = MIN_RADIUS + (curve.at(curve.count()-2).z() + 4500) / rangeZ * rangeRadius;
   float rTube1 = MIN_RADIUS + (curve.at(curve.count()-3).z() + 4500) / rangeZ * rangeRadius;
   for (int j = 0; j < SMOOTHING; j++)
   {
       double th = j * 2 * M_PI / SMOOTHING;
       vertices[0][j] = curve.at(curve.count()-3) + cos(th) * rTube * a + sin(th) * rTube * b;
       vertices[1][j] = curve.at(curve.count()-2) + cos(th) * rTube * a + sin(th) * rTube * b;
   }

   for (int j = 0; j < SMOOTHING; j++)
   {
       addTetragon(vertices[0][j], vertices[0][(j + 1) % SMOOTHING], vertices[1][(j + 1) % SMOOTHING], vertices[1][j]);
   }

   set = true;

   if (finish) {
       //end of tube
   }
}