path_simulate.cpp

Go to the documentation of this file.

/***************************************************************************
 *   Copyright (c) 2007                                                    *
 *   Human Rezaijafari <H.Rezai@web.de>                                    *
 *   This file is part of the FreeCAD CAM development system.              *
 *                                                                         *
 *   This library is free software; you can redistribute it and/or         *
 *   modify it under the terms of the GNU Library General Public           *
 *   License as published by the Free Software Foundation; either          *
 *   version 2 of the License, or (at your option) any later version.      *
 *                                                                         *
 *   This library  is distributed in the hope that it will be useful,      *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU Library General Public License for more details.                  *
 *                                                                         *
 *   You should have received a copy of the GNU Library General Public     *
 *   License along with this library; see the file COPYING.LIB. If not,    *
 *   write to the Free Software Foundation, Inc., 59 Temple Place,         *
 *   Suite 330, Boston, MA  02111-1307, USA                                *
 *                                                                         *
 ***************************************************************************/

#include "PreCompiled.h"
#include "path_simulate.h"

#include <GeomAPI_ProjectPointOnCurve.hxx>

#include <TColgp_Array1OfPnt.hxx>
#include <Base/Exception.h>
#include <Base/Builder3D.h>
#include <GCPnts_AbscissaPoint.hxx>
#include <Geom_BSplineCurve.hxx>

#define curvTOL  30.0  // gibt maximalen Krümmungsradius an ab welchem eine Unterteilung der Kurve erfolgt
#define TolDist  1.0   // entspricht der Samplingschrittweite der Kurvenpunkte für den Roboter-Output 

/* Konstruktor mit zwei Bahnfolgen (master tool & supporting die) als Input */
path_simulate::path_simulate(const std::vector<Handle_Geom_BSplineCurve> &BSplineTop,
                             const std::vector<Handle_Geom_BSplineCurve> &BSplineBottom,
                             struct CuttingToolsSettings& set)
        :m_BSplineTop(BSplineTop),m_BSplineBottom(BSplineBottom),m_set(set),
        m_t0(0.0),m_step(1e-3),m_clip(90000)
{
        
        m_pretension = m_set.spring_pretension;
        m_blech          = m_set.sheet_thickness;
        m_amax           = m_set.max_Acc;
    m_vmax       = m_set.max_Vel;
    
        m_single = false;
   
        if(m_pretension > 0) beam = true;  // flag für "write_output_***" generierung
        else                     beam = false;

    //Initialize the Iterators
    m_it1 = m_BSplineTop.begin();
    m_it2 = m_BSplineBottom.begin();

    //Initalise some vars
    gp_Pnt p(0,0,0);
    gp_Pnt q(0,0,0);

    /*------------------------------Generate the first movement of the Master------------*/

    /* Fill p with the starting point of the first Master curve*/
    (*m_it1)->D0((*m_it1)->FirstParameter(),p);

    /* Set q to the initial-Z-level in the Simulation: 2mm + Master-Radius over the sheet which is located at Z=0 */
    q.SetZ(2.0 + set.master_radius);
        
        //First we clear the vectors
    m_StartPnts1.clear();
    m_StartPnts2.clear();

    //Now we insert the start points for the Master movement
    m_StartPnts1.push_back(q);
    m_StartPnts1.push_back(p);

        //Inserts the start parameter
    m_StartParam.push_back((*m_it1)->FirstParameter());

    /*Master finished here*/

    /*-----------------------------Generate the first movement for the Slave--------------*/

    /* Fill p with the starting point of the first Slave curve*/
    (*m_it2)->D0((*m_it2)->FirstParameter(),p);

    /* Set q to the initial-Z-level in the Simulation: -5mm - Slave-Radius-Spring-Pretensionbelow the sheet upper level which is located at Z=0 */
    q.SetZ(-5.0 - set.slave_radius - m_pretension);

    /*Now we insert the start points for the Slave movement*/
    m_StartPnts2.push_back(q);
    m_StartPnts2.push_back(p);

        //Inserts the start parameter
        m_StartParam.push_back((*m_it2)->FirstParameter());

    /*Slave finished here*/
}

path_simulate::~path_simulate()
{
}

/*
double path_simulate::GetLength(GeomAdaptor_Curve& curve, const Standard_Real startParameter,const Standard_Real endParameter)
{
    Standard_Real firstParameter = curve.FirstParameter();
    Standard_Real lastParameter  = curve.LastParameter();


    Standard_Real sParam = Min(startParameter,endParameter);
    Standard_Real eParam = Max(startParameter,endParameter);

    //Standard_Real length = 0.0;

    if ( eParam > lastParameter )
    {
        //get the first part of the lenght
        Standard_Real l1 = GetLength(curve,firstParameter,eParam-lastParameter);
        Standard_Real l2 = GetLength(curve,sParam,lastParameter);
        return l1 + l2;
    }
    if ( sParam < firstParameter )
    {
        Standard_Real l1 = GetLength(curve,lastParameter-fabs(sParam),lastParameter);
        Standard_Real l2 = GetLength(curve,firstParameter,eParam);
        return l1 + l2;
    }


    return GCPnts_AbscissaPoint::Length(curve,sParam,eParam);   // genauigkeitssteuerung über parameter TOL nach eParam 
}
*/

/*
double path_simulate::FindParamAt(GeomAdaptor_Curve& curve, double dist, double startParam)
{
    double param;

    //compute the parameter of the next point
    GCPnts_AbscissaPoint absc(curve, dist, startParam);
    if ( absc.IsDone() )
    {
        //the parameter is computed
        param = absc.Parameter();
    }

    return param;
}
*/

/* Hier wird die absolute Geschwindigkeitsfunktion definiert und liefert die Geschwindigkeit zur Zeiteingabe <t>. 
Die Funktion gliedert sich in drei Abschnitte mit den Parametergrenzen <m_t0>, <m_t1>, <m_t2>, <m_T> welche z.B.
mittels path_simulate::ParameterCalculation() ermittelt werden kann. 
Die Start- und Endgeschwindigkeit müssen vorher in <m_v[0]> und <m_v[2]> bestimmt werden.
Die maximale Geschwindigkeit welche zwischen <m_t1> und <m_t2> erreicht werden soll, entspricht hier <m_v[1]> */
double path_simulate::GetVelocity(double t)
{
    double vel;
    double c[2];

    c[0] = m_a/2.0;

    if (t>=m_t0 && t<=m_t1)
    {
        if (t==m_t0 || (m_v[1] - m_v[0]) == 0.0)
        {
            vel = m_v[0];
        }
        else
        {
                        c[1] = PI*m_a / (m_v[1] - m_v[0]);
            vel = c[0]*(sin(c[1]*(t-m_t0) - PI)/c[1] + (t-m_t0)) + m_v[0];
        }
    }
    else if (t>m_t1  && t<=m_t2)
    {
        vel = m_v[1];
    }
    else if (t>m_t2 && t<=m_T)
    {
        if (t==m_T)
        {
            vel = m_v[1] - c[0]*(t-m_t2);
        }
        else
        {
                        c[1] = PI*m_a / (m_v[1] - m_v[2]);
            vel = m_v[1] - c[0]*(sin(c[1]*(t-m_t2) - PI)/c[1] + (t-m_t2));
        }
    }
    else
        throw Base::Exception("time input not inside [t0, T]");

    return vel;
}

/* Diese Funktion liefert den zurückgelegten Weg zur Zeiteingabe <t> und entspricht 
dem Integral der Funktion GetVelocity(t)*/
double path_simulate::GetDistance(double t)
{
    double d;
    double c[2];

    c[0] = m_a/2.0;

    if (t>=m_t0 && t<m_t1)
    {
        c[1] = PI*m_a / (m_v[1] - m_v[0]);

                if(t==m_t0){d = 0.0;}
                else{       d = -(c[0]/pow(c[1],2.0))*cos(c[1]*(t-m_t0) - PI) + 
                                      c[0]*pow((t-m_t0),2.0)/2 - c[0]/(c[1]*c[1]) + 
                                                  m_v[0]*(t-m_t0);}
    }
    else if (t>=m_t1  && t<=m_t2)
    {
        d = c[0]*pow((m_t1-m_t0),2.0)/2 + 
                        m_v[0]*(m_t1-m_t0) + 
                        m_v[1]*(t-m_t1);
    }
    else if (t>m_t2 && t<=m_T)
    {
        c[1] = PI*m_a / (m_v[1] - m_v[2]);

        if (t==m_T)
        {
            d = m_v[0]*(m_t1-m_t0) + c[0]*pow((m_t1-m_t0),2.0)/2 +
                                m_v[1]*(m_t2-m_t1) - c[0]*pow((t-m_t2),2.0)/2 + 
                                m_v[1]*(t-m_t2);
        }
                else
                {
                        d =  m_v[0]*(m_t1-m_t0) + c[0]*pow((m_t1-m_t0),2.0)/2 +
                                 m_v[1]*(m_t2-m_t1) + (c[0]/pow(c[1],2.0))*cos(c[1]*(t-m_t2) - PI) - 
                                                       c[0]*pow((t-m_t2),2.0)/2 + 
                                                                           c[0]/pow(c[1],2.0) + 
                                                                           m_v[1]*(t-m_t2);
                }
    }
    else
    {
        throw Base::Exception("time input not inside [t0,T]");
    }

    return d;
}

/*
double path_simulate::GetWireLength(TopoDS_Wire &aWire)
{
 GProp_GProps lProps;
 BRepGProp::LinearProperties(aWire,lProps);
 double length = lProps.Mass();
 return length;
}
*/

/*Parameterberechnung der Geschwindigkeitsfunktion für eine gerade Strecke der Länge <S1>*/
bool path_simulate::ParameterCalculation_Line(double S1)
{
    if (S1 == 0.0)  // hier gibts nichts zu tun
    {
        m_T = m_t0;
        return true;
    }

        m_a = m_amax;

        m_v[0] = 0.0;                  // Startgeschwindigkeit wird auf Null gesetzt
        m_v[1] = sqrt(m_a*S1/2.0);     // Geschwindigkeit die notwendig ist damit der Weg <S1> zur Zeit <m_T> erreicht wird
        m_v[2] = 0.0;                  // Endgeschwindigkeit wird auf Null gesetzt

        while(m_v[1] > m_vmax)         // maximale Geschwindigkeit darf nicht überschritten werden
        {
                m_a /= 2;                  // Versuchs erneut mit halber Beschleunigung
                m_v[1] = sqrt(m_a*S1/2.0); // Geschwindigkeit die notwendig ist damit der Weg <S1> zur Zeit <m_T> erreicht wird
        }

        // Jetzt lassen sich die Zeitgrenzen berechnen
    m_t1 = 2*m_v[1]/m_a + m_t0;
    m_t2 = m_t1;
    m_T = 2*m_t1 - m_t0;

    return true;
}

/*Parameterberechnung der Geschwindigkeitsfunktion (definiert in path_simulate::GetVelocity())
für einen Kurvenabschnitt der Länge <S1>. Aufruf muss stets vor der Funktion path_simulate::GetVelocity() erfolgen*/
bool path_simulate::ParameterCalculation_Curve(double S1)
{
        // Berechnung der Zeitgrenzen
    m_t1 = m_t0 + 2*(abs(m_v[1]-m_v[0]))/m_a;
    m_t2 = m_t1;
    m_T  = m_t1 + 2*(abs(m_v[1]-m_v[2]))/m_a;

    double tmp, v_tmp;
    tmp = GetDistance(m_T);  // liefert den Weg zurück, der unter den gegebenen Parametereinstellungen,
                                 // zum hoch- und runterbeschleunigen, midestens notwendig ist 

    if (tmp <= S1) // d.h. der Weg reicht aus
    {
        m_t2 = m_t1 + (S1 - tmp)/m_v[1];  // zwischen <m_t1> und <m_t2> wird die Kurve mit der konstanten 
                                                  // Geschwindigkeit <m_v[1]> durchlaufen
    }
    else // Weg reicht nicht aus -> Parameterkorrektur                            
    {
                // Berechne Geschwindigkeit die mindestenns notwendig ist damit der Weg <S1> zur Zeit <m_T> erreicht wird 
        m_v[1] = sqrt((m_a*S1 + m_v[0]*m_v[0] + m_v[2]*m_v[2])/2.0);  
                m_t1 = m_t0 + 2*(abs(m_v[1]-m_v[0]))/m_a;
        m_t2 = m_t1;
                
                // hier wird evtl. eine Korrektur notwendig
                if(m_v[1] > m_vmax)
                {
                        m_v[1] = m_vmax;
                        
                        // ab hier wieder analog zu oben
                        m_t1 = m_t0 + 2*(abs(m_v[1]-m_v[0]))/m_a;
                        m_t2 = m_t1;
                        m_T  = m_t1 + 2*(abs(m_v[1]-m_v[2]))/m_a;

                        tmp = GetDistance(m_T);

                        if (tmp <= S1)
                        {
                                m_t2 = m_t1 + (S1 - tmp)/m_v[1];
                        }
                        else
                        {
                                v_tmp = (m_vmax - std::min(m_v[1],m_v[2]))/2.0;
                                
                                while(tmp > S1) // hier wird die Geschwindigkeit <m_v[1]> solange in Richtung <m_v[2]> vekleinert bis
                                                    // der Weg schließlich ausreicht
                                {
                                        m_v[1] = std::min(m_v[1],m_v[2]) + v_tmp;
                        
                                        m_t1 = m_t0 + 2*(abs(m_v[1]-m_v[0]))/m_a;
                                    m_t2 = m_t1;
                                    m_T  = m_t1 + 2*(abs(m_v[1]-m_v[2]))/m_a;

                                    tmp = GetDistance(m_T);
                                
                                        v_tmp /= 2.0;
                                }
                        }
                }
    }

    m_T = m_t2 + 2*(abs(m_v[1]-m_v[2]))/m_a;  // Endzeit lässt sich jetzt berechnen

    return true;
}

/* setzt die Outputvektoren und den Beschleunigungsparameter <m_a> zurück. Die Startzeit <m_t0> wird aktualisiert*/
bool path_simulate::UpdateParam()
{
    m_Output.clear();
        m_Output2.clear();
    m_Output_time.clear();
    m_Output_time2.clear();

    m_t0 = m_T;     // Endzeit des letzten Durchlaufs wird zur neuen Startzeit
        m_a  = m_amax;
   
    return true;
}

/* Hilfsfunktion für die Zustellung. Rückgabewert legt fest ob zuerst in z- oder in xy-Richtung zugestellt wird*/
bool path_simulate::CheckConnect()
{
    gp_Pnt tmp;

    // ab dem 2. lauf
    if (m_it1 != m_BSplineTop.begin() || m_it2 != m_BSplineBottom.begin())
    {
        m_StartPnts1.clear(); 
        m_StartPnts2.clear(); 

        // Berechne neue Verbindungspunkte für die Zustellung - MASTER -
                m_it1--;

                (*m_it1)->D0((*m_it1)->LastParameter(),tmp);  // Speichert Endpunkt der vorigen Master-Kurve in <tmp>                   
                m_StartPnts1.push_back(tmp);                  // Pushe Endpunkt der vorigen Master-Kurve
                
                m_it1++; 
                
                (*m_it1)->D0((*m_it1)->FirstParameter(),tmp); // Speichert Startpunkt der vorigen Master-Kurve in <tmp>        
                m_StartPnts1.push_back(tmp);                  // Pushe Startpunkt der aktuellen Master-Kurve
                               
        if (m_single == false) // Falls beidseitig gefahren wird, mache dasselbe, wie für den Slave (s.o.)
        {
            // Berechne neue Verbindungspunkte für die Zustellung - SLAVE -
            m_it2--; 
                        
                        (*m_it2)->D0((*m_it2)->LastParameter(),tmp); // Speichert Startpunkt der vorigen Slave-Kurve in <tmp> 
                        m_StartPnts2.push_back(tmp);                             // Pushe Endpunkt der vorigen Slave-Kurve
            
                        m_it2++;            
                          
            (*m_it2)->D0((*m_it2)->FirstParameter(),tmp); // Speichert Startpunkt der aktuellen Slave-Kurve in <tmp>             
                        m_StartPnts2.push_back(tmp);                  // Pushe Startpunkt der aktuellen Slave-Kurve
        }
    }
    else
    {
        return true;  // Erste zustellung immer gleich (in negativer z-Richtung)
    }

    if (m_StartPnts1[0].Z() - m_StartPnts1[1].Z() >= 0.0) return true;  // Zustellung in negativer z-Richtung
    else                                                  return false; // Zustellung in positiver z-Richtung
}

