下述的C++代码以及matlab代码实现了相同的作用，C++代码是matlab代码通过matlab coder工具转换而来，对比两种代码，详细解释C++代码的具体实现过程。
C++代码：  
d = b_uavD3DConn.AirSpeed;
  //  UAV俯仰角限制下界
  //  UAV俯仰角限制上界
  //  俯仰角计算
  //  未归一化的方向向量
  //  高度差
  //  负高度为正方向
  //  待跟踪航迹俯仰角不能超过UAV性能限制
  //  validateattributes(pitch, 'double', {">=", pitchLimit_1, "<=",
  //  pitchLimit_2}); 航向角计算 目标轨线长度 方向向量 航迹采样
  if (std::isnan(d)) {
    y.set_size(1, 1);
    y[0] = rtNaN;
  } else if ((d == 0.0) || (d < 0.0)) {
    y.set_size(1, 0);
  } else if (std::isinf(d)) {
    y.set_size(1, 1);
    y[0] = 0.0;
  } else if (std::floor(d) == d) {
    loop_ub = static_cast<int>(4000.0 / d);
    y.set_size(1, loop_ub + 1);
    for (i = 0; i <= loop_ub; i++) {
      y[i] = d * static_cast<double>(i);
    }
  } else {
    double apnd;
    ndbl = std::floor(4000.0 / d + 0.5);
    apnd = ndbl * d;
    if (d > 0.0) {
      cdiff = apnd - 4000.0;
    } else {
      cdiff = 4000.0 - apnd;
    }
    if (std::abs(cdiff) < 1.7763568394002505E-12) {
      ndbl++;
      apnd = 4000.0;
    } else if (cdiff > 0.0) {
      apnd = (ndbl - 1.0) * d;
    } else {
      ndbl++;
    }
    if (ndbl >= 0.0) {
      n = static_cast<int>(ndbl);
    } else {
      n = 0;
    }
    y.set_size(1, n);
    if (n > 0) {
      y[0] = 0.0;
      if (n > 1) {
        y[n - 1] = apnd;
        nm1d2 = (n - 1) / 2;
        for (k = 0; k <= nm1d2 - 2; k++) {
          cdiff = (static_cast<double>(k) + 1.0) * d;
          y[k + 1] = cdiff;
          y[(n - k) - 2] = apnd - cdiff;
        }
        if (nm1d2 << 1 == n - 1) {
          y[nm1d2] = apnd / 2.0;
        } else {
          cdiff = static_cast<double>(nm1d2) * d;
          y[nm1d2] = cdiff;
          y[nm1d2 + 1] = apnd - cdiff;
        }
      }
    }
  }
  loop_ub = y.size(1);
  sample_LenSeq.set_size(y.size(1));
  for (i = 0; i < loop_ub; i++) {
    sample_LenSeq[i] = y[i];
  }
  //  目标航迹采样长度索引序列
  //  目标航迹采样数量
  //  航迹采样点不能超过10个
  if (sample_LenSeq.size(0) > maxSampleNum) {
    unsigned int u;
    cdiff =
        std::floor(static_cast<double>(sample_LenSeq.size(0)) / maxSampleNum);
    if (cdiff < 4.294967296E+9) {
      if (cdiff >= 0.0) {
        u = static_cast<unsigned int>(cdiff);
      } else {
        u = 0U;
      }
    } else if (cdiff >= 4.294967296E+9) {
      u = MAX_uint32_T;
    } else {
      u = 0U;
    }
    if ((u == 0U) ||
        ((u > 0U) && (static_cast<unsigned int>(sample_LenSeq.size(0)) < 1U))) {
      i = 1;
      k = -1;
    } else {
      i = static_cast<int>(u);
      k = sample_LenSeq.size(0) - 1;
    }
    loop_ub = div_s32(k, i);
    n = loop_ub + 1;
    b_sample_LenSeq.set_size(loop_ub + 1);
    for (k = 0; k <= loop_ub; k++) {
      b_sample_LenSeq[k] = sample_LenSeq[i * k];
    }
    sample_LenSeq.set_size(loop_ub + 1);
    for (i = 0; i < n; i++) {
      sample_LenSeq[i] = b_sample_LenSeq[i];
    }
  }
  loop_ub = sample_LenSeq.size(0);
  sample_Poses.set_size(sample_LenSeq.size(0), 4);
  n = sample_LenSeq.size(0) << 2;
  for (i = 0; i < n; i++) {
    sample_Poses[i] = 0.0;
  }
  //  显式定义为四列矩阵
  n = (loop_ub / 2) << 1;
  nm1d2 = n - 2;
  for (i = 0; i < 3; i++) {
    for (k = 0; k <= nm1d2; k += 2) {
      __m128d r;
      r = _mm_loadu_pd(&sample_LenSeq[k]);
      _mm_storeu_pd(
          &sample_Poses[k + sample_Poses.size(0) * i],
          _mm_add_pd(
              _mm_mul_pd(_mm_set1_pd(static_cast<double>(dir_vec[i])), r),
              _mm_set1_pd(static_cast<double>(startPos[i]))));
    }
    for (k = n; k < loop_ub; k++) {
      sample_Poses[k + sample_Poses.size(0) * i] =
          static_cast<double>(dir_vec[i]) * sample_LenSeq[k] +
          static_cast<double>(startPos[i]);
    }
  }
  //  前三列为位置线性采样
  //  sample_Poses = startPos + dir_vec .* sample_LenSeq;     %
  //  目标轨线位置线性采样 保证最后一个线性插值点为终点
  x_data[0] = (sample_Poses[sample_Poses.size(0) - 1] != 0.0);
  x_data[1] =
      (sample_Poses[(sample_Poses.size(0) + sample_Poses.size(0)) - 1] !=
       4000.0);
  x_data[2] =
      (sample_Poses[(sample_Poses.size(0) + sample_Poses.size(0) * 2) - 1] !=
       -1000.0);
  b_y = false;
  k = 0;
  exitg1 = false;
  while ((!exitg1) && (k <= 2)) {
    if (x_data[k]) {
      b_y = true;
      exitg1 = true;
    } else {
      k++;
    }
  }
  if (b_y) {
    sample_Poses[sample_Poses.size(0)] = 0.0;
    sample_Poses[sample_Poses.size(0) + sample_Poses.size(0)] = 4000.0;
    sample_Poses[sample_Poses.size(0) + sample_Poses.size(0) * 2] = -1000.0;
  }
  n = sample_Poses.size(0);
  iobj_1.sumple_num = n;
  for (i = 0; i < n; i++) {
    sample_Poses[i + sample_Poses.size(0) * 3] = 0.0;
  }
  //  初始化第四列
  for (i = 0; i < n; i++) {
    sample_Poses[i + sample_Poses.size(0) * 3] = 1.5707963267948966;
  }
