Trajectory Planner using Ruckig Lib

MotionData SCurveTp::calculateMotion(double maxSpeed, double acceleration, double jerkTimePercentage) {
    MotionData data;
    double distance = 100.0; // Total driving distance

    // Calculation of the total acceleration and deceleration time without taking into account jerk
    double baseTimeAcceleration = maxSpeed / acceleration;
    double jerkTime = baseTimeAcceleration * (jerkTimePercentage / 100.0);

    // Total time for acceleration and deceleration phase including jerk
    double totalTimeAcceleration = baseTimeAcceleration + 2 * jerkTime;
    double totalTimeDeceleration = totalTimeAcceleration;

    // Calculation of time and distance for the constant speed phase
    double distanceAcceleration = 0.5 * acceleration * pow(totalTimeAcceleration - jerkTime, 2) + acceleration * jerkTime * (totalTimeAcceleration - 0.5 * jerkTime);
    double distanceDeceleration = distanceAcceleration;
    double distanceConstantSpeed = distance - (distanceAcceleration + distanceDeceleration);
    double timeConstantSpeed = distanceConstantSpeed / maxSpeed;

    // Total driving time
    double totalTime = totalTimeAcceleration + timeConstantSpeed + totalTimeDeceleration;

    double currentSpeed = 0.0, currentPosition = 0.0, currentAcceleration = 0.0, currentJerk = 0.0;
    double timeStep = 0.01; // Time step




    for (double t = 0; t <= totalTime; t += timeStep) {
        // Determining the current phase of movement
        if (t < jerkTime) {
            currentJerk = acceleration / jerkTime;
            currentAcceleration += currentJerk * timeStep;
        } else if (t < totalTimeAcceleration - jerkTime) {
            currentJerk = 0;
            currentAcceleration = acceleration;
        } else if (t < totalTimeAcceleration) {
            currentJerk = -acceleration / jerkTime;
            currentAcceleration += currentJerk * timeStep;
        } else if (t < totalTimeAcceleration + timeConstantSpeed) {
            currentJerk = 0;
            currentAcceleration = 0;
            currentSpeed = maxSpeed;
        } else if (t < totalTime - totalTimeDeceleration + jerkTime) {
            currentJerk = -acceleration / jerkTime;
            currentAcceleration += currentJerk * timeStep;
        } else if (t < totalTime - jerkTime) {
            currentJerk = 0;
            currentAcceleration = -acceleration;
        } else {
            currentJerk = acceleration / jerkTime;
            currentAcceleration += currentJerk * timeStep;
        }
        // Calculate speed and position


        currentSpeed += currentAcceleration * timeStep;
        currentPosition += currentSpeed * timeStep;

        // Speed ​​and Position Limit
        if (currentSpeed > maxSpeed) currentSpeed = maxSpeed;
        if (currentSpeed < 0) currentSpeed = 0;
        if (currentPosition > distance) currentPosition = distance;

        // Adding data to lists
        data.trajectory.append(QPointF(t, currentPosition));
        data.velocity.append(QPointF(t, currentSpeed));
        data.acceleration.append(QPointF(t, currentAcceleration));
        data.jerk.append(QPointF(t, currentJerk));
    }
    double t=totalTime;
    // If the total time is less than the maximum time, fill in the remaining time
    while (t < 100.0) {
        currentSpeed = (t >= totalTime) ? 0.0 : currentSpeed; // Speed ​​0 after end of movement
        currentAcceleration = (t >= totalTime) ? 0.0 : currentAcceleration; // Acceleration 0 after end of movement
        currentJerk = (t >= totalTime) ? 0.0 : currentJerk; // Jerk 0 after movement ends

        data.trajectory.append(QPointF(t, currentPosition));
        data.velocity.append(QPointF(t, currentSpeed));
        data.acceleration.append(QPointF(t, currentAcceleration));
        data.jerk.append(QPointF(t, currentJerk));

        t += timeStep;
    }
    return data;
}