/* Hilfsfunktion für die Zustellung. Rückgabewert legt fest ob zuerst in z- oder in xy-Richtung zugestellt wird*/
bool path_simulate::CheckConnect(bool tool)
{
    gp_Pnt tmp;

    // ab dem 2. lauf
    if (m_it1 != m_BSplineTop.begin() || m_it2 != m_BSplineBottom.begin())
    {
        if (m_Feat == true)  // Für den Feature-Basierten Fall werden die Zustellungen von Master und Slave seperat behandelt   
                {
                         if (!tool)
                         {
                                 m_StartPnts1.clear();
                                 
                                 m_it1--;
                                 
                                 (*m_it1)->D0((*m_it1)->LastParameter(),tmp);
                                 m_StartPnts1.push_back(tmp);  // Startpunkt
                                 
                                 m_it1++;
                                 
                                 (*m_it1)->D0((*m_it1)->FirstParameter(),tmp);
                                 m_StartPnts1.push_back(tmp);  // Zielpunkt

                                 if(m_StartPnts1[0].Z() - m_StartPnts1[1].Z() >= 0.0) return true;
                                 else                                                 return false;
                        }
                        else
                        {
                                 m_StartPnts2.clear();
                                 
                                 m_it2--;
                                 (*m_it2)->D0((*m_it2)->LastParameter(),tmp);
                                 m_it2++;
                                 m_StartPnts2.push_back(tmp);  // Startpunkt

                                 (*m_it2)->D0((*m_it2)->FirstParameter(),tmp);
                                 m_StartPnts2.push_back(tmp);  // Zielpunkt

                                 if(m_StartPnts2[0].Z() - m_StartPnts2[1].Z() >= 0.0) return true;
                                 else                                                 return false;
                        }
                }

                // Ab hier: Berechnung der Zustellungsvektoren für die synchrone Zustellung von Master und Slave
                m_StartPnts1.clear();
                m_StartPnts2.clear();

                // Berechne neue Verbindungspunkte für die Zustellung - MASTER
                m_it1--;
                
                (*m_it1)->D0((*m_it1)->LastParameter(),tmp);
                m_StartPnts1.push_back(tmp);  // Startpunkt
                
                m_it1++;
                
                (*m_it1)->D0((*m_it1)->FirstParameter(),tmp);
                m_StartPnts1.push_back(tmp);  // Zielpunkt

                if (m_single == false)
                {
                        // Berechne neue Verbindungspunkte für die Zustellung - SLAVE
                        m_it2--;
                        
                        (*m_it2)->D0((*m_it2)->LastParameter(),tmp);
                        m_StartPnts2.push_back(tmp);  // Startpunkt
                        
                        m_it2++;
                        
                        (*m_it2)->D0((*m_it2)->FirstParameter(),tmp);
                        m_StartPnts2.push_back(tmp);  // Zielpunkt
                }
    }
    else
    {
        return true;  // erste Zustellung immer gleich
    }

    if (m_StartPnts1[0].Z() - m_StartPnts1[1].Z() >= 0.0) return true;
    else                                                  return false;
}

/* Füllt die Outputvektoren für die erste Zustellung. Der Eingabeparameter <brob> legt den Ausgabetyp fest*/
bool path_simulate::ConnectPaths_xy(bool brob)
{
    int N;
    double t = m_t0;
      
    std::vector<Base::Vector3d> tmp2;
        std::vector<double> d;
        Base::Vector3d tmp;
        
        gp_Pnt tmpPnt, pnt1, pnt2, p;
        gp_Vec vec_t(m_StartPnts1[0], m_StartPnts1[1]);
        
        if( 1e-3 > vec_t.Magnitude())  // keine Zustellung erforderlich
                return true;

    if (m_single == false)
    {
        gp_Vec vec_1(m_StartPnts1[0], m_StartPnts1[1]); 
        gp_Vec vec_2(m_StartPnts2[0], m_StartPnts2[1]);

        gp_Vec2d vec_11, // Speichert Master-Zustellung in XY-Richtung
                             vec_21; // Speichert Master-Zustellung in  Z-Richtung

        vec_11.SetX(vec_1.X());  
        vec_11.SetY(vec_1.Y());

        if (m_it1 == m_BSplineTop.begin() && m_it2 == m_BSplineBottom.begin())  // erster lauf -> xy zustellung (Slave)
        {
            vec_21.SetX(vec_2.X());
            vec_21.SetY(vec_2.Y());
        }
        else
        {
            vec_21.SetX(0.0);
            vec_21.SetY(vec_2.Z());  // slave zustellung in z-Richtung (ab 2.Lauf)
        }

        // Simulationsoutput
        if (brob == false)
        {
                        // ***** MASTER ******
            
                        ParameterCalculation_Line(vec_11.Magnitude());

            if (vec_11.Magnitude() != 0)
                vec_11.Normalize();

            N = std::max(2, int(ceil((m_T - m_t0)/m_step))); // Anzahl der zu erzeugenden Outputwerte
            m_del_t = (m_T - m_t0)/N;                   // Zeitschrittweite

            for (int i=0; i<N; ++i)
            {
                                // Erzeuge XY-Outputvektor
                tmp.x = vec_11.X()*GetVelocity(t);
                tmp.y = vec_11.Y()*GetVelocity(t);
                tmp.z = 0.0;

                tmp2.push_back(tmp);
                                
                m_Output.push_back(tmp2);
                                m_Output_time.push_back(t);
                
                                t += m_del_t;
                                tmp2.clear();
                        }
                         
                        m_Output_time.push_back(m_T);

                        // ***** SLAVE ******
                        t = m_t0;
                        ParameterCalculation_Line(vec_21.Magnitude());

            if (vec_21.Magnitude() != 0)
                  vec_21.Normalize();

            N = std::max(2, int(ceil((m_T - m_t0)/m_step)));  // Anzahl der zu erzeugenden Outputwerte
            m_del_t = (m_T - m_t0)/N;                    // Zeitschrittweite

            for (int i=0; i<N; ++i)
            {
                if (m_it1 == m_BSplineTop.begin() && m_it2 == m_BSplineBottom.begin())
                {
                                        // Erzeuge XY-Outputvektor
                    tmp.x = vec_21.X()*GetVelocity(t);
                    tmp.y = vec_21.Y()*GetVelocity(t);
                    tmp.z = 0.0;
                }
                else
                {
                                        // Erzeuge Z-Outputvektor
                    tmp.x = 0.0;
                    tmp.y = 0.0;
                    tmp.z = vec_21.Y()*GetVelocity(t);
                }

                tmp2.push_back(tmp);
                                m_Output_time2.push_back(t);
                m_Output2.push_back(tmp2);
                

                                t += m_del_t;
                                tmp2.clear();
                        }
               
            m_Output_time2.push_back(m_T);

                        // Nullvektor
            tmp.x = 0.0;
            tmp.y = 0.0;
            tmp.z = 0.0;

            tmp2.push_back(tmp);

            m_Output.push_back(tmp2);
            m_Output2.push_back(tmp2);
        }
        else  // Roboter Output für den beidseitigen Fall
        {
            bool con = false;

            // hier wird die Anzahl <N> der Punkte für die Diskretisierung der Zustellungslinie berechnet
            if (vec_11.Magnitude() > vec_21.Magnitude()) N = std::max(2, int(ceil(vec_21.Magnitude()/TolDist)));
            else                                                     N = std::max(2, int(ceil(vec_11.Magnitude()/TolDist)));

            if (vec_11.Magnitude() == 0.0 && vec_21.Magnitude() == 0.0) N=0;
                        if (!m_conn) con = true;

            // Erster Punkt wird stets weggelassen
            if (m_it1 == m_BSplineTop.begin() && m_it2 == m_BSplineBottom.begin())
            {
                                // Startpunkt Master
                tmp.x = m_StartPnts1[0].X();
                tmp.y = m_StartPnts1[0].Y();
                tmp.z = m_StartPnts1[0].Z();
                
                                m_Output_robo1.push_back(tmp);
                RoboFlag_Master.push_back(0);

                                // Startpunkt Slave
                tmp.x = m_StartPnts2[0].X();
                tmp.y = m_StartPnts2[0].Y();
                tmp.z = m_StartPnts2[0].Z();
                
                                m_Output_robo2.push_back(tmp);
                RoboFlag_Slave.push_back(0);
            }

            // Erzeuge Output - MASTER
            for (int i=1; i<N; ++i)
            {
                tmp.x = m_StartPnts1[0].X() + (double(i)*vec_11.X())/(double(N)-1.0);
                tmp.y = m_StartPnts1[0].Y() + (double(i)*vec_11.Y())/(double(N)-1.0);
                tmp.z = m_StartPnts1[con].Z();

                m_Output_robo1.push_back(tmp);
                RoboFlag_Master.push_back(0);
            }

            // // Erzeuge Output - SLAVE
            for (int i=1; i<N; ++i)
            {
                if (m_it1 == m_BSplineTop.begin() && m_it2 == m_BSplineBottom.begin())
                {
                    tmp.x = m_StartPnts2[0].X() + (double(i)*vec_21.X())/(double(N)-1.0);
                    tmp.y = m_StartPnts2[0].Y() + (double(i)*vec_21.Y())/(double(N)-1.0);
                    tmp.z = m_StartPnts2[0].Z();
                }
                else
                {
                    tmp.x = m_StartPnts2[con].X();
                    tmp.y = m_StartPnts2[con].Y();
                    tmp.z = m_StartPnts2[0].Z() + (double(i)*vec_21.Y())/(double(N)-1.0);
                }
                
                                m_Output_robo2.push_back(tmp);
                RoboFlag_Slave.push_back(0);
            }
        }
    }
    else
    {
                gp_Vec2d vec_11,vec_12;
        gp_Vec vec_1(m_StartPnts1[0], m_StartPnts1[1]);

        vec_11.SetX(vec_1.X());
        vec_11.SetY(vec_1.Y());

        if(brob == false)
        {
            ParameterCalculation_Line(vec_11.Magnitude());

            if(vec_11.Magnitude() != 0.0) 
                                vec_11.Normalize();

                        N = std::max(2, (int)((m_T-m_t0)/m_step));
            m_del_t = (m_T-m_t0)/double(N);

            for(int i=0; i<N; ++i)
            {    
                                // Erzeuge XY-Outputvektor
                tmp.x = vec_11.X()*GetVelocity(t);
                tmp.y = vec_11.Y()*GetVelocity(t);
                tmp.z = 0.0;

                tmp2.push_back(tmp);

                                m_Output_time.push_back(t);
                m_Output.push_back(tmp2);
                
                t += m_del_t;
                                tmp2.clear();
            }

            m_Output_time.push_back(m_T);

                        // Nullvektor 
            tmp.x = 0.0;
            tmp.y = 0.0;
            tmp.z = 0.0;

            tmp2.push_back(tmp);
            m_Output.push_back(tmp2);
            tmp2.clear();
        }
        else
        {
            N = (int) vec_11.Magnitude();  // Anzahl der zu erzeugenden Outputwerte

            for (int i=0; i<N; ++i)
            {
                tmp.x = m_StartPnts1[0].X() + (i*vec_11.X())/N;
                tmp.y = m_StartPnts1[0].Y() + (i*vec_11.Y())/N;
                tmp.z = m_StartPnts1[0].Z();

                m_Output_robo1.push_back(tmp);
            }
        }
    }
    
        return true;
}