matlab代码：
    % 用直线构造目标航迹并采样航迹点位姿
    function [sample_Poses, sample_Num, target_PathSeg] = Get_TargetPathSeg_Line(~, uavD3DConn, target_PathPose, maxSampleNum)
    %   说明：
    %       所有Get_TargetPathSeg_*方法拥有相同的输入输出接口
    %   输入：
    %   输出：
        
        % 输入获取
        airSpeed        = uavD3DConn.AirSpeed;
        pitchLimit_1    = uavD3DConn.FlightPathAngleLimit(1);   % UAV俯仰角限制下界
        pitchLimit_2    = uavD3DConn.FlightPathAngleLimit(2);   % UAV俯仰角限制上界
        startPos        = target_PathPose(1, 1:3);
        goalPos         = target_PathPose(2, 1:3);
        % 俯仰角计算
        delta_XYZ   = goalPos - startPos;                       % 未归一化的方向向量
        delta_Z     = delta_XYZ(3);                             % 高度差
        pitch       = atan(-delta_Z / norm(delta_XYZ(1:2)));    % 负高度为正方向
        % 待跟踪航迹俯仰角不能超过UAV性能限制
        % validateattributes(pitch, 'double', {">=", pitchLimit_1, "<=", pitchLimit_2});
        % 航向角计算
        target_PathLength   = norm(delta_XYZ);                  % 目标轨线长度
        dir_vec             = (delta_XYZ) / target_PathLength;  % 方向向量
        heading             = CalConvergencePt.Get_NED_Heading(dir_vec(1:2));
        % 航迹采样
        sample_LenSeq   = (0 : airSpeed : target_PathLength)';  % 目标航迹采样长度索引序列
        sample_Num      = length(sample_LenSeq);                % 目标航迹采样数量
        % 航迹采样点不能超过10个
        if sample_Num > maxSampleNum
            sampleLenInterval   = uint32(floor(sample_Num / maxSampleNum));
            sample_LenSeq       = sample_LenSeq(1 : sampleLenInterval : sample_Num);
        end
        sample_Poses = zeros(length(sample_LenSeq), 4);  % 显式定义为四列矩阵
        sample_Poses(:, 1:3) = startPos + dir_vec .* sample_LenSeq;  % 前三列为位置线性采样
        % sample_Poses = startPos + dir_vec .* sample_LenSeq;     % 目标轨线位置线性采样
        numRows = size(sample_Poses, 1);
        % 保证最后一个线性插值点为终点
        if any(sample_Poses(numRows, 1:3) ~= goalPos)
            sample_Poses(numRows+1, 1:3) = goalPos;
        end
        sample_Poses(:, 4)  = 0;                                % 初始化第四列
        sample_Poses(:, 4)  = heading;                                  % 线性采样点加入姿态
        sample_Num          = size(sample_Poses, 1);                    % 线性采样点数量
        target_PathSeg      = [startPos, heading; goalPos, heading];    % 起止目标航迹点位姿