You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
50 lines
1.6 KiB
50 lines
1.6 KiB
1 year ago
|
//
|
||
|
|
//#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);
|
||
|
|
//}
|