フレネセレの公式が解けない

前提・実現したいこと

曲率と捩率を与えることで、フレネセネの公式を解いて3次元の曲線を得ようとしていましたが、出力したグラフが歪んでしまいます。
フレネセレの公式の微分方程式を4次ルンゲクッタで解いています。

発生している問題・エラーメッセージ

入力している曲率と捩率は螺旋のものなので、真っ直ぐに伸びるはずですがx方向に歪んでしまいます。

該当のソースコード

c++
1#include<iostream>
2#include<cmath>
3#include<limits>
4#include<iomanip>
5#include<vector>
6#include "/usr/include/eigen3/Eigen/Dense"
7#include "/usr/include/eigen3/Eigen/Sparse"
8#include "/usr/include/eigen3/Eigen/Core"
9#include "/usr/include/eigen3/Eigen/LU"
10#include "matplotlibcpp.h"
11namespace plt = matplotlibcpp;
12
13double r_c = 1.0;
14double h_c = 0.5;
15
16double curvature(double _s){
17    return r_c / (r_c * r_c + h_c * h_c);
18}
19double torsion(double _s){
20    return h_c / (r_c * r_c + h_c * h_c);
21}
22Eigen::Matrix<double, 3, 1> Func_c(double _s, Eigen::Matrix<double, 3, 1> _E_1){
23    return (_E_1);
24}
25Eigen::Matrix<double, 3, 1> Func_1(double _s, Eigen::Matrix<double, 3, 1> _E_2){
26    return (curvature(_s) * _E_2);
27}
28Eigen::Matrix<double, 3, 1> Func_2(double _s, Eigen::Matrix<double, 3, 1> _E_1, Eigen::Matrix<double, 3, 1> _E_3){
29    return ((-1 * curvature(_s) * _E_1) + (torsion(_s) * _E_3));
30}
31Eigen::Matrix<double, 3, 1> Func_3(double _s, Eigen::Matrix<double, 3, 1> _E_2){
32    return (-1 * torsion(_s) * _E_2);
33}
34
35int main(){
36    Eigen::Matrix<double, 3, 1> C;
37    Eigen::Matrix<double, 3, 1> E_1;
38    Eigen::Matrix<double, 3, 1> E_2;
39    Eigen::Matrix<double, 3, 1> E_3;
40    //set initial value
41    C << 0, 0, 0;
42    E_1 << 1, 0, 0;
43    E_2 << 0, 1, 0;
44    E_3 << 0, 0, 1;
45
46    Eigen::Matrix<double, 3, 1> E_1_SUM;
47    Eigen::Matrix<double, 3, 1> E_2_SUM;
48    Eigen::Matrix<double, 3, 1> E_3_SUM;
49    E_1_SUM << 1, 0, 0;
50    E_2_SUM << 0, 1, 0;
51    E_3_SUM << 0, 0, 1;
52    
53    
54    //RungeKutta
55    Eigen::Matrix<double, 3, 1> K_a_c;
56    Eigen::Matrix<double, 3, 1> K_a_1;
57    Eigen::Matrix<double, 3, 1> K_a_2;
58    Eigen::Matrix<double, 3, 1> K_a_3;
59
60    Eigen::Matrix<double, 3, 1> K_b_c;
61    Eigen::Matrix<double, 3, 1> K_b_1;
62    Eigen::Matrix<double, 3, 1> K_b_2;
63    Eigen::Matrix<double, 3, 1> K_b_3;
64
65    Eigen::Matrix<double, 3, 1> K_c_c;
66    Eigen::Matrix<double, 3, 1> K_c_1;
67    Eigen::Matrix<double, 3, 1> K_c_2;
68    Eigen::Matrix<double, 3, 1> K_c_3;
69
70    Eigen::Matrix<double, 3, 1> K_d_c;
71    Eigen::Matrix<double, 3, 1> K_d_1;
72    Eigen::Matrix<double, 3, 1> K_d_2;
73    Eigen::Matrix<double, 3, 1> K_d_3;
74
75    double s_long = 30;
76    double s = 0;
77    double h = 0.05;
78    double n = s_long / h;
79    std::vector<double> C_x, C_y, C_z;
80    std::vector<double> E_1_x, E_1_y, E_1_z;
81    std::vector<double> E_2_x, E_2_y, E_2_z;
82    std::vector<double> E_3_x, E_3_y, E_3_z;
83
84    for(int i = 0; i < n; ++i){
85        K_a_c = h * Func_c(s, E_1);
86        K_a_1 = h * Func_1(s, E_2);
87        K_a_2 = h * Func_2(s, E_1, E_3);
88        K_a_3 = h * Func_3(s, E_2);
89
90        K_b_c = h * Func_c(s + h / 2, E_1 + K_a_1 / 2);
91        K_b_1 = h * Func_1(s + h / 2, E_2 + K_a_2 / 2);
92        K_b_2 = h * Func_2(s + h / 2, E_1 + K_a_1 / 2, E_3 + K_a_3 / 2);
93        K_b_3 = h * Func_3(s + h / 2, E_2 + K_a_2 / 2);
94
95
96        K_c_c = h * Func_c(s + h / 2, E_1 + K_b_1 / 2);
97        K_c_1 = h * Func_1(s + h / 2, E_2 + K_b_2 / 2);
98        K_c_2 = h * Func_2(s + h / 2, E_1 + K_b_1 / 2, E_3 + K_b_3 / 2);
99        K_c_3 = h * Func_3(s + h / 2, E_2 + K_b_2 / 2);
100
101        K_d_c = h * Func_c(s + h, E_1 + K_c_1);
102        K_d_1 = h * Func_1(s + h, E_2 + K_c_2);
103        K_d_2 = h * Func_2(s + h, E_1 + K_c_1, E_3 + K_c_3);
104        K_d_3 = h * Func_3(s + h, E_2 + K_c_2);
105
106        s += h;
107
108        C   += (K_a_c + 2 * K_b_c + 2 * K_c_c + K_d_c) / 6;
109        E_1 += (K_a_1 + 2 * K_b_1 + 2 * K_c_1 + K_d_1) / 6;
110        E_2 += (K_a_2 + 2 * K_b_2 + 2 * K_c_2 + K_d_2) / 6;
111        E_3 += (K_a_3 + 2 * K_b_3 + 2 * K_c_3 + K_d_3) / 6;
112        
113        C_x.push_back(C(0, 0));
114        C_y.push_back(C(1, 0));
115        C_z.push_back(C(2, 0));
116
117        E_1_x.push_back(E_1(0, 0));
118        E_1_y.push_back(E_1(1, 0));
119        E_1_z.push_back(E_1(2, 0));
120        
121        E_2_x.push_back(E_2(0, 0));
122        E_2_y.push_back(E_2(1, 0));
123        E_2_z.push_back(E_2(2, 0));
124        
125        E_3_x.push_back(E_3(0, 0));
126        E_3_y.push_back(E_3(1, 0));
127        E_3_z.push_back(E_3(2, 0));
128    }
129    
130    //matplotlib
131    std::map<std::string, std::string> keywords;
132    keywords.insert(std::pair<std::string, std::string>("label", "parametric curve") );
133    plt::plot3(C_x, C_y, C_z, keywords);
134    plt::plot3(E_1_x, E_1_y, E_1_z, keywords);
135    plt::plot(E_1_x, E_1_y);
136    plt::xlabel("x label");
137    plt::ylabel("y label");
138    plt::set_zlabel("z label"); 
139    plt::legend();
140    plt::show();
141}
142