/* Füllt die Outputvektoren für die zweite Zustellung. Der Eingabeparameter <brob> legt den Ausgabetyp fest*/
bool path_simulate::ConnectPaths_z(bool brob)
{
    int N;
    double t = m_t0;

    Base::Vector3d tmp;
    std::vector<double> d;
    std::vector<Base::Vector3d> tmp2;

        gp_Vec vec_t(m_StartPnts1[0], m_StartPnts1[1]);
        if( 1e-3 > vec_t.Magnitude())
                return true;

    if (m_single == false)
    {
        gp_Vec vec_1(m_StartPnts1[0], m_StartPnts1[1]);
        gp_Vec vec_2(m_StartPnts2[0], m_StartPnts2[1]);

        gp_Vec2d vec_11,vec_12;

        vec_11.SetX(0.0);
        vec_11.SetY(vec_1.Z());

        if (m_it1 == m_BSplineTop.begin() && m_it2 == m_BSplineBottom.begin())
        {
            vec_12.SetX(0.0);
            vec_12.SetY(vec_2.Z());
        }
        else
        {
            vec_12.SetX(vec_2.X());
            vec_12.SetY(vec_2.Y());
        }

                if (brob == false)
        {
                        // ***** MASTER ******            
                        ParameterCalculation_Line(vec_11.Magnitude());

            if (vec_11.Magnitude() != 0)
                vec_11.Normalize();

            N = std::max(2, int(ceil((m_T - m_t0)/m_step)));  // Anzahl der zu erzeugenden Outputwerte
            m_del_t = (m_T - m_t0)/N;                    // Zeitschrittweite

            for (int i=0; i<N; ++i)
            {
                                // Erzeuge Z-Outputvektor
                tmp.x = 0.0;
                tmp.y = 0.0;
                tmp.z = vec_11.Y()*GetVelocity(t); 

                tmp2.push_back(tmp);
                                
                m_Output.push_back(tmp2);
                                m_Output_time.push_back(t);
                
                                t += m_del_t;
                                tmp2.clear();
                        }
                         
                        m_Output_time.push_back(m_T);

                        // ***** SLAVE ******
                        t = m_t0;
                        ParameterCalculation_Line(vec_12.Magnitude());

            if (vec_12.Magnitude() != 0)
                  vec_12.Normalize();

            N = std::max(2, int(ceil((m_T - m_t0)/m_step)));  // Anzahl der zu erzeugenden Outputwerte
            m_del_t = (m_T - m_t0)/N;                    // Zeitschrittweite

            for (int i=0; i<N; ++i)
            {
                if (m_it1 == m_BSplineTop.begin() && m_it2 == m_BSplineBottom.begin())
                {
                                        // Erzeuge Z-Outputvektor im allerersten Schritt
                    tmp.x = 0.0;
                    tmp.y = 0.0;
                    tmp.z = vec_12.Y()*GetVelocity(t);
                }
                else // Slave Zustellung in XY-Richtung
                {
                                        // Erzeuge XY-Outputvektor
                    tmp.x = vec_12.X()*GetVelocity(t);   
                    tmp.y = vec_12.Y()*GetVelocity(t);
                    tmp.z = 0.0;
                }

                tmp2.push_back(tmp);
                                m_Output_time2.push_back(t);
                m_Output2.push_back(tmp2);
                

                                t += m_del_t;
                                tmp2.clear();
                        }
               
            m_Output_time2.push_back(m_T);

            tmp.x = 0.0;
            tmp.y = 0.0;
            tmp.z = 0.0;

            tmp2.push_back(tmp);

            m_Output.push_back(tmp2);
            m_Output2.push_back(tmp2);
        }
        else  // Roboteroutput
        {
            if (vec_11.Magnitude() > vec_12.Magnitude()) std::max(2, N = int(ceil(vec_12.Magnitude()/TolDist)));  // Anzahl der zu erzeugenden Outputwerte
            else                                         std::max(2, N = int(ceil(vec_11.Magnitude()/TolDist)));  // Anzahl der zu erzeugenden Outputwerte
    
            if (vec_11.Magnitude() == 0.0 && vec_12.Magnitude() == 0.0) N=1;

            for (int i=1; i<N; ++i)
            {
                                /*MASTER*/

                                // Erzeuge Outputvektor für die Zustellung in Z-Richtung
                tmp.x = m_StartPnts1[m_conn].X();
                tmp.y = m_StartPnts1[m_conn].Y();
                tmp.z = m_StartPnts1[0].Z() + (double(i)*vec_11.Y())/double(N-1);

                m_Output_robo1.push_back(tmp);
                RoboFlag_Master.push_back(0);



                                /*SLAVE*/

                if (m_it1 == m_BSplineTop.begin() && m_it2 == m_BSplineBottom.begin())
                {   
                                        // Erzeuge Outputvektor für die Zustellung in Z-Richtung (nur im allerersten Schritt)
                    tmp.x = m_StartPnts2[1].X();
                    tmp.y = m_StartPnts2[1].Y();
                    tmp.z = m_StartPnts2[0].Z() + (double(i)*vec_12.Y())/double(N-1);
                }
                else
                {
                                        // Erzeuge Outputvektor für die Zustellung in XY-Richtung
                    tmp.x = m_StartPnts2[0].X() + (double(i)*vec_12.X())/double(N-1);
                    tmp.y = m_StartPnts2[0].Y() + (double(i)*vec_12.Y())/double(N-1);
                    tmp.z = m_StartPnts2[m_conn].Z();
                }

                m_Output_robo2.push_back(tmp);
                RoboFlag_Slave.push_back(0);
            }
        }
    }
    else
    {
        gp_Vec vec_1(m_StartPnts1[0], m_StartPnts1[1]);
        gp_Vec2d vec_12;

        vec_12.SetX(0.0);
        vec_12.SetY(vec_1.Z());

        if (brob == false)
        {
            ParameterCalculation_Line(vec_12.Magnitude());
                        
                        if (vec_12.Magnitude() != 0)
                  vec_12.Normalize();

            N = std::max(2, int(ceil((m_T - m_t0)/m_step)));  // Anzahl der zu erzeugenden Outputwerte
            m_del_t = (m_T - m_t0)/N;                    // Zeitschrittweite

            for (int i=0; i<N; ++i)
            {
                m_Output_time.push_back(t);

                                // Erzeuge Z-Outputvektor
                tmp.x = 0.0;
                tmp.y = 0.0;
                tmp.z = vec_12.Y()*GetVelocity(t);

                tmp2.push_back(tmp);
                m_Output.push_back(tmp2);
                
                t += m_del_t;
                                tmp2.clear();
            }

            t = m_T;

            m_Output_time.push_back(t);

                        // Nullvektor
            tmp.x = 0.0;
            tmp.y = 0.0;
            tmp.z = 0.0;

            tmp2.push_back(tmp);
            m_Output.push_back(tmp2);
            tmp2.clear();
        }
        else // Roboteroutput
        {
            N =(int) vec_12.Magnitude();  // Anzahl der zu erzeugenden Outputwerte

            for (int i=0; i<N; ++i)
            {
                                // Erzeuge Outputvektor für die Zustellung in XY-Richtung
                tmp.x = m_StartPnts1[0].X() + (i*vec_12.X())/N;
                tmp.y = m_StartPnts1[0].Y() + (i*vec_12.Y())/N;
                tmp.z = m_StartPnts1[0].Z();

                m_Output_robo1.push_back(tmp);
            }
        }
    }

    return true;
}

/* Füllt die Outputvektoren für die Zustellung für den Feature-Basierten Fall. 
Der Eingabeparameter <brob> legt den Ausgabetyp fest*/
bool path_simulate::ConnectPaths_Feat(bool tool,  // Tool           (Master, Slave)
                                      bool brob,  // Ausgabetyp     (Roboter, Simulation)
                                      bool c_typ) // Zustellungsart (in zwei bzw. drei Schritten)
{
    int N, ind;
    double rad, t;
    bool dir;

    std::vector<double> Times;
    std::vector<gp_Pnt> ConnPnts;
    std::vector< std::vector<Base::Vector3d> > Out;
    gp_Vec vec[3], vec_tmp[3];

    Base::Vector3d tmp;
    std::vector<double> d;
    std::vector<Base::Vector3d> tmp2;
    double vel;

    dir = CheckConnect(tool);  // setze Starpunkte neu

    if (!tool)
    {
        ConnPnts = m_StartPnts1;
        rad = m_set.master_radius;
    }
    else
    {
        ConnPnts = m_StartPnts2;
        rad = m_set.slave_radius;
    }

    if (c_typ)  // Zustellung in 2 Schritten
    {
        ind = 2;
        vec_tmp[0].SetCoord(0.0, 0.0, abs(ConnPnts[1].Z()-ConnPnts[0].Z()));
        vec_tmp[1].SetCoord(ConnPnts[1].X()-ConnPnts[0].X(), ConnPnts[1].Y()-ConnPnts[0].Y(), 0.0);

        if (dir) // tool  (Master/Slave) muss runter fahren
        {
            if (!tool)
            {
                vec[0] =  vec_tmp[1];    // tool = Master
                vec[1] = -vec_tmp[0];
            }
            else
            {
                vec[0] = -vec_tmp[0];    // tool = Slave
                vec[1] =  vec_tmp[1];
            }
        }
        else   // tool  (Master/Slave) muss hoch fahren
        {
            if (!tool)
            {
                vec[0] = vec_tmp[0];    // tool = Master
                vec[1] = vec_tmp[1];
            }
            else
            {
                vec[0] = vec_tmp[1];    // tool = Slave
                vec[1] = vec_tmp[0];
            }
        }
    }
    else // Zustellung in 3 Schritten
    {
        ind = 3;

        vec_tmp[0].SetCoord(0.0, 0.0, rad);
        vec_tmp[1].SetCoord(ConnPnts[1].X()-ConnPnts[0].X(), ConnPnts[1].Y()-ConnPnts[0].Y(), 0.0);
        vec_tmp[2].SetCoord(0.0, 0.0, abs(ConnPnts[1].Z()-ConnPnts[0].Z()) + rad);

        if (dir) // tool  (Master/Slave) muss runter fahren
        {
            if (!tool)
            {
                vec[0] =  vec_tmp[0];    // tool = Master
                vec[1] =  vec_tmp[1];
                vec[2] = -vec_tmp[2];
            }
            else
            {
                vec[0] = -vec_tmp[2];    // tool = Slave
                vec[1] =  vec_tmp[1];
                vec[2] =  vec_tmp[0];
            }
        }
        else   // tool  (Master/Slave) muss hoch fahren
        {
            if (!tool)
            {
                vec[0] =  vec_tmp[2];    // tool = Master
                vec[1] =  vec_tmp[1];
                vec[2] = -vec_tmp[0];
            }
            else
            {
                vec[0] = -vec_tmp[0];    // tool = Slave
                vec[1] =  vec_tmp[1];
                vec[2] =  vec_tmp[2];
            }
        }
    }

    if (brob) // Roboteroutput
        {   
                for (int i=0; i<ind; ++i)
                {
                        N = std::max(2, int(ceil(vec[i].Magnitude()/TolDist)));  // Anzahl der zu erzeugenden Outputwerte

                        for(int j=1; j<N; ++j)
                        {
                                tmp.x = ConnPnts[0].X() + (double(j)*vec[i].X())/(double(N)-1.0);
                                tmp.y = ConnPnts[0].Y() + (double(j)*vec[i].Y())/(double(N)-1.0);
                                tmp.z = ConnPnts[0].Z() + (double(j)*vec[i].Z())/(double(N)-1.0);;

                                if(!tool) m_Output_robo1.push_back(tmp);  // Master
                                else      m_Output_robo2.push_back(tmp);  // Slave
                        }

                        ConnPnts[0].SetCoord(tmp.x , tmp.y, tmp.z); // Setze Startpunkt = Endpunkt für die nächste Iteration
                }

                return true;
        }
        
        if(ConnPnts[0].Distance(ConnPnts[1]) < 1e-3) return true;  // keine Zustellung bei Spiralbahnen !!!

    for (int i=0; i<ind; ++i)
    {
        t = m_t0;
        ParameterCalculation_Line(vec[i].Magnitude());
        if (vec[i].Magnitude() != 0.0) vec[i].Normalize();
        else continue;

        N = std::max(2,(int) ceil((m_T - m_t0)/m_step)); // Anzahl der zu erzeugenden Outputwerte
        m_del_t = (m_T - m_t0)/N;                   // Zeitschrittweite

        for (int j=0; j<N; ++j)
        {
            Times.push_back(t);
            vel = GetVelocity(t);

            tmp.x = vec[i].X()*vel;
            tmp.y = vec[i].Y()*vel;
            tmp.z = vec[i].Z()*vel;

            tmp2.push_back(tmp);
            Out.push_back(tmp2);  // Fülle temporären Output-Vektor (wird weiter unten zugewiesen)

            t += m_del_t;
                        tmp2.clear();
        }

        m_t0 = m_T; // Endzeit des letzten Durchlaufs wird zur neuen Startzeit
    }

    Times.push_back(m_T);

    tmp.x = 0.0;
    tmp.y = 0.0;
    tmp.z = 0.0;

    tmp2.clear();
    tmp2.push_back(tmp);
    Out.push_back(tmp2);

    if(!tool)
    {
        m_Output      = Out;
        m_Output_time = Times;
    }
    else
    {
        m_Output2      = Out;
        m_Output_time2 = Times;
    }

    return true;  
}

/* Hier wird unter Berücksichtigung der Krümmungstoleranz <curvTOL> eine Unterteilung der Kurve vorgenommen.
Die Bereichsgrenzen werden im Rückgabevektor zurückgeliefert. Der Ausgabevektor ist leer wenn die maximale Krümmung
den Toleranzwert nicht überschreitet und somit auch keine Unterteilung notwendig ist */ 
std::vector<std::vector<double> > path_simulate::CompBounds(bool tool,std::vector<double> knots)
{
        m_curMax = 0.0; // setze maximale Krümmung initial auf Null     
        double cr_bound = 1/curvTOL;
        double cr_last;
        gp_Vec dtmp1, dtmp2;
        gp_Pnt dtmp0;
    GeomAdaptor_Curve curve;
        std::vector<double> single_bound;
    std::vector<double> bounds;
    std::vector<std::vector<double> > CriticalBounds;

    // lade aktuelle Kurve
    if (!tool)  curve.Load(*m_it1);
    else        curve.Load(*m_it2);

    double fParam = curve.FirstParameter(), // Erster Kurvenparameter
           lParam = curve.LastParameter(),  // Letzter Kurvenparameter
           period = lParam - fParam;        // Länge des Parameterbereichs
 

        int n  = knots.size();  // Länge des Knotenvektors
        bool b = false;

        // Hier erfolgt die Berechnung der maximalen Krümmung <m_curMax>
        // Die Parameter der Bereichsgrenzen an denen die Kurvenkrümmung dem Toleranzwert <cr_bound> 
        // entspricht werden in den Vektor <bounds> für die weitere Nachbearbeitung gefüllt
    for (int i=0; i<n; ++i)
    {
                curve.D2(knots[i], dtmp0, dtmp1, dtmp2); // Da es sich um eine kubische B-Spline Kurve handelt,
                                                         // kann die maximale Krümmung nur an den Knotenpunkten angenomen werden
                if(dtmp2.Magnitude() >= cr_bound && !b)
                {
                        if(knots[i] >= m_boundTol && knots[i] < lParam - m_boundTol)
                        {
                                if(cr_last < cr_bound)
                                {
                                        bounds.push_back(knots[i-1] + (knots[i] - knots[i-1])*
                                                                      (cr_last - cr_bound)/
                                                                                          (cr_last - dtmp2.Magnitude()));
                                }
                                else
                                {
                                        bounds.push_back(fParam + m_boundTol);
                                }
                                b = true;
                        }
                }
                
                if(dtmp2.Magnitude() < cr_bound && b)
                {
                        if(knots[i] <= lParam - m_boundTol)
                        {
                                bounds.push_back(knots[i-1] + (knots[i] - knots[i-1])*
                                                                  (cr_bound - cr_last)/
                                                                                          (dtmp2.Magnitude() - cr_last));
                        }
                        else
                        {
                                bounds.push_back(lParam - m_boundTol);
                                
                        }
                        b = false;
                }
                
                cr_last = dtmp2.Magnitude(); // Krümmung entspricht hier dem Betrag der zweiten Ableitung
                        
                if(m_curMax < cr_last)  // Speichert maximale Krümmung
                        m_curMax = cr_last;
        }

        if(period < 2*m_boundTol || bounds.size() == 0)
                return CriticalBounds;

        if(b) bounds.push_back(lParam - m_boundTol);  // Hier muss evtl. noch die letzte Grenze eingefügt werden

        n = (int) bounds.size()/2;  // <bounds> hat stets gerade Länge
        for(int i=0; i<n; i++)      // Fasse Bereiche welche einen geringeren Abstand als <m_boundTol> haben zusammen
        {
                single_bound.push_back(bounds[2*i]);
                
                while(i<n-1 && bounds[2*i+2] - bounds[2*i+1] < m_boundTol)
                        i++;
                
                single_bound.push_back(bounds[2*i+1]);
        CriticalBounds.push_back(single_bound);
        single_bound.clear();
    }

    return CriticalBounds;
}

