/*
  一些路径生成器
*/

#include "path.h"

float Path_Straight1d(float start, float end, float t) {
    return start + (end - start) * t;
}

/**
 * @brief 生成一条直线段的路径点
 * @param start 起始点
 * @param end 结束点
 * @param t 时间路径点，范围[0, 1]
 * @param out 输出路径点
 */
void Path_Straight2d(float start[2], float end[2], float t, float out[2]){
    out[0] = start[0] + (end[0] - start[0]) * t; // x坐标插值
    out[1] = start[1] + (end[1] - start[1]) * t; // y坐标插值
}

/**
 * @brief 生成一条三维直线段的路径点
 * @param start 起始点
 * @param end 结束点
 * @param t 时间路径点，范围[0, 1]
 * @param out 输出路径点
 */
void Path_straight3d(float start[3], float end[3], float t, float out[3]){
    out[0] = start[0] + (end[0] - start[0]) * t; // x坐标插值
    out[1] = start[1] + (end[1] - start[1]) * t; // y坐标插值
    out[2] = start[2] + (end[2] - start[2]) * t; // z坐标插值
}

/**
 * @brief 生成一条二次贝塞尔曲线的路径点
 * @param start 起始点
 * @param mid 中间控制点
 * @param end 结束点
 * @param t 时间路径点，范围[0, 1]
 * @param out 输出路径点
 */
void Path_Bezier2d(float start[2], float mid[2], float end[2], float t, float out[2]){
    float t1 = 1 - t; // 计算1-t
    out[0] = t1 * t1 * start[0] + 2 * t1 * t * mid[0] + t * t * end[0]; // x坐标插值
    out[1] = t1 * t1 * start[1] + 2 * t1 * t * mid[1] + t * t * end[1]; // y坐标插值
}

/**
 * @brief 生成一条三次贝塞尔曲线的路径点
 * @param start 起始点
 * @param mid1 中间控制点1
 * @param mid2 中间控制点2
 * @param end 结束点
 * @param t 时间路径点，范围[0, 1]
 * @param out 输出路径点
 */
void Path_Bezier3d(float start[3], float mid1[3], float mid2[3], float end[3], float t, float out[3]){
    float t1 = 1 - t; // 计算1-t
    out[0] = t1 * t1 * t1 * start[0] + 3 * t1 * t1 * t * mid1[0] + 3 * t1 * t * t * mid2[0] + t * t * t * end[0]; // x坐标插值
    out[1] = t1 * t1 * t1 * start[1]
                + 3 * t1 * t1 * t * mid1[1] + 3 * t1 * t * t * mid2[1] + t * t * t * end[1]; // y坐标插值
    out[2] = t1 * t1 * t1 * start[2]
                + 3 * t1 * t1 * t * mid1[2] + 3 * t1 * t * t * mid2[2] + t * t * t * end[2]; // z坐标插值
}