//
//#include "three.h"
//
//vector<State> Tree::run(const State &init_state, double start, double end, double t) {
////    integrate_const(runge_kutta4<State>(), dmove2, init_state, start ,end ,t, write_pendulum);
//    return _stateList;
//}
//
//void Tree::dmove2(const State &x_input, State &dxdt, const double t) {
//    double velocity = x_input[0];
//    double gamma = x_input[1];
//    double varphi = x_input[2];
//    double nx = _control[0];
//    double nz = _control[1];
//    varphi  = _control[2];
////    if(varphi == 0){
////        varphi = control[2];
////    }
//    double g = 9.81;  // 重力加速度
//    double velocity_ = g * (nx - sin(gamma));  // 米每秒
//    double gamma_ = (g / velocity) * (nz * cos(varphi) - cos(gamma));  // 米每秒
//    double varphi_ = g * nz * sin(varphi) / (velocity * cos(gamma));
//    dxdt[0] = velocity_;
//    dxdt[1] = gamma_;
//    dxdt[2] = varphi_;
//}
//
//void Tree::setController(const State &control) {
//    _k = 1;
//    _control = control;
//}
//
//void Tree::write_pendulum(const State &x, const double t) {
//    double velocity = x[0];
//    double gamma = x[1];
//    double varphi = x[2];
//    double dx = velocity * cos(gamma) * sin(varphi);
//    double dy = velocity * cos(gamma) * cos(varphi);
//    double dz = velocity * sin(gamma);
//    State new_state(6);
//    new_state[0] = _stateList[_k - 1][0] + dx;
//    new_state[1] = _stateList[_k - 1][1] + dy;
//    new_state[2] = _stateList[_k - 1][2] + dz;
//    new_state[3] = velocity;
//    new_state[4] = gamma;
//    new_state[5] = varphi;
//    _k++;
//    _stateList.push_back(new_state);
//}