/* Hier wird die Vorarbeit für die Funktion Gen_Path() geleistet.
Die Vektoren <m_length>, <m_velocity>, <m_accel> werden für die aktuelle Kurve gefüllt.*/ 
bool path_simulate::CompPath(bool tool) // tool = 0  -> Master
                                        // tool = 1  -> Slave
{
        m_boundTol = pow(m_vmax, 2.0)/m_amax; // Toleranzbereich vor kritischen Bereichen (notwendig zum Hochbeschleunigen)
        
        double cur     = 1.0/curvTOL,         // Krümmungstoleranz für die Kurvenunterteilung
               pos     = 0.0, 
               cur_tmp = 0.0;

        int    nb_knots;

        std::vector<std::vector<double> > v_vec;
        std::vector<double> v(3), l_vec, a_vec;
        
        std::vector<Base::Vector3d> Pnt1Vec;
    
        GeomAdaptor_Curve curve;

    gp_Pnt pnt0;
    gp_Vec pnt, pnt1, pnt2, vec;
    Base::Vector3d Pnt1;
    
    double start,
                   fParam,
           lParam,
                   period;

        double d2, velo, tetha, 
                   len, len_1;
   
        double t0 = m_t0; // Übergibt aktuelle Startzeit

        int num = Detect_FeatCurve(tool);  // Liefert Anzahl der zu fahrenden Kurven (i.d.R. num = 1) 
        
        for(int a=0; a<num; a++)  // Schleife über die zusammenhängenden Kurven
        {
                // lade aktuelle kurven
            if (!tool)  curve.Load(*m_it1);
                else        curve.Load(*m_it2);

                // setze Parameter neu
                fParam = curve.FirstParameter();
                lParam = curve.LastParameter();
                period = lParam - fParam;

                // übergibt Starparameter der aktuellen Kurve
                m_StartParam[tool] = fParam;
                start = m_StartParam[tool];
                
                // *** Berechnung des Knotenvektors ***
                if (!tool)
                {
                        nb_knots = (*m_it1)->NbKnots();
                        m_Knots  = new TColStd_Array1OfReal(0,nb_knots-1);
                   (*m_it1)->Knots(*m_Knots);
                }
                else
                {
                        nb_knots = (*m_it2)->NbKnots();
                        m_Knots  = new TColStd_Array1OfReal(0,nb_knots-1);
                   (*m_it2)->Knots(*m_Knots);
                }
                
                std::vector<double> knot_vec(m_Knots->Length());
                for(int i=0; i<m_Knots->Length(); i++)
                        knot_vec[i] = m_Knots->Value(i);

                // *** Ende der Berechnung des Knotenvektors ***


                std::vector<std::vector<double> > CriticalBounds = 
                                                        CompBounds(tool, knot_vec); // Berechnet auf Basis der aktuellen Kurve, 
                                                                                                                                            // die kritischen Bereiche im Parameterraum und 
                                                                                                                                            // berechnet gleichzeitig die maximale Kurvenkrümmung <m_curMax>
                
                m_vmid = std::min(m_vmax,sqrt(m_amax/m_curMax)); // Legt Geschwindigkeit fest mit der allen kritischen Bereiche     
                                                            // abgefahren werden

Newtry: // Falls die generierten Weglängen nicht ausreichen, dann wird ein neuer Versuch mit halbem <m_vmid> gestartet
                
                v[0] = 0.0;  // starte jede Kurve mit v = 0
                int m = 0;

                for (unsigned int i=0; i<CriticalBounds.size(); ++i)  // Schleife über einzelne Unterteilungsbereiche
                {
                        d2 = 0.0;
                        pos = m_Knots->Value(m);

                        while(pos < m_StartParam[tool])
                        {
                                m++;
                                pos = m_Knots->Value(m);
                        }
                        
                        /*------------------- Gerader Bereich ---------------------*/
                        
                        while(pos < CriticalBounds[i][0]) // Berechnung der maximalen Krümmung dieses geraden Bereichs
                        {
                                curve.D2(pos, pnt0, pnt1, pnt2);
                                cur_tmp = pnt2.Magnitude();   // Krümmung am aktuellen Punkt
                                
                                if(d2 < cur_tmp) 
                                        d2 = cur_tmp;             // Speichert maximale Krümmung dieses geraden Bereichs

                                m++;
                                pos = m_Knots->Value(m);
                        }

                        tetha = 0.6 + 0.25*sqrt(d2/m_curMax);         // Setzt tetha-parameter -> (0.6 < tetha < 0.85)
                        velo = std::min(m_vmax, tetha*(sqrt(m_amax/d2)));  // Setzt maximale Geschwindigkeit <velo>
                        m_a = m_amax - d2*velo*velo;                  // wenn <velo> zu groß gewählt wurde, kann <m_a> evtl. negativ werden

                        // Korrektur
                        while(m_a <= 0.0)
                        {
                                velo = velo/2.0;
                                m_a = m_amax - d2*velo*velo;
                        }

                        if(velo < m_vmid)
                                m_vmid = velo;

                        v[1] = velo;

                        /*-------------- Korrektur der Geschwindigkeiten (falls der Weg zu lang) -------------*/

                        len = CriticalBounds[i][0] - m_StartParam[tool];              // Länge des i. geraden Abschnitts
                        len_1 = (pow(v[1] - v[0],2.0) + pow(v[1] - m_vmid,2.0))/m_a;  // notwendige Länge

                        if(len < pow(v[0] - m_vmid,2.0)/m_a)  // für diesen Fall ist keine Korrektur möglich
                        {
                                l_vec.clear();
                                v_vec.clear();
                                a_vec.clear();

                                m_StartParam[tool] = start; // setze Startparameter zurück
                            m_vmid = m_vmid/2;          // halbiere kritische Durchlaufgeschwindigkeit
                                goto Newtry;
                        }

                        while(len < len_1)
                        {
                                v[1] = v[0] + (v[1] - v[0])/2.0;
                                len_1 = (pow(v[1] - v[0],2.0) + pow(v[1] - m_vmid,2.0))/m_a; 
                        }

                        v[2] = m_vmid;

                        /*---Korrekturende---*/ 

                        // Fülle Vektoren
                        l_vec.push_back(len);  // Länge
                        v_vec.push_back(v);    // Geschwindigkeiten
                        a_vec.push_back(m_a);  // Beschleunigung
                
                        m_StartParam[tool] += len;   // setzt Startparameter neu

                        /*------------------------ gekrümmter Bereich ------------------------*/

                        len = CriticalBounds[i][1] - CriticalBounds[i][0];  // Bogenlänge des gekrümmten Bereichs
                        m_StartParam[tool] += len;                          // setzt Startparameter neu
                        v[0] = v[2];                                        // Endgeschwindigkeit wird zur Startgeschwindigkeit
                        
                        l_vec.push_back(len);
                }

                d2 = 0; 
                
                // Korrigiere aktuellen Knotenparameter
                while(pos < m_StartParam[tool])
                {
                        m++;
                        pos = m_Knots->Value(m);
                }

                pos = m_Knots->Value(m);
            
                // Berechnung der maximalen Krümmung für den letzten Abschnitt
                while(pos < lParam)
                {               
                        curve.D2(pos, pnt0, pnt1, pnt2);
                    
                        Pnt1.x = pnt2.X();
                        Pnt1.y = pnt2.Y();
                        Pnt1.z = pnt2.Z();
         
                        if(d2 < Pnt1.Length()) d2 = Pnt1.Length();

                        m++;
                        pos = m_Knots->Value(m);
                }

                tetha = 0.6 + 0.25*sqrt(d2/m_curMax);        // 0.6 < tetha < 0.85
                velo = std::min(m_vmax, tetha*(sqrt(m_amax/d2))); // vgl. oben
                        
                v[1] = velo;
                v[2] = 0.0;
                m_a = m_amax - d2*velo*velo; // wenn velo zu groß gewählt wurde, kann m_a negativ werden

                // Korrektur
                while(m_a <= 0.0)
                {
                        velo = velo/2.0;
                        v[1] = velo;
                        m_a = m_amax - cur*velo*velo;
                }

                d2 = 0;

                len = lParam - m_StartParam[tool] + start;          // Länge des letzten geraden Abschnitts
                len_1 = (pow(v[1] - v[0],2.0) + pow(v[1],2.0))/m_a; // notwendige Länge

                if(len < pow(v[0],2.0)/m_a) // hier keine Korrektur möglich
                {
                        l_vec.clear();
                        v_vec.clear();
                        a_vec.clear();

                        m_StartParam[tool] = start; // setze Startparameter zurück
                        m_vmid = m_vmid/2;          // halbiere kritische Durchlaufgeschwindigkeit und versuchs erneut
                        goto Newtry;
                }

                while(len < len_1)
                {
                        v[1] = v[0] + (v[1] - v[0])/2;
                        len_1 = (pow(v[1] - v[0],2.0) + pow(v[1],2.0))/m_a; 
                }

                // Fülle Vektoren
                l_vec.push_back(len);  // Länge
                v_vec.push_back(v);    // Geschwindigkeiten
                a_vec.push_back(m_a);  // Beschleunignung
                
                // Fülle hier erst die Ausgabevektoren (einmal pro Kurve)
                if(tool)
                {       
                        m_length_sl.push_back(l_vec);
                        m_velocity_sl.push_back(v_vec);
                        m_accel_sl.push_back(a_vec);
                }
                else
                {
                        m_length_ma.push_back(l_vec);
                        m_velocity_ma.push_back(v_vec);
                        m_accel_ma.push_back(a_vec);
                }

                // lade aktuelle kurven
            if (!tool)  m_it1++;
                else        m_it2++;

                m_StartParam[tool] = start;
                
                l_vec.clear();
                v_vec.clear();
                a_vec.clear();
                
                curve.Delete();
                CriticalBounds.clear();
        }

        // Setze Iterator und Startparameter wieder zurück
        if (!tool)
        { 
                for(int i=0; i<num; i++){m_it1--;}
                m_StartParam[tool] = (*m_it1)->FirstParameter();
                
        }
        else
        { 
                for(int i=0; i<num; i++){m_it2--;}
                m_StartParam[tool] = (*m_it2)->FirstParameter();
        }

    return true;
}

/* Integriert erst nach der Trapezregel die Outputwerte nach der Zeit und liefert den Fehlervektor als Verbindung von Start- und Endpunkt.
Abschließend erfolgt die Korrektur der Outputvektoren um eben diesen Fehlervektor */
bool path_simulate::Correction(bool tool)
{
    int N;
    gp_Vec vec;
        gp_Pnt pnt;
    Base::Vector3d tmp;
    std::vector<Base::Vector3d> tmp2;

        double Sum[3];

        Sum[0] = 0.0;
    Sum[1] = 0.0;
    Sum[2] = 0.0;

        // Hier erfolgt die numerische Integration des Outputvektors nach der Trapezregel und der
        // entstehende Fehlervektor wird in <vec> abgelegt
    if (tool==false) // Master
    {
                N = m_Output.size();
        for (int i=1; i<N; ++i)
        {
            Sum[0] += (m_Output[i][0].x + m_Output[i-1][0].x)*(m_Output_time[i] - m_Output_time[i-1]) / 2.0;
            Sum[1] += (m_Output[i][0].y + m_Output[i-1][0].y)*(m_Output_time[i] - m_Output_time[i-1]) / 2.0;
            Sum[2] += (m_Output[i][0].z + m_Output[i-1][0].z)*(m_Output_time[i] - m_Output_time[i-1]) / 2.0;
        }

                if(m_StartPnts1[0].Distance(m_StartPnts1[1]) > 1e-3)  // falls Kurve NICHT geschlossen
                {
                        vec.SetCoord(Sum[0],Sum[1],Sum[2]);
                        vec.SetCoord(m_StartPnts1[1].X() - m_StartPnts1[0].X() - vec.X(),
                                                 m_StartPnts1[1].Y() - m_StartPnts1[0].Y() - vec.Y(),
                                                 m_StartPnts1[1].Z() - m_StartPnts1[0].Z() - vec.Z());
                }
                else
                        vec.SetCoord(-Sum[0],-Sum[1],-Sum[2]);
    }
    else  // Slave
    {
                N = m_Output2.size();
        for (int i=1; i<N; ++i)
        {
            Sum[0] += (m_Output2[i][0].x + m_Output2[i-1][0].x)*(m_Output_time2[i] - m_Output_time2[i-1]) / 2.0;
            Sum[1] += (m_Output2[i][0].y + m_Output2[i-1][0].y)*(m_Output_time2[i] - m_Output_time2[i-1]) / 2.0;
            Sum[2] += (m_Output2[i][0].z + m_Output2[i-1][0].z)*(m_Output_time2[i] - m_Output_time2[i-1]) / 2.0;
        }

                if(m_StartPnts2[0].Distance(m_StartPnts2[1]) > 1e-3)  // falls Kurve NICHT geschlossen
                {
                        vec.SetCoord(Sum[0],Sum[1],Sum[2]);
                        vec.SetCoord(m_StartPnts2[1].X() - m_StartPnts2[0].X() - vec.X(),
                                                 m_StartPnts2[1].Y() - m_StartPnts2[0].Y() - vec.Y(),
                                                 m_StartPnts2[1].Z() - m_StartPnts2[0].Z() - vec.Z());
                }
                else
                        vec.SetCoord(-Sum[0],-Sum[1],-Sum[2]);
    }

    ParameterCalculation_Line(vec.Magnitude());  // Berechnung der Zeitgrenzen

    N = (int) ceil((m_T - m_t0)/m_step);  // Anzahl der zu erzeugenden Outputwerte
    m_del_t = (m_T - m_t0)/N;             // Zeitschrittweite

    if (N==1)  // Nur ein Outputvektor
    {
        m_T = m_t0 + 2e-3;

        if (tool==false) // Master
        {
            m_Output_time.push_back(m_t0);
            m_Output_time.push_back(m_t0 + 1e-3);
        }
        else // Slave
        {
            m_Output_time2.push_back(m_t0);
            m_Output_time2.push_back(m_t0 + 1e-3);
        }

        tmp.x = 0.0;
        tmp.y = 0.0;
        tmp.z = 0.0;

        tmp2.push_back(tmp);

        if(tool==false)  m_Output.push_back(tmp2);  // füllt Master-Output
        else             m_Output2.push_back(tmp2); // füllt Slave-Output

        tmp2.clear();

        tmp.x = (vec.X()/vec.Magnitude())*(vec.X()/1e-3);
        tmp.y = (vec.Y()/vec.Magnitude())*(vec.Y()/1e-3);
        tmp.z = (vec.Z()/vec.Magnitude())*(vec.Z()/1e-3);

        tmp2.push_back(tmp);

        if (tool==false) m_Output.push_back(tmp2);
        else             m_Output2.push_back(tmp2);

        tmp2.clear();
    }
    else
    {
        double vel,t = m_t0;
        if (vec.Magnitude() != 0) vec.Normalize();

        for (int i=0; i<N; ++i)
        {
            if (tool==false) m_Output_time.push_back(t);
            else             m_Output_time2.push_back(t);

            // MASTER
                        vel = GetVelocity(t);

                        // Ausgabevektor zum Zeitpunkt <t>
                        tmp.x = vec.X()*vel;
            tmp.y = vec.Y()*vel;
            tmp.z = vec.Z()*vel;

            tmp2.push_back(tmp);
          
                        if (tool==false) m_Output.push_back(tmp2);
            else             m_Output2.push_back(tmp2);

            t += m_del_t;
            tmp2.clear();
        }
    }

        // Nullvektor
        tmp.x = 0.0;
    tmp.y = 0.0;
    tmp.z = 0.0;
    
        tmp2.push_back(tmp);
        
        // Übergebe Nullvektor zur Endzeit <m_T>
        if (tool==false) 
        {
                m_Output.push_back(tmp2);
                m_Output_time.push_back(m_T);
        }
    else
        {
                m_Output2.push_back(tmp2);
                m_Output_time2.push_back(m_T);
        }

    return true;
}
// Hier wird der eigentliche Output für den Simulationsprozess generiert unter der Berücksichtigung 
// der in CompPath() berechneten Vektoren m_velocity, m_accel, m_length
bool path_simulate::Gen_Path()
{
        int n,m;        
        double length, lam, t_start, t_ma, t_sl, t_tmp; 

        t_start = m_t0;                           // setze Startzeit
        m_StartPnts1[0] = (*m_it1)->StartPoint(); // setze Startparameter

        if(m_single == false)
        {
                m_StartPnts2[0] = (*m_it2)->StartPoint();

/*------------------------------ Berechne Durchlaufzeit für das Master-Tool --------------------------------*/

                n = m_velocity_ma.size();
                for(int i=0; i<n; i++)                    // Schleife über die (zusammenhängenden) Kurven 
                {
                        m = m_velocity_ma[i].size();       
                        for(int j=0; j<m; j++)                // Schleife über die einzelnen Unterteilungsgebiete 
                        {
                                m_v[0] = m_velocity_ma[i][j][0];  // Startgeschwindigkeit des i-ten Abschnitts
                                m_v[1] = m_velocity_ma[i][j][1];  // maximale Geschwindigkeit des i-ten Abschnitts
                                m_v[2] = m_velocity_ma[i][j][2];  // Endgeschwindigkeit des i-ten Abschnitts

                                m_a = m_accel_ma[i][j];

                                length = m_length_ma[i][2*j];      // Länge des unkritischen Abschnitts 
                                                                                                   // Bem: Jede Kurve beginnt und endet in einem unkritischen Abschnitt
                                ParameterCalculation_Curve(length);   // Hier wird die Endzeit m_T berechnet

                                if(j != m-1)
                                {
                                        length = m_length_ma[i][2*j+1];// Länge des kritischen Abschnitts 
                                        m_T = m_T + length/m_v[2];     // Berechnet neue Endzeit (kritische Abschnitte werden mit konstanter
                                                                                                   // Geschwindigkeit m_v[2] durchlaufen)
                                }

                                m_t0 = m_T;                        // Endzeit wird zur Startzeit
                        }
                }

                t_ma = m_T - t_start;                  // Entspricht Durchlaufzeit für den Master
                m_t0 = t_start;                        // Setzt Startzeit zurück

/*-------------------------- Berechnung der Durchlaufzeit für das Slave-Tool -------------------------------*/

                n = m_velocity_sl.size();
                for(int i=0; i<n; i++)                    // Schleife über die (zusammenhängenden) Kurven 
                {
                        m = m_velocity_sl[i].size();       
                        for(int j=0; j<m; j++)                // Schleife über die einzelnen Unterteilungsgebiete 
                        {
                                m_v[0] = m_velocity_sl[i][j][0];  // Startgeschwindigkeit des i-ten Abschnitts
                                m_v[1] = m_velocity_sl[i][j][1];  // maximale Geschwindigkeit des i-ten Abschnitts
                                m_v[2] = m_velocity_sl[i][j][2];  // Endgeschwindigkeit des i-ten Abschnitts

                                m_a = m_accel_sl[i][j];

                                length = m_length_sl[i][2*j];      // Länge des unkritischen Abschnitts 
                                                                                                   // Bem: Jede Kurve beginnt und endet in einem unkritischen Abschnitt
                                ParameterCalculation_Curve(length);// Hier wird u.a. die Endzeit <m_T> berechnet

                                if(j != m-1)
                                {
                                        length = m_length_sl[i][2*j+1];// Länge des kritischen Abschnitts 
                                        m_T = m_T + length/m_v[2];     // Berechnet neue Endzeit (kritische Abschnitte werden mit der konstanten
                                                                                                   // Geschwindigkeit <m_v[2]> durchlaufen)
                                }

                                m_t0 = m_T;                        // Endzeit wird zur Startzeit
                        }
                }
                                                                                                                                                           
                t_sl = m_T - t_start;                              // Entspricht Durchlaufzeit für den Slave
                m_t0 = t_start;                                            // Setzt Startzeit zurück
                                                
/*----------------------- Synchronisierung von Master und Slave -----------------------------*/

// Idee: Skaliere die Geschwindigkeiten und Beschleunigungen derjenigen Bahn mit der kürzeren Durchlaufzeit,
// so dass die Durchlaufzeiten für Master und Slave übereinstimmen

                
                if(t_ma <= t_sl)                                           
                {
                        // Ab hier: Korrektur für den Master

                        lam = t_ma/t_sl;  // Skalierungsfaktor 0 < lam <= 1
                        
                        // Zunächst werden alle Geschwindigkeiten runterskaliert
                        n = m_velocity_ma.size();
                        for(int i=0; i<n; i++)
                        {
                                m = m_velocity_ma[i].size();
                                for(int j=0; j<m; j++)
                                {
                                        // linearen Beziehung zwischen den Geschwindigkeiten und der Durchlaufzeit
                                        m_velocity_ma[i][j][0] = lam*m_velocity_ma[i][j][0];
                                        m_velocity_ma[i][j][1] = lam*m_velocity_ma[i][j][1];
                                        m_velocity_ma[i][j][2] = lam*m_velocity_ma[i][j][2];
                                }
                        }

                        // Skalierung der Beschleunigungen
                        n = m_accel_ma.size();
                        for(int i=0; i<n; i++)
                        {
                                m = m_accel_ma[i].size();
                                for(int j=0; j<m; j++)
                                        m_accel_ma[i][j] = lam*lam*m_accel_ma[i][j];  // Die Beschleunigungen müssen mit dem Quadrat des Skalierungsfaktors
                        }                                                     // multipliziert werden (quadratische Abhängigkeit zur Durchlaufzeit)
                }
                else
                {
                        // Ab hier: Korrektur für den Slave

                        lam = t_sl/t_ma;  // Skalierungsfaktor 0 < lam <= 1
                        
                        // Zunächst werden alle Geschwindigkeiten runterskaliert
                        n = m_velocity_sl.size();
                        for(int i=0; i<n; i++)
                        {
                                m = m_velocity_sl[i].size();
                                for(int j=0; j<m; j++)
                                {
                                        // vgl. Master
                                        m_velocity_sl[i][j][0] = lam*m_velocity_sl[i][j][0];
                                        m_velocity_sl[i][j][1] = lam*m_velocity_sl[i][j][1];
                                        m_velocity_sl[i][j][2] = lam*m_velocity_sl[i][j][2];
                                }
                        }

                        // Skalierung der Beschleunigungen
                        n = m_accel_sl.size();
                        for(int i=0; i<n; i++)
                        {
                                m = m_accel_sl[i].size();
                                for(int j=0; j<m; j++)
                                        m_accel_sl[i][j] = lam*lam*m_accel_sl[i][j];  // Die Beschleunigungen müssen mit dem Quadrat des Skalierungsfaktors
                        }                                                     // multipliziert werden
                }
        }

/*------------------------------- Generierung der Outputvektoren ------------------------------------*/
        
/*MASTER*/      
        
        bool l;
        int  q,p;

        n = m_length_ma.size();
        for(int i=0; i<n; i++)  // Schleife geht über die Anzahl der zu fahrenden Kurven, vgl. path_simulate::CompPath()
        {
                if(i!=0)
                        m_it1++;

                l = false;  // Legt fest ob wir uns gerade in einem kritischen Abschnitt befinden
                q = 0;      // Zu Beginn jeder Kurve auf Null
            p = 0;      // Zu Beginn jeder Kurve auf Null

                m_StartParam[0] = (*m_it1)->FirstParameter(); // Setzt Startparameter der aktuellen Kurve
                
                m = m_length_ma[i].size();
                for(int j=0; j<m; j++)
                {
                        if(l==false)  // wird in jedem zweiten Schritt aufgerufen
                        {
                                m_a = m_accel_ma[i][p];
                                
                                m_v[0] = m_velocity_ma[i][q][0];
                                m_v[1] = m_velocity_ma[i][q][1];
                                m_v[2] = m_velocity_ma[i][q][2];
                                
                                p++;
                                q++;
                        }

                        MakePathSingle(0,m_length_ma[i][j],l,0);  // Hier werden die Outputvektoren erzeugt
                        
                        // Update der Variablen 
                        m_StartParam[0] += m_length_ma[i][j];
                        m_t0 = m_T;
                        l = !l;
                }
        }

        m_t0 = t_start;

/*ENDE MASTER*/

        
/*SLAVE*/
        
        // Analog zum Master (s.o.)
        if(m_single == false)
        {
                n = m_length_sl.size();
                for(int i=0; i<n; i++)
                {
                        if(i!=0)
                                m_it2++;
                        
                        l = false;
                        q = 0;
                        p = 0;
                        m_StartParam[1] = (*m_it2)->FirstParameter();
                        m = m_length_sl[i].size();

                        // vgl. Master
                        for(int j=0; j<m; j++)
                        {
                                if(l==false)
                                {
                                        m_a = m_accel_sl[i][p];
                                        
                                        m_v[0] = m_velocity_sl[i][q][0];
                                        m_v[1] = m_velocity_sl[i][q][1];
                                        m_v[2] = m_velocity_sl[i][q][2];
                                        
                                        p++;
                                        q++;
                                }

                                MakePathSingle(0,m_length_sl[i][j],l,1);  // hier wird der output für den slave generiert
                                m_StartParam[1] += m_length_sl[i][j];
                                m_t0 = m_T;
                                l = !l;
                        }
                }
        }

/*ENDE SLAVE*/


/*WEG-KORREKTUR*/
        
        t_tmp = m_t0;

        m_StartPnts1[1] = (*m_it1)->EndPoint(); // Setzt Endpunkt, notwendig für die folgende Wegkorrektur
        Correction(0);                          // Wegkorrektur für den Master

        if(m_single == false)
        {
                m_t0 = t_tmp;
                m_StartPnts2[1] = (*m_it2)->EndPoint(); // Setzt Endpunkt, notwendig für die folgende Wegkorrektur
                Correction(1);                          // Wegkorrektur für den Slave
        }

        m_t0 = t_start;

/*ENDE WEG-KORREKTUR*/


        m_velocity_ma.clear();
        m_velocity_sl.clear();
        m_length_ma.clear();
        m_length_sl.clear();
        m_accel_ma.clear();
        m_accel_sl.clear();
        
        return true;
}

/* Hier werden die Simulations-Outputvektoren für die jeweiligen Kurvenabschnitte erzeugt */
bool path_simulate::MakePathSingle(bool   brob,   // Beschreibt Ausgabeart (Roboter, Simulation) 
                                                                   double length, // Bogenlänge des betrachteten Abschnitts
                                                                   bool   part,   // Gibt an ob wir einen kritischen Abschnitt betrachten
                                                                   bool   tool)   // Tool (Master, Slave)
{
    GeomAdaptor_Curve anAdaptorCurve;
    double firstParam, lastParam, param, period, d, velo;
    int N;

    if(tool == false) anAdaptorCurve.Load(*m_it1);  // Master
    else              anAdaptorCurve.Load(*m_it2);  // Slave

    firstParam = anAdaptorCurve.FirstParameter();
    lastParam  = anAdaptorCurve.LastParameter();
    period     = lastParam - firstParam;
   
        gp_Vec                      dtmp1, dtmp2;   
        gp_Pnt                      tmp;
    Base::Vector3d              tmp2;
    std::vector<Base::Vector3d> tmp3;

        // Base::Vector3f pnt1,pnt2;

        if(brob)
        {
                N = std::max(2, (int) ceil(period / double(TolDist)));

                for (int i=1; i<N; ++i)  // niemals den ersten punkt mitnehmen
                {
                        if (m_StartParam[tool] + double(i)*period/(double(N)-1.0) > lastParam)
                        {
                                anAdaptorCurve.D0(m_StartParam[tool] + double(i)*period/(double(N)-1.0) - period, tmp);
                        }
                        else if (m_StartParam[tool] + double(i)*period/(double(N)-1.0) < firstParam )
                        {
                                anAdaptorCurve.D0(m_StartParam[tool] + double(i)*period/(double(N)-1.0) + period, tmp);
                        }
                        else
                        {
                                anAdaptorCurve.D0(m_StartParam[tool] + double(i)*period/(double(N)-1.0), tmp);
                        }

                        tmp2.x = tmp.X();
                        tmp2.y = tmp.Y();
                        tmp2.z = tmp.Z();

                        if (!tool)
                        {
                                m_Output_robo1.push_back(tmp2);

                                if (i==1)
                                {
                                        RoboFlag_Master.pop_back();
                                        RoboFlag_Master.push_back(1);
                                }

                                RoboFlag_Master.push_back(0);
                        }
                        else
                        {
                                m_Output_robo2.push_back(tmp2);

                                if (i==1)
                                {
                                        RoboFlag_Slave.pop_back();
                                        RoboFlag_Slave.push_back(1);
                                }

                                RoboFlag_Slave.push_back(0);
                        }
                }

                return true;
        }

    if (part==true) m_T = m_t0 + length/m_v[2]; // kritische Bereiche werden mit konstanter Geschwindigkeit durchlaufen
    else            ParameterCalculation_Curve(length);
    
        N = std::max(2, (int)((m_T-m_t0)/m_step));

    if (N>=100000) 
        N = 99999;  // maximale Anzahl möglicher Outputwerte

        m_del_t = (m_T-m_t0)/double(N);

    std::vector< std::vector<Base::Vector3d> > D0;
    std::vector< std::vector<Base::Vector3d> > D1;

    double t = m_t0;

        //anAdaptorCurve.D0(m_StartParam[tool],tmp);
        //pnt2.Set(tmp.X(),tmp.Y(),tmp.Z());

        if (part == true)  // kritischer Abschnitt
    {
        for (int i=0; i<N; ++i)  // Hauptschleife
        {
            if (!tool) m_Output_time.push_back(t);
            else       m_Output_time2.push_back(t);

            d = m_v[2]*(t-m_t0);
                        param = m_StartParam[tool] + d; // Kurvenparameter entspricht zurückgelegtem Weg 
                                                        // für den Fall einer Bogenlängenparametrisierung.                      
                                                        // Bei beliebiger Parametrisierung: 
                                                        // param = FindParamAt(anAdaptorCurve, d, m_StartParam[tool]);

                        // Berechnet alles bis zur zweiten Ableitung
                        if      ( param > lastParam  ){ anAdaptorCurve.D2(param - period, tmp, dtmp1, dtmp2);}
            else if ( param < firstParam ){ anAdaptorCurve.D2(param + period, tmp, dtmp1, dtmp2);}
                        else                          { anAdaptorCurve.D2(param,          tmp, dtmp1, dtmp2);}

                        //m_times_tmp.push_back(t);
                    //m_velo_tmp.push_back(m_v[2]);

                        // Ausgabevektor zum Zeitpunkt <t>
                        tmp2.x = dtmp1.X()*m_v[2];
            tmp2.y = dtmp1.Y()*m_v[2];
            tmp2.z = dtmp1.Z()*m_v[2];

            tmp3.push_back(tmp2);
            
                        if (!tool) m_Output.push_back(tmp3);
            else       m_Output2.push_back(tmp3);
            
                        tmp3.clear();
                        
                        /*
                        if (tool == false)  // zeichnet den tatsächlichen Weg nach
                        {
                                pnt1 = pnt2;
                                
                                pnt2.x = pnt2.x + dtmp1.X()*m_v[2]*m_del_t;
                                pnt2.y = pnt2.y + dtmp1.Y()*m_v[2]*m_del_t;
                                pnt2.z = pnt2.z + dtmp1.Z()*m_v[2]*m_del_t;
                                
                                if(i==0)
                                        m_log.addSingleArrow(pnt1,pnt2,2,1,1,1);
                                else
                                        m_log.addSingleArrow(pnt1,pnt2,2,1,0,0);
                        }
                        */
            
                        t += m_del_t;
        }
        return true;
    }

    // unkritischer Abschnitt
    for (int i=0; i<N; ++i)
    {
        if (!tool) m_Output_time.push_back(t);
        else       m_Output_time2.push_back(t);
        
                d = GetDistance(t);
                param = m_StartParam[tool] + d;     // Kurvenparameter entspricht zurückgelegtem Weg 
                                                        // für den Fall einer Bogenlängenparametrisierung.                      
                                                        // Bei beliebiger Parametrisierung: 
                                                        // param = FindParamAt(anAdaptorCurve, d, m_StartParam[tool]);

                // Berechnet alles bis zur zweiten Ableitung
        if      ( param > lastParam  ){ anAdaptorCurve.D2(param - period, tmp, dtmp1, dtmp2);}
        else if ( param < firstParam ){ anAdaptorCurve.D2(param + period, tmp, dtmp1, dtmp2);}
                else                          { anAdaptorCurve.D2(param,          tmp, dtmp1, dtmp2);}

                velo = GetVelocity(t);  // Berechnet die Geschwindigkeit <velo> des Tools zum Zeitpunkt <t>
                                        // bzgl. den Parametern m_t0, m_t1, m_t2, m_T, m_a, m_v[i], i =1,2,3 

                // Ausgabevektor zum Zeitpunkt <t>
        tmp2.x = dtmp1.X()*velo;
        tmp2.y = dtmp1.Y()*velo;
        tmp2.z = dtmp1.Z()*velo;

        tmp3.push_back(tmp2);
       
                if (!tool) m_Output.push_back(tmp3);
        else       m_Output2.push_back(tmp3);

                tmp3.clear();
                
                /*
                if (tool == false)  // zeichnet den tatsächlichen Weg nach
                {
                        pnt1 = pnt2;
                        
                        pnt2.x = pnt2.x + dtmp1.X()*velo*m_del_t;
                        pnt2.y = pnt2.y + dtmp1.Y()*velo*m_del_t;
                        pnt2.z = pnt2.z + dtmp1.Z()*velo*m_del_t;
                        
                        if(i==0)
                                m_log.addSingleArrow(pnt1,pnt2,2,1,1,1);
                        else
                                m_log.addSingleArrow(pnt1,pnt2,2,1,0,0);
                }
                */

                //m_times_tmp.push_back(t);
                //m_velo_tmp.push_back(velo);

        t += m_del_t;
    }

        return true;
}

/* Hauptroutine zur Generierung des Roboteroutputs für den normalen beidseitigen Fall */
bool path_simulate::MakePathRobot()
{
    ofstream anOutputFile;
    ofstream anOutputFile2;

    anOutputFile.open("output_master.k");
    anOutputFile.precision(7);
        
        anOutputFile  << "none" << std::endl;

    if (m_single == false)
    {
        anOutputFile2.open("output_slave.k");
        anOutputFile2.precision(7);
                
                anOutputFile2 << "none" << std::endl;
    }

        // Outputgenerierung über alle Kurven
    for (m_it1 = m_BSplineTop.begin(); m_it1 != m_BSplineTop.end(); ++m_it1)
    {
        m_StartParam[0] = ((*m_it1)->FirstParameter());
        if (m_single == false) m_StartParam[1] = ((*m_it2)->FirstParameter());

        /*------ 1.ZUSTELLUNG ------*/
        
                m_conn = CheckConnect();
        
                if (m_conn)   ConnectPaths_xy(1);
        else          ConnectPaths_z(1);

        UpdateParam();
        

                /*------ 2.ZUSTELLUNG ------*/
        
                if (m_conn)   ConnectPaths_z(1);
        else          ConnectPaths_xy(1);

        UpdateParam();

       
                /*------ KURVE ------*/

                MakePathSingle(1,0,0,0);  // Master
        MakePathSingle(1,0,0,1);  // Slave
        UpdateParam();

        if (m_single==false && (m_it1 != (m_BSplineTop.end()-1)))
            ++m_it2;

    }

        int c = 1;

        /*--- Schreibe Roboter-Output ---*/

        if (m_single==false)
        {
                WriteOutputDouble(anOutputFile,anOutputFile2,c,c,1,beam);
                anOutputFile2 << "*END" << endl;
                anOutputFile2.close();
        }
        else
        {
                WriteOutputSingle(anOutputFile,c,1,0,beam);
        }
    
        anOutputFile  << "*END" << endl;
    anOutputFile.close();

    return true;
}

/* Wird nur zu Beginn der Ausschreibung in path_simulate::WriteOurputDouble() aufgerufen und passt die Ausgabevektoren
<m_Output_time>, <m_Output_time2> so aneinander an, dass die Endzeiten übereinstimmen*/ 
bool path_simulate::TimeCorrection()
{
        int N;
        Base::Vector3d vec(0.0,0.0,0.0);
    std::vector<Base::Vector3d> vecc;
        
        vecc.push_back(vec);
    
    if (m_single == false)  // Eine Zeitkorrektur macht nur für diesen Fall auch Sinn
    {
        if(m_Output_time.size() == 0 || m_Output_time2.size() == 0)  // Sonderbehandlung für diesen Fall
                {
                        if(m_Output_time.size() > m_Output_time2.size())
                        {
                                m_Output_time2 = m_Output_time;
                                N = m_Output_time2.size();
                                m_Output2.resize(N);
                                
                                for (int i=0; i<N; ++i) // Füllt leeren Output mit Nullvektoren (Werkzeug hat zu warten)
                                {
                                        m_Output2[i] = vecc; 
                                }

                                return true;
                        }
                        else if(m_Output_time2.size() > m_Output_time.size())
                        {
                                m_Output_time = m_Output_time2;
                                N = m_Output_time.size();
                                m_Output.resize(N);
                                
                                for (int i=0; i<N; ++i) // Füllt leeren Output mit Nullvektoren (-> Werkzeug hat zu warten)
                                {
                                        m_Output[i] = vecc;
                                }

                                return true;
                        }

                        return true;  //gibt true zurück wenn beide Outputvektoren <m_Output_time> und <m_Output_time2> leer sein sollten
                }

        if (m_Output_time[m_Output_time.size()-1] < m_Output_time2[m_Output_time2.size()-1])
        {
            m_T = m_Output_time2[m_Output_time2.size()-1];

            N = std::max(1,int(ceil((m_Output_time2[m_Output_time2.size()-1] - m_Output_time[m_Output_time.size()-1])/ m_step)));
            m_del_t = (m_Output_time2[m_Output_time2.size()-1] - m_Output_time[m_Output_time.size()-1])/double(N);

            int ind = m_Output_time.size()-1;
            double time = 0.0;

            for (int i=1; i<N; ++i)
            {
                time += m_del_t;
                m_Output_time.push_back(m_Output_time[ind] + time);
                m_Output.push_back(vecc);
            }

            m_Output_time.push_back(m_Output_time2[m_Output_time2.size()-1]);
            m_Output.push_back(vecc);
        }
        else if (m_Output_time[m_Output_time.size()-1] > m_Output_time2[m_Output_time2.size()-1])
        {
            m_T = m_Output_time[m_Output_time.size()-1];

            N = std::max(1,int(ceil((m_Output_time[m_Output_time.size()-1] - m_Output_time2[m_Output_time2.size()-1])/ m_step)));
            m_del_t = (m_Output_time[m_Output_time.size()-1] - m_Output_time2[m_Output_time2.size()-1])/double(N);

            int ind = m_Output_time2.size()-1;
            double time = 0.0;

            for (int i=1; i<N; ++i)
            {
                time += m_del_t;
                m_Output_time2.push_back(m_Output_time2[ind] + time);
                m_Output2.push_back(vecc);
            }

            m_Output_time2.push_back(m_Output_time[m_Output_time.size()-1]);
            m_Output2.push_back(vecc);
        }
    }
    else // falls <m_single> = true (d.h nur Master -> keine Zeitkorrektur erforderlich!!!)
    {
        return false;
    }

    return true;
}

/* Hauptroutine zur Generierung des Simulationsoutputs für den feature-basierten und spiral-basierten beidseitigen Fall */
bool path_simulate::MakePathSimulate_Feat(const std::vector<float> &flatAreas, bool spiral)
{
    m_Feat = true;
        
        double rad[2];
        bool   tool;
        int    c[2];
    
    gp_Pnt pnt0, pnt1;

    std::vector<Handle_Geom_BSplineCurve>::iterator *it_1,*it_2;
    std::vector<Handle_Geom_BSplineCurve> *curves_1, *curves_2;

    ofstream anOutputFile[2];

    anOutputFile[0].open("output_master.k");
    anOutputFile[1].open("output_slave.k");
        anOutputFile[0].precision(7);
    anOutputFile[1].precision(7);

    m_it1 = m_BSplineTop.begin();
    m_it2 = m_BSplineBottom.begin();

    rad[0] =  m_set.master_radius;
    rad[1] = - m_set.slave_radius - m_set.sheet_thickness;

    c[0] = 1;    // Start Index der Master Kurven
    c[1] = 2001; // Start Index der Slave  Kurven

    int i = 0;

    while (m_it1 != m_BSplineTop.end() && m_it2 != m_BSplineBottom.end())
    {
        tool = StartingTool();  // bestimmt welches tool warten muss:
                                                                // tool == true  : Roboter = Slave  & NC = Master
                                                                // tool == fasle : Roboter = Master & NC = Slave
                if (!tool)
                {
                        it_1 = &m_it1;
                        it_2 = &m_it2;
                        curves_1 = &m_BSplineTop;
                        curves_2 = &m_BSplineBottom;
                }
                else
                {
                        it_1 = &m_it2;
                        it_2 = &m_it1;
                        curves_1 = &m_BSplineBottom;
                        curves_2 = &m_BSplineTop;
                }
       
        m_StartParam[0] = ((*m_it1)->FirstParameter()); // setze neue Startparameter (in unserem Fall immer 0)
        
                if (m_single == false) 
                        m_StartParam[1] = ((*m_it2)->FirstParameter());
 
                // die erste Zustellung vor dem Kontakt mit dem Blech wird hier seperat gehandelt
        if (i==0)
        {
            /*------ ZUSTELLUNG 1 ------*/
            ConnectPaths_xy(0);
            WriteOutputDouble(anOutputFile[0],anOutputFile[1],c[0],c[1],0,beam);
            UpdateParam();

            /*------ ZUSTELLUNG 2 ------*/
            ConnectPaths_z(0);
            WriteOutputDouble(anOutputFile[0],anOutputFile[1],c[0],c[1],0,beam);
            UpdateParam();
        }

        while (true)
        {
            if (*it_1 != (*curves_1).end()-1)
            {
                                /* ------ Kurve ------*/
                CompPath(0);                                                                                                             // Berechne Parameter für den Master
                                CompPath(1);                                                                                                             // Berechne Parameter für den Slave
                                Gen_Path();                                                                                                              // Erzeuge Output für aktuelle Kurve
                                WriteOutputDouble(anOutputFile[0],anOutputFile[1],c[0],c[1],0,beam); // Schreibe Output
                UpdateParam();

                                /*------ Zustellung ------*/
                (*it_1)++;                                                                                               // Gehe zur nächsten Kurve
                ConnectPaths_Feat(tool, 0, 1);                                                                           // Erzeuge Output für Zustellung
                                WriteOutputDouble(anOutputFile[0],anOutputFile[1],c[0],c[1],0,beam); // Schreibe Output
                UpdateParam();

                if (*it_1 == (*curves_1).end()-1 && *it_2 == (*curves_2).end()-1)    // Letzter Schritt
                {
                                        /* ------ Kurve ------*/
                                        CompPath(0);  // Berechne Parameter für den Master
                                        CompPath(1);  // Berechne Parameter für den Slave
                                        Gen_Path();       // Erzeuge Output für aktuelle Kurve
                                        WriteOutputDouble(anOutputFile[0],anOutputFile[1],c[0],c[1],0,beam); // Schreibe Output
                                        UpdateParam();
                    break;
                }

                (**it_1)->D0((**it_1)->FirstParameter(),pnt0); // übergebe aktuellen Startpunkt

                
                if ((pnt0.Z() > (flatAreas[i+1] + rad[tool] - 1e-1)) &&  // Erreicht der MASTER den nächsten ebenen 
                    (pnt0.Z() < (flatAreas[i+1] + rad[tool] + 1e-1)))    // Bereich, muss auf den SLAVE gewartet werden
                {
                                        if(!spiral)  // Bei Spiralbahnen erfolgt die Zustellung sofort
                                        {
                                                /* ------ Kurve ------*/
                                                CompPath(0);  // Berechne Parameter für den Master
                                                CompPath(1);  // Berechne Parameter für den Slave
                                                Gen_Path();       // Erzeuge Output für aktuelle Kurve
                                                WriteOutputDouble(anOutputFile[0],anOutputFile[1],c[0],c[1],0,beam); // Schreibe Output
                                                UpdateParam();
                                        }
                                        break;
                }
            }
            else
            {
                if (*it_2 == (*curves_2).end()-1)  // letzter Schritt
                {
                                        /* ------ Kurve ------*/
                    CompPath(0);  // Berechne Parameter für den Master
                                        CompPath(1);  // Berechne Parameter für den Slave
                                        Gen_Path();       // Erzeuge Output für aktuelle Kurve
                                        WriteOutputDouble(anOutputFile[0],anOutputFile[1],c[0],c[1],0,beam); // Schreibe Output
                                        UpdateParam();
                    break;
                }
                else break;
            }
        }

        if (m_it1 == m_BSplineTop.end()-1 && m_it2 == m_BSplineBottom.end()-1) // Fertig !!!
            break;

                /* ------ Zustellung ------*/
                if(!spiral) 
                {
                        (*it_1)++;                                              // gehe zur nächsten Kurve
                        ConnectPaths_Feat(tool, 0, 1);  // Erzeuge Output für die Master-Zustellung
                }
                
                (*it_2)++;                                                                                                                       // gehe zur nächsten Kurve                                                                              
                ConnectPaths_Feat(!tool, 0, 0);                                                                  // Erzeuge Output für die Slave-Zustellung
                WriteOutputDouble(anOutputFile[0],anOutputFile[1],c[0],c[1],0,beam); // Schreibe Output
                UpdateParam();
        
                ++i;
    }

    anOutputFile[0] << "*END" << endl;
    anOutputFile[1] << "*END" << endl;
        anOutputFile[0].close();
    anOutputFile[1].close();

        //WriteTimes();

    return true;
}

/* Hauptroutine zur Generierung des Roboteroutputs für den feature-basierten und spiral-basierten beidseitigen Fall */
bool path_simulate::MakePathRobot_Feat(const std::vector<float> &flatAreas)
{
    m_Feat = true;
    int f = 0, run;
    bool tool;
    double rad[2];
    gp_Pnt pnt0, pnt1;

    std::vector<Handle_Geom_BSplineCurve>::iterator *it_1,*it_2;
    std::vector<Handle_Geom_BSplineCurve> *curves_1, *curves_2;

    ofstream anOutputFile[2];

        std::stringstream master_file, slave_file;

    m_it1 = m_BSplineTop.begin();
    m_it2 = m_BSplineBottom.begin();

    rad[0] =  m_set.master_radius;
    rad[1] = - m_set.slave_radius - m_set.sheet_thickness;

    int i = 0;

    while (m_it1 != m_BSplineTop.end() && m_it2 != m_BSplineBottom.end())
    {
        tool = StartingTool();  // Bestimmt welches tool warten muss

        // Setze Startparameter (in unserem Fall unnötig da immer 0)
        m_StartParam[0] = ((*m_it1)->FirstParameter());
        if (m_single == false) m_StartParam[1] = ((*m_it2)->FirstParameter());

        // Erste Zustellung wird seperat gehandelt
        if (i==0)
        {
            ConnectPaths_xy(1); /* 1. ZUSTELLUNG */
            ConnectPaths_z(1);  /* 2. ZUSTELLUNG */
        }

        if (!tool)
        {
            it_1 = &m_it1;
            it_2 = &m_it2;
            curves_1 = &m_BSplineTop;
            curves_2 = &m_BSplineBottom;
        }
        else
        {
            it_1 = &m_it2;
            it_2 = &m_it1;
            curves_1 = &m_BSplineBottom;
            curves_2 = &m_BSplineTop;
        }

        // Hauptschleife
        while (true)
        {
            MakePathSingle(1,0,0,tool);  // Bahngenerierung des kontinuierlich fahrenden tools für aktuelle kurve

            // MASTER
            if (*it_1 != (*curves_1).end()-1)
            {
                (*it_1)++;
                (**it_1)->D0((**it_1)->FirstParameter(),pnt0);

                if (*it_1 == (*curves_1).end()-1 && *it_2 == (*curves_2).end()-1)
                {
                    ConnectPaths_Feat(tool, 1, 1);
                    MakePathSingle(1,0,0,tool);
                    break;
                }

                if ((pnt0.Z() > (flatAreas[i+1] + rad[tool] - 1e-1)) &&
                    (pnt0.Z() < (flatAreas[i+1] + rad[tool] + 1e-1)))
                {

                    run = Detect_FeatCurve(tool);

                    for (int i=0; i<run; ++i)
                    {
                        ConnectPaths_Feat(tool, 1, 1);
                        MakePathSingle(1,0,0,tool);
                        ++(*it_1);
                    }

                    break;
                }
            }
            else
            {
                if (*it_2 == (*curves_2).end()-1)
                {
                    ConnectPaths_Feat(tool, 1, 1);
                    MakePathSingle(1,0,0,tool);
                    break;
                }
                else break;
            }

            ConnectPaths_Feat(tool, 1, 1);
        }

        (**it_2)->D0((**it_2)->FirstParameter(),pnt0);

        while ((pnt0.Z() > (flatAreas[i] + rad[!tool] - 1e-1)) && 
                           (pnt0.Z() < (flatAreas[i] + rad[!tool] + 1e-1)))
        {
            MakePathSingle(1,0,0,!tool);

            if (*it_2 != (*curves_2).end()-1)
                (*it_2)++;
            else
                break;

            (**it_2)->D0((**it_2)->FirstParameter(),pnt0);
        }

        master_file << "output_master" << f << ".k";
        slave_file  << "output_slave"  << f << ".k";
        ++f;

        std::cout << master_file.str() << "  ,  " << slave_file << std::endl;

        anOutputFile[0].open((master_file.str()).c_str());
        anOutputFile[1].open((slave_file.str()).c_str());

        if (!tool)
        {
            anOutputFile[0] << "none" << std::endl;
            anOutputFile[1] << "continuous" << std::endl;
        }
        else
        {
            anOutputFile[0] << "continuous" << std::endl;
            anOutputFile[1] << "none" << std::endl;
        }

        WriteOutput_Feat(anOutputFile[0], anOutputFile[1],f,1);

        master_file.str("");
        master_file.clear();
        slave_file.str("");
        slave_file.clear();

        m_Output_robo1.clear();
        m_Output_robo2.clear();

        if (*it_1 == (*curves_1).end()-1 && *it_2 == (*curves_2).end()-1)
            break;

        ConnectPaths_Feat(0, 1, 1);
        ConnectPaths_Feat(1, 1, 0);
        ++i;
    }

    anOutputFile[0] << "*END" << endl;
        anOutputFile[1] << "*END" << endl;
    anOutputFile[0].close();
    anOutputFile[1].close();

    return true;
}

// Hilfsfunktion für den Featurebasierten Teil:
// Bestimmt welches Tool wartet, während das andere läuft
bool path_simulate::StartingTool()
{
    double z0,z1;
    gp_Pnt pnt0,pnt1;

    if(m_it1 != m_BSplineTop.end()-1)
    {
        // z-Abstand zur nächsten Bahn - MASTER
        (*m_it1)->D0((*m_it1)->FirstParameter(),pnt0); m_it1++;
        (*m_it1)->D0((*m_it1)->FirstParameter(),pnt1); m_it1--;
       
        z0 = abs(pnt0.Z() - pnt1.Z());
    }

    else
        z0 = 1e+3;

    if(m_it2 != m_BSplineBottom.end()-1)
    {
        // z-Abstand zur nächsten Bahn - SLAVE
        (*m_it2)->D0((*m_it2)->FirstParameter(),pnt0); m_it2++;
        (*m_it2)->D0((*m_it2)->FirstParameter(),pnt1); m_it2--;
        
        z1 = abs(pnt0.Z() - pnt1.Z());
    }
    else z1 = 1e+3;

    if(z0<z1)  return false;  // Slave muss warten
    else       return true;   // Master muss warten
}

/* Hier wird geschaut, ob es sich um eine geschlossene Kurve handelt bzw. wieviele Kurven abgefahren werden müssen
um wieder am Anfangspunkt der Kurve anzukommen. Die Anzahl der zu fahrenden Kurven wird als integer zurückgegeben */
int path_simulate::Detect_FeatCurve(bool tool)
{
    gp_Pnt pt0,pt1;
    int num = 1;

    if(!tool)
        {
                pt0 = (*m_it1)->StartPoint(); // Startpunkt der aktuellen Master-Kurve
                pt1 = (*m_it1)->EndPoint();   // Endpunkt der aktuellen Master-Kurve
                
                while(pt0.Distance(pt1) > 1e-3)  // Mache weiter solange Start und Endpunkt nicht passen
                {
                        if(m_it1 == m_BSplineTop.end()-1)  // Stoppe wenn bei der letzten Kurve angekommen
                        {
                                for(int i=1; i<num; i++)
                                        m_it1--;

                                num = 1;
                                pt1 = pt0; // Damit die äußere while-Schleife verlassen wird
                        }
                        else
                        {
                                num++;
                                m_it1++;
                                pt1 = (*m_it1)->EndPoint();
                        }
                }

                // Zurück zum Status quo
                for(int i=1; i<num; i++)
                        m_it1--;
        }
        else
        {       
                pt0 = (*m_it2)->StartPoint(); // Startpunkt der aktuellen Slave-Kurve
                pt1 = (*m_it2)->EndPoint();   // Endpunkt der aktuellen Slave-Kurve
                
                while(pt0.Distance(pt1) > 1e-3) // Mache weiter solange Start und Endpunkt nicht passen
                {
                        if(m_it2 == m_BSplineBottom.end()-1) // Stoppe wenn bei der letzten Kurve angekommen
                        {
                                for(int i=1; i<num; i++)
                                        m_it2--;

                                num = 1;
                                pt1 = pt0;  // Damit die while-Schleife verlassen wird
                        }
                        else
                        {                       
                                num++;
                                m_it2++;
                                pt1 = (*m_it2)->EndPoint();
                        }
                }

                // Zurück zum Status quo
                for(int i=1; i<num; i++)
                        m_it2--;
        }

        return num;
}

/* Hauptroutine zur Generierung des Simulationsoutputs für den normalen beidseitigen Fall */
bool path_simulate::MakePathSimulate()
{
    int c1 = 1, 
                c2 = 2001; 

    ofstream anOutputFile, anOutputFile2;

    anOutputFile.open("output_master.k");
    anOutputFile.precision(7);

    if (m_single == false)
    {
        anOutputFile2.open("output_slave.k");
        anOutputFile2.precision(7);
    }

    for (m_it1 = m_BSplineTop.begin(); m_it1 < m_BSplineTop.end(); ++m_it1)  // Schleife über alle Kurven 
    {
        m_StartParam[0] = ((*m_it1)->FirstParameter());     // speichert Startparameterwert der aktuellen Master-Kurve
       
                if (m_single == false)
                        m_StartParam[1] = ((*m_it2)->FirstParameter()); // speichert Startparameterwert der aktuellen Slave-Kurve
      

                /*Zustellung Start*/

        m_conn = CheckConnect(); // Rückgabewert = 1 bei negativer z-Richtungszustellung
                                         // Rückgabewert = 0 bei positiver z-Richtungszustellung 
       
                //  negative z-Richtung: 1. XY --> 2. Z              
                //  positive z-Richtung: 1. Z  --> 2. XY    
 
                // *** 1. ***
                if (m_conn)   ConnectPaths_xy(0);
        else          ConnectPaths_z(0);

        if (m_single == false) WriteOutputDouble(anOutputFile,anOutputFile2,c1,c2,0,beam);
        else                   WriteOutputSingle(anOutputFile,c1,0,0,beam);

        UpdateParam();

                // *** 2. ***
        if (m_conn)   ConnectPaths_z(0);
        else          ConnectPaths_xy(0);

                // Schreibe Output
        if (m_single == false) WriteOutputDouble(anOutputFile,anOutputFile2,c1,c2,0,beam);
        else                   WriteOutputSingle(anOutputFile,c1,0,0,beam);

        UpdateParam();

        /*Zustellung Ende*
                
                
                /*Kurve Start*/

        CompPath(0); // Berechnung der Parameter für den Master
        
                if (m_single == false)
                        CompPath(1); // Berechnung der Parameter für den Slave

                Gen_Path();  // Erzeugung der Outputvektoren

                // Schreibe Outputvektoren
        if (m_single == false) WriteOutputDouble(anOutputFile,anOutputFile2,c1,c2,0,beam);
        else                   WriteOutputSingle(anOutputFile,c1,0,0,beam);

        UpdateParam();
       
                if (m_single==false && (m_it1 != (m_BSplineTop.end()-1)))
            ++m_it2;

                /*Kurve Ende*/
    }

        //m_log.saveToFile("c:/Master-Path.iv");

        /*
        ofstream anOutputvelocity;
    anOutputvelocity.open("output_velocity.k");
    anOutputvelocity.precision(7);

        for(int i=0; i<(int)m_times_tmp.size(); ++i) // Schreibe absolute Geschwindigkeitswerte aus
                anOutputvelocity << m_times_tmp[i] << ", " << m_velo_tmp[i] << endl;
        
        anOutputvelocity.close();
        */

    anOutputFile  << "*END" << endl;
    anOutputFile.close();
        

        if (m_single == false)
    {
                anOutputFile2 << "*END" << endl;
        anOutputFile2.close();
    }
    
    // WriteTimes();
    return true;
}

/* Schreibt den Output für den feature- und spiral-basierten beidseitigen Fall */
bool path_simulate::WriteOutput_Feat(ofstream &anOutputFile, ofstream &anOutputFile2, int &c, bool brob)
{
    if (m_single == false)
    {
        if (brob == false)
        {
            int n = m_Output.size();

            anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
            anOutputFile << "$#     pid       dof       vad      lcid        sf       vid     death     birth" << std::endl;
            anOutputFile << "2,1,0," << c <<  ",1.000000, ," << m_Output_time[n-1] << ","  << m_Output_time[0] << std::endl;
            anOutputFile << "*DEFINE_CURVE" << std::endl << c << std::endl;

            for (int i=0; i<n; ++i)
            {
                anOutputFile << m_Output_time[i] - m_Output_time[0]<<"," << m_Output[i][0].x << std::endl;
            }

            anOutputFile << m_Output_time[n-1] - m_Output_time[0] + 0.1 << "," << m_Output[n-1][0].x<< std::endl;

            anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
            anOutputFile << "$#     pid       dof       vad      lcid        sf       vid     death     birth" << std::endl;
            anOutputFile << "3,1,0," << c <<  ",1.000000, ," << m_Output_time[n-1] << ","  << m_Output_time[0] << std::endl;
            anOutputFile << "*DEFINE_CURVE" << std::endl << c+1 << std::endl;

            for (int i=0; i<n; ++i)
            {
                anOutputFile << m_Output_time[i] - m_Output_time[0]<<"," << m_Output[i][1].x << std::endl;
            }

            anOutputFile << m_Output_time[n-1] - m_Output_time[0] + 0.1 << "," << m_Output[n-1][1].x<< std::endl;

            anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
            anOutputFile << "$#     pid       dof       vad      lcid        sf       vid     death     birth" << std::endl;
            anOutputFile << "2,2,0," << c+1 <<  ",1.000000, ," << m_Output_time[n-1] << "," << m_Output_time[0] << std::endl;
            anOutputFile << "*DEFINE_CURVE" << std::endl << c+2 << std::endl;

            for (int i=0; i<n; ++i)
            {
                anOutputFile <<  m_Output_time[i] - m_Output_time[0]<< "," << m_Output[i][0].y<< std::endl;
            }

            anOutputFile << m_Output_time[n-1] - m_Output_time[0] + 0.1 << "," << m_Output[n-1][0].y<< std::endl;


            anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
            anOutputFile << "$#     pid       dof       vad      lcid        sf       vid     death     birth" << std::endl;
            anOutputFile << "3,2,0," << c+1 <<  ",1.000000, ," << m_Output_time[n-1] << "," << m_Output_time[0] << std::endl;
            anOutputFile << "*DEFINE_CURVE" << std::endl << c+3 << std::endl;

            for (int i=0; i<n; ++i)
            {
                anOutputFile <<  m_Output_time[i] - m_Output_time[0]<< "," << m_Output[i][1].y<< std::endl;
            }

            anOutputFile << m_Output_time[n-1] - m_Output_time[0] + 0.1 << "," << m_Output[n-1][1].y<< std::endl;


            anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
            anOutputFile << "$#     pid       dof       vad      lcid        sf       vid     death     birth" << std::endl;
            anOutputFile << "2,3,0," << c+2 <<  ",1.000000, ," << m_Output_time[n-1] << "," << m_Output_time[0] << std::endl;
            anOutputFile << "*DEFINE_CURVE" << std::endl << c+4 <<  std::endl;

            for (int i=0; i<n; ++i)
            {
                anOutputFile <<  m_Output_time[i] - m_Output_time[0]<<"," << m_Output[i][0].z<< std::endl;
            }

            anOutputFile << m_Output_time[n-1] - m_Output_time[0] + 0.1 << "," << m_Output[n-1][0].z<< std::endl;


            anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
            anOutputFile << "$#     pid       dof       vad      lcid        sf       vid     death     birth" << std::endl;
            anOutputFile << "3,3,0," << c+2 <<  ",1.000000, ," << m_Output_time[n-1] << "," << m_Output_time[0] << std::endl;
            anOutputFile << "*DEFINE_CURVE" << std::endl << c+5 <<  std::endl;

            for (int i=0; i<n; ++i)
            {
                anOutputFile <<  m_Output_time[i] - m_Output_time[0]<<"," << m_Output[i][1].z<< std::endl;
            }

            anOutputFile << m_Output_time[n-1] - m_Output_time[0] + 0.1 << "," << m_Output[n-1][1].z<< std::endl;

            c = c+6;
        }
        else
        {
            int n1 = m_Output_robo1.size();
            for (int i=0; i<n1; ++i)
                anOutputFile  << m_Output_robo1[i].x + m_set.x_offset_robot << "," <<  m_Output_robo1[i].y + m_set.y_offset_robot << "," << m_Output_robo1[i].z  << endl;

            anOutputFile.close();

            int n2 = m_Output_robo2.size();
            for (int i=0; i<n2; ++i)
                anOutputFile2 << m_Output_robo2[i].x + m_set.x_offset_robot << "," <<  m_Output_robo2[i].y + m_set.y_offset_robot << "," << m_Output_robo2[i].z  << endl;

            anOutputFile2.close();
        }
    }
    else
    {
        if (brob == false)
        {

            int n = m_Output.size();

            anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
            anOutputFile << "$#     pid       dof       vad      lcid        sf       vid     death     birth" << std::endl;
            anOutputFile << "2,1,0," << c <<  ",1.000000, ," << m_Output_time[n-1] << ","  << m_Output_time[0] << std::endl;
            anOutputFile << "*DEFINE_CURVE" << std::endl << c << std::endl;

            for (int i=0; i<n; ++i)
            {
                anOutputFile << m_Output_time[i] - m_Output_time[0]<<"," << m_Output[i][0].x << std::endl;
            }

            anOutputFile << m_Output_time[n-1] - m_Output_time[0] + 0.1 << "," << m_Output[n-1][0].x<< std::endl;

            anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
            anOutputFile << "$#     pid       dof       vad      lcid        sf       vid     death     birth" << std::endl;
            anOutputFile << "2,2,0," << c+1 <<  ",1.000000, ," << m_Output_time[n-1] << "," << m_Output_time[0] << std::endl;
            anOutputFile << "*DEFINE_CURVE" << std::endl << c+1 << std::endl;

            for (int i=0; i<n; ++i)
            {
                anOutputFile <<  m_Output_time[i] - m_Output_time[0]<< "," << m_Output[i][0].y<< std::endl;
            }

            anOutputFile << m_Output_time[n-1] - m_Output_time[0] + 0.1 << "," << m_Output[n-1][0].y<< std::endl;

            anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
            anOutputFile << "$#     pid       dof       vad      lcid        sf       vid     death     birth" << std::endl;
            anOutputFile << "2,3,0," << c+2 <<  ",1.000000, ," << m_Output_time[n-1] << "," << m_Output_time[0] << std::endl;
            anOutputFile << "*DEFINE_CURVE" << std::endl << c+4 <<  std::endl;

            for (int i=0; i<n; ++i)
            {
                anOutputFile <<  m_Output_time[i] - m_Output_time[0]<<"," << m_Output[i][0].z<< std::endl;
            }

            anOutputFile << m_Output_time[n-1] - m_Output_time[0] + 0.1 << "," << m_Output[n-1][0].z<< std::endl;

            c = c+3;
        }
        else
        {
            int n = m_Output_robo1.size();

            for (int i=0; i<n; ++i)
                anOutputFile << m_Output_robo1[i].x << "," <<  m_Output_robo1[i].y << "," << m_Output_robo1[i].z << ",";

            anOutputFile << std::endl;
        }
    }

    return true;
}

/*
// Schreibt alle Zeitwerte der Ausgabevektoren aus
bool path_simulate::WriteTimes()
{
    ofstream anOutputFile;

    anOutputFile.open("CurveTimes.k");
    anOutputFile.precision(7);

    for (unsigned int i=0; i< m_PathTimes_Master.size(); ++i)
    {
        anOutputFile << m_PathTimes_Master[i].first << " " << m_PathTimes_Master[i].second << std::endl;
    }

    for (unsigned int i=0; i< m_PathTimes_Slave.size(); ++i)
    {
        anOutputFile << m_PathTimes_Slave[i].first << " " << m_PathTimes_Slave[i].second << std::endl;
    }

    anOutputFile.close();

    return true;
}
*/

/* Schreibt Output für den normalen einseitigen Fall */
bool path_simulate::WriteOutputSingle(ofstream &anOutputFile, int &c, bool brob, bool tool, bool beamfl)
{
    std::vector< std::vector<Base::Vector3d> > Out_val;
    std::vector<double> Out_time;
    std::pair<float,float> times;
    int n;
    int ind;

    int pid;

    if (brob == true)  // Schreibe Roboter-Output
        {
                std::vector<Base::Vector3d> Out_rob;

                if (!tool) Out_rob  = m_Output_robo1;
                else       Out_rob  = m_Output_robo2;

                n = Out_rob.size();

                for (int i=0; i<n; ++i)
                        anOutputFile << Out_rob[i].x + m_set.x_offset_robot << "," <<  Out_rob[i].y + m_set.y_offset_robot << "," << Out_rob[i].z << "," << std::endl;

                anOutputFile << std::endl;
                return true;
        }

        if (!tool)
    {
        Out_val  = m_Output;
        Out_time = m_Output_time;
        ind = 2;
    }
    else
    {
        Out_val  = m_Output2;
        Out_time = m_Output_time2;
        ind = 3;
    }

    if (beamfl && tool) pid = ind+1;    // pid: 2 - Master
    else                pid = ind;      //      3 - Slave
                                        //      4 - Plate (x-,y-movement) 

    n = Out_val.size();

    if (n != Out_time.size())
        throw Base::Exception("Outputlängen passen nicht zusammen");

    if (n>1)
    {
        anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
        anOutputFile << "$#     pid       dof       vad      lcid        sf       vid     death     birth" << std::endl;
        anOutputFile << ind << ",1,0," << c <<  ",1.000000, ," << Out_time[n-1] << ","  << Out_time[0] << std::endl;
        

                if (beamfl && tool)
        {
            anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
                        anOutputFile << "$#     pid       dof       vad      lcid        sf       vid     death     birth" << std::endl;
                        anOutputFile << pid << ",1,0," << c <<  ",1.000000, ," << Out_time[n-1] << ","  << Out_time[0] << std::endl;
        }
                
                anOutputFile << "*DEFINE_CURVE" << std::endl << c << std::endl;

        for (int i=0; i<n; ++i)
        {
            anOutputFile << Out_time[i] - Out_time[0]<<"," << Out_val[i][0].x << std::endl;
        }

        anOutputFile << Out_time[n-1] - Out_time[0] + 0.1 << "," << Out_val[n-1][0].x << std::endl;

        times.first  = (float) Out_time[0];
        times.second = (float) Out_time[n-1];

        if (!tool) m_PathTimes_Master.push_back(times);
        else      m_PathTimes_Slave.push_back(times);

        anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
        anOutputFile << "$#     pid       dof       vad      lcid        sf       vid     death     birth" << std::endl;
        anOutputFile << ind << ",2,0," << c+1 <<  ",1.000000, ," << Out_time[n-1] << "," << Out_time[0] << std::endl;
        

                if (beamfl && tool)
        {
            anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
                        anOutputFile << "$#     pid       dof       vad      lcid        sf       vid     death     birth" << std::endl;
                        anOutputFile << pid << ",2,0," << c+1 <<  ",1.000000, ," << Out_time[n-1] << "," << Out_time[0] << std::endl;
        }
                
                anOutputFile << "*DEFINE_CURVE" << std::endl << c+1 << std::endl;

        for (int i=0; i<n; ++i)
        {
            anOutputFile <<  Out_time[i] - Out_time[0]<< "," << Out_val[i][0].y << std::endl;
        }

        anOutputFile << Out_time[n-1] - Out_time[0] + 0.1 << "," << Out_val[n-1][0].y << std::endl;

        times.first  = (float) Out_time[0];
        times.second = (float) Out_time[n-1];

        if (!tool) m_PathTimes_Master.push_back(times);
        else      m_PathTimes_Slave.push_back(times);

        if (beamfl && tool)
        {
                        anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
            anOutputFile << "$#     pid       dof       vad      lcid        sf       vid     death     birth" << std::endl;
            anOutputFile << pid << ",3,0," << c+2 <<  ",1.000000, ," << Out_time[n-1] << "," << Out_time[0] << std::endl;
        }
                else
                {
                        anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
                        anOutputFile << "$#     ind       dof       vad      lcid        sf       vid     death     birth" << std::endl;
                        anOutputFile << ind << ",3,0," << c+2 <<  ",1.000000, ," << Out_time[n-1] << "," << Out_time[0] << std::endl;
                }

        anOutputFile << "*DEFINE_CURVE" << std::endl << c+2 <<  std::endl;


        for (int i=0; i<n; ++i)
        {
            anOutputFile <<  Out_time[i] - Out_time[0]<<"," << Out_val[i][0].z<< std::endl;
        }

        anOutputFile << Out_time[n-1] - Out_time[0] + 0.1 << "," << Out_val[n-1][0].z<< std::endl;

        times.first  = (float) Out_time[0];
        times.second = (float) Out_time[n-1];

        if (!tool) m_PathTimes_Master.push_back(times);
        else       m_PathTimes_Slave.push_back(times);

        c = c+3;
    }

    return true;
}

/* Schreibt Output für den normalen beidseitigen Fall */
bool path_simulate::WriteOutputDouble(ofstream &anOutputFile, ofstream &anOutputFile2, int &c1, int &c2, bool brob, bool beamfl)
{
    std::pair<float,float> times;
    int pid1 = 2; // Master
        int pid2 = 3; // Slave
        int pid3 = 4; // Platte

    if (brob == false) // Simulations-Output (brob == true -> roboter-ouput)
    {
                TimeCorrection();

        int n = m_Output.size();   // Master
        int n2 = m_Output2.size(); // Slave

        if (n>1)
        {

            // MASTER-X

            anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
            anOutputFile << "$#     pid       dof       vad      lcid        sf       vid     death     birth" << std::endl;
            anOutputFile << pid1 << ",1,0," << c1 <<  ",1.000000, ," << m_Output_time[n-1] << ","  << m_Output_time[0] << std::endl;
            anOutputFile << "*DEFINE_CURVE" << std::endl << c1 << std::endl;

            for (int i=0; i<n; ++i)
            {
                anOutputFile << m_Output_time[i] - m_Output_time[0]<<"," << m_Output[i][0].x << std::endl;
            }

            anOutputFile << m_Output_time[n-1] - m_Output_time[0] + 0.1 << "," << m_Output[n-1][0].x << std::endl;

            times.first  = (float) m_Output_time[0];
            times.second = (float) m_Output_time[n-1];
            m_PathTimes_Master.push_back(times);       // fülle vektor für curve-times

            
                        
                        
                        // SLAVE-X

            anOutputFile2 << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
            anOutputFile2 << "$#     pid       dof       vad      lcid        sf       vid     death     birth" << std::endl;
            anOutputFile2 << pid2 << ",1,0," << c2 <<  ",1.000000, ," << m_Output_time2[n2-1] << ","  << m_Output_time2[0] << std::endl;
                        
                        if (beamfl) // wenn auf true, dann füge neuen part mit ein
            {
                anOutputFile2 << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
                anOutputFile2 << "$#     pid       dof       vad      lcid        sf       vid     death     birth" << std::endl;
                anOutputFile2 << pid3 << ",1,0," << c2<<  ",1.000000, ," << m_Output_time[n-1] << "," << m_Output_time[0] << std::endl;
            }

                        anOutputFile2 << "*DEFINE_CURVE" << std::endl << c2 << std::endl;

            for (int i=0; i<n2; ++i)
            {
                anOutputFile2 << m_Output_time2[i] - m_Output_time2[0]<<"," << m_Output2[i][0].x << std::endl;
            }

            anOutputFile2 << m_Output_time2[n2-1] - m_Output_time2[0] + 0.1 << "," << m_Output2[n2-1][0].x << std::endl;

            times.first  = (float) m_Output_time2[0];
            times.second = (float) m_Output_time2[n2-1];
            m_PathTimes_Slave.push_back(times);

            
                        
                        
                        
                        // MASTER-Y

            anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
            anOutputFile << "$#     pid       dof       vad      lcid        sf       vid     death     birth" << std::endl;
            anOutputFile << pid1 << ",2,0," << c1+1 <<  ",1.000000, ," << m_Output_time[n-1] << "," << m_Output_time[0] << std::endl;
            anOutputFile << "*DEFINE_CURVE" << std::endl << c1+1 << std::endl;

            for (int i=0; i<n; ++i)
            {
                anOutputFile <<  m_Output_time[i] - m_Output_time[0]<< "," << m_Output[i][0].y << std::endl;
            }

            anOutputFile << m_Output_time[n-1] - m_Output_time[0] + 0.1 << "," << m_Output[n-1][0].y << std::endl;

            times.first  = (float) m_Output_time[0];
            times.second = (float) m_Output_time[n-1];
            m_PathTimes_Master.push_back(times);

            
                        
                        
                        // SLAVE-Y

            anOutputFile2 << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
            anOutputFile2 << "$#     pid       dof       vad      lcid        sf       vid     death     birth" << std::endl;
            anOutputFile2 << pid2 << ",2,0," << c2+1 <<  ",1.000000, ," << m_Output_time2[n2-1] << ","  << m_Output_time2[0] << std::endl;
            
                        if (beamfl) // wenn auf true, dann füge neuen part mit ein
            {
                anOutputFile2 << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
                anOutputFile2 << "$#     pid       dof       vad      lcid        sf       vid     death     birth" << std::endl;
                anOutputFile2 << pid3 << ",2,0," << c2+1 <<  ",1.000000, ," << m_Output_time[n-1] << "," << m_Output_time[0] << std::endl;
            }
                        
                        anOutputFile2 << "*DEFINE_CURVE" << std::endl << c2+1 << std::endl;

            for (int i=0; i<n2; ++i)
            {
                anOutputFile2 << m_Output_time2[i] - m_Output_time2[0]<<"," << m_Output2[i][0].y << std::endl;
            }

            anOutputFile2 << m_Output_time2[n2-1] - m_Output_time2[0] + 0.1 << "," << m_Output2[n2-1][0].y << std::endl;

            times.first  = (float) m_Output_time2[0];
            times.second = (float) m_Output_time2[n2-1];
            m_PathTimes_Slave.push_back(times);

            
                        
                        
                        
                        // MASTER-Z
                        anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
                        anOutputFile << "$#     pid       dof       vad      lcid        sf       vid     death     birth" << std::endl;
                        anOutputFile << pid1 << ",3,0," << c1+2 <<  ",1.000000, ," << m_Output_time[n-1] << "," << m_Output_time[0] << std::endl;
            anOutputFile << "*DEFINE_CURVE" << std::endl << c1+2 <<  std::endl;


            for (int i=0; i<n; ++i)
            {
                anOutputFile <<  m_Output_time[i] - m_Output_time[0]<<"," << m_Output[i][0].z<< std::endl;
            }

            anOutputFile << m_Output_time[n-1] - m_Output_time[0] + 0.1 << "," << m_Output[n-1][0].z<< std::endl;

            times.first  = (float) m_Output_time[0];
            times.second = (float) m_Output_time[n-1];
            m_PathTimes_Master.push_back(times);


                        
                        // SLAVE-Z

                        if (beamfl)
            {
                anOutputFile2 << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
                anOutputFile2 << "$#     pid       dof       vad      lcid        sf       vid     death     birth" << std::endl;
                anOutputFile2 << pid3 << ",3,0," << c2+2 <<  ",1.000000, ," << m_Output_time[n-1] << "," << m_Output_time[0] << std::endl;

            }
                        else
                        {
                                anOutputFile2 << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
                                anOutputFile2 << "$#     pid       dof       vad      lcid        sf       vid     death     birth" << std::endl;
                                anOutputFile2 << pid2 << ",3,0," << c2+2 <<  ",1.000000, ," << m_Output_time2[n2-1] << ","  << m_Output_time2[0] << std::endl;
                        }

            anOutputFile2 << "*DEFINE_CURVE" << std::endl << c2+2 <<  std::endl;

            for (int i=0; i<n2; ++i)
            {
                anOutputFile2 << m_Output_time2[i] - m_Output_time2[0]<<"," << m_Output2[i][0].z << std::endl;
            }

            anOutputFile2 << m_Output_time2[n2-1] - m_Output_time2[0] + 0.1 << "," << m_Output2[n2-1][0].z << std::endl;

            times.first  = (float) m_Output_time2[0];
            times.second = (float) m_Output_time2[n2-1];
            m_PathTimes_Slave.push_back(times);

            c1 += 3;
            c2 += 3;
        }
    }
    else  // Schreibe Roboter-Output
    {
            int n1 = m_Output_robo1.size();
            for (int i=0; i<n1; ++i)
                anOutputFile  << m_Output_robo1[i].x + m_set.x_offset_robot << "," <<  m_Output_robo1[i].y + m_set.y_offset_robot << "," << m_Output_robo1[i].z << "," << RoboFlag_Master[i] << endl;

            int n2 = m_Output_robo2.size();
            for (int i=0; i<n2; ++i)
                anOutputFile2 << m_Output_robo2[i].x + m_set.x_offset_robot << "," <<  m_Output_robo2[i].y + m_set.y_offset_robot << "," << m_Output_robo2[i].z << "," << RoboFlag_Slave[i] << endl;
    }

    return true;
}