Skip to content

Commit

Permalink
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
add C++ version of utils and dynamics
Browse files Browse the repository at this point in the history
A-Hayasaka committed May 24, 2024
1 parent c986fe9 commit 38b3ae2
Showing 16 changed files with 345 additions and 143 deletions.
15 changes: 11 additions & 4 deletions Makefile
Original file line number Diff line number Diff line change
@@ -1,14 +1,21 @@

CC = g++
CFLAGS = -O3 -Wall -shared -std=c++11 -I/usr/include/eigen3 -fPIC -rdynamic
CFLAGS = -O2 -Wall -shared -std=c++11 -I/usr/include/eigen3 -fPIC -rdynamic
PYBIND = `python3 -m pybind11 --includes`
PYCONFIG = `python3-config --extension-suffix`

SRCS = src/Air.cpp src/Earth.cpp src/gravity.cpp src/Coordinate.cpp

all: utils
all: atmosphere coordinate dynamics utils

utils: utils
atmosphere:
$(CC) $(CFLAGS) $(PYBIND) $(SRCS) src/pybind_USStandardAtmosphere.cpp -o USStandardAtmosphere_c$(PYCONFIG)

coordinate:
$(CC) $(CFLAGS) $(PYBIND) $(SRCS) src/pybind_coordinate.cpp -o coordinate_c$(PYCONFIG)
$(CC) $(CFLAGS) $(PYBIND) $(SRCS) src/pybind_dynamics.cpp -o dynamics_c$(PYCONFIG)

dynamics:
$(CC) $(CFLAGS) $(PYBIND) $(SRCS) src/pybind_dynamics.cpp -o dynamics_c$(PYCONFIG)

utils:
$(CC) $(CFLAGS) $(PYBIND) $(SRCS) src/pybind_utils.cpp -o utils_c$(PYCONFIG)
2 changes: 1 addition & 1 deletion constraints_b.py
Original file line number Diff line number Diff line change
@@ -30,7 +30,7 @@
import sys
import numpy as np
from numba import jit
from utils import *
from utils_p import *
from coordinate_p import conj, quatrot, normalize


2 changes: 1 addition & 1 deletion constraints_c.py
Original file line number Diff line number Diff line change
@@ -28,7 +28,7 @@

import numpy as np
from numba import jit
from utils import dynamic_pressure_pa, angle_of_attack_all_rad, angle_of_attack_ab_rad
from utils_p import dynamic_pressure_pa, angle_of_attack_all_rad, angle_of_attack_ab_rad
from coordinate_p import *


3 changes: 1 addition & 2 deletions constraints_d.py
Original file line number Diff line number Diff line change
@@ -27,8 +27,7 @@
# constraints about dynamics

import numpy as np
from dynamics_p import dynamics_velocity
from dynamics_c import dynamics_velocity_NoAir, dynamics_quaternion
from dynamics_c import dynamics_velocity, dynamics_velocity_NoAir, dynamics_quaternion


def equality_dynamics_mass(xdict, pdict, unitdict, condition):
2 changes: 1 addition & 1 deletion constraints_e.py
Original file line number Diff line number Diff line change
@@ -28,7 +28,7 @@


import numpy as np
from utils import *
from utils_c import *
from coordinate_c import geodetic2ecef, eci2ecef, normalize, quatrot, quat_nedg2ecef, vel_eci2ecef, eci2geodetic
from tools.IIP import posLLH_IIP_FAA
from tools.downrange import distance_vincenty
2 changes: 1 addition & 1 deletion constraints_u.py
Original file line number Diff line number Diff line change
@@ -27,7 +27,7 @@
# constraints about user conditions

from user_constraints import equality_user, inequality_user
from utils import jac_fd
from utils_p import jac_fd

def equality_jac_user(xdict, pdict, unitdict, condition):
"""Jacobian of user-defined equality constraint."""
8 changes: 4 additions & 4 deletions dynamics_p.py
Original file line number Diff line number Diff line change
@@ -26,11 +26,11 @@
from numba import jit
import numpy as np
from numpy.linalg import norm
from coordinate_c import quatrot, conj, quatmult, vel_eci2ecef, quat_nedg2eci, ecef2geodetic, ecef2eci, gravity
from USStandardAtmosphere_c import airdensity_at, airpressure_at, geopotential_altitude, speed_of_sound
from utils import wind_ned

from coordinate_p import quatrot, conj, quatmult, vel_eci2ecef, quat_nedg2eci, ecef2geodetic, ecef2eci, gravity
from USStandardAtmosphere_p import airdensity_at, airpressure_at, geopotential_altitude, speed_of_sound
from utils_p import wind_ned

@jit(nopython=True)
def dynamics_velocity(
mass_e, pos_eci_e, vel_eci_e, quat_eci2body, t, param, wind_table, CA_table, units
):
2 changes: 1 addition & 1 deletion initialize.py
Original file line number Diff line number Diff line change
@@ -26,7 +26,7 @@
import sys
import numpy as np
from scipy.interpolate import interp1d
from utils import *
from utils_c import *
from PSfunctions import *
from USStandardAtmosphere_c import *
from coordinate_c import *
2 changes: 1 addition & 1 deletion output_result.py
Original file line number Diff line number Diff line change
@@ -25,7 +25,7 @@

import numpy as np
import pandas as pd
from utils import *
from utils_c import *
from PSfunctions import *
from USStandardAtmosphere_c import *
from coordinate_c import *
35 changes: 1 addition & 34 deletions src/pybind_USStandardAtmosphere.cpp
Original file line number Diff line number Diff line change
@@ -1,37 +1,4 @@
#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <pybind11/eigen.h>
#include <Eigen/Core>
#include <cmath>
#include "Air.hpp"


namespace py = pybind11;

using vec3d = Eigen::Matrix<double, 3, 1>;
using vec4d = Eigen::Matrix<double, 4, 1>;
using vecXd = Eigen::Matrix<double, -1, 1>;
using matXd = Eigen::Matrix<double, -1, -1, Eigen::RowMajor>;

double geopotential_altitude(double z) {
return Air::geopotential_altitude(z);
}

double airtemperature_at(double altitude_m) {
return Air::temperature(altitude_m);
}

double airpressure_at(double altitude_m) {
return Air::pressure(altitude_m);
}

double airdensity_at(double altitude_m) {
return Air::density(altitude_m);
}

double speed_of_sound(double altitude_m) {
return Air::speed_of_sound(altitude_m);
}
#include "wrapper_air.hpp"

PYBIND11_MODULE(USStandardAtmosphere_c, m) {
m.def("geopotential_altitude", &geopotential_altitude, "geopotential altitude");
54 changes: 53 additions & 1 deletion src/pybind_dynamics.cpp
Original file line number Diff line number Diff line change
@@ -1,4 +1,55 @@
#include "wrapper_utils.hpp"
#include "wrapper_coordinate.hpp"
#include "wrapper_air.hpp"


matXd dynamics_velocity(
vecXd mass_e,
matXd pos_eci_e,
matXd vel_eci_e,
matXd quat_eci2body,
vecXd t,
vecXd param,
matXd wind_table,
matXd CA_table,
vecXd units) {

vecXd mass = mass_e * units[0];
matXd pos_eci = pos_eci_e * units[1];
matXd vel_eci = vel_eci_e * units[2];
matXd acc_eci = matXd::Zero(pos_eci.rows(), 3);

double thrust_vac = param[0];
double air_area = param[2];
double nozzle_area = param[4];

for (int i = 0; i < mass.rows(); i++) {
vec3d pos_llh = ecef2geodetic(pos_eci(i, 0), pos_eci(i, 1), pos_eci(i, 2));
double altitude = geopotential_altitude(pos_llh[2]);
double rho = airdensity_at(altitude);
double p = airpressure_at(altitude);

vec3d vel_ecef = vel_eci2ecef(vel_eci.row(i), pos_eci.row(i), t[i]);
vec3d vel_wind_ned = wind_ned(altitude, wind_table);

vec3d vel_wind_eci = quatrot(quat_nedg2eci(pos_eci.row(i), t[i]), vel_wind_ned);
vec3d vel_air_eci = ecef2eci(vel_ecef, t[i]) - vel_wind_eci;
double mach_number = vel_air_eci.norm() / speed_of_sound(altitude);

double ca = interp(mach_number, CA_table.col(0), CA_table.col(1));

vec3d aeroforce_eci = 0.5 * rho * air_area * ca * vel_air_eci.norm() * -vel_air_eci;

double thrust = thrust_vac - nozzle_area * p;
vec3d thrustdir_eci = quatrot(conj(quat_eci2body.row(i)), vec3d(1.0, 0.0, 0.0));
vec3d thrust_eci = thrust * thrustdir_eci;
vec3d gravity_eci = gravity(pos_eci.row(i));
vec3d acc_i = (thrust_eci + aeroforce_eci) / mass[i] + gravity_eci;
acc_eci.row(i) = acc_i;
}

return acc_eci / units[2];
}

matXd dynamics_velocity_NoAir(vecXd mass_e, matXd pos_eci_e, matXd quat_eci2body, vecXd param, vecXd units) {

@@ -36,6 +87,7 @@ matXd dynamics_quaternion(matXd quat_eci2body, matXd u_e, double unit_u) {
}

PYBIND11_MODULE(dynamics_c, m) {
m.def("dynamics_velocity_NoAir", &dynamics_velocity_NoAir, "velocity without air resistance");
m.def("dynamics_velocity", &dynamics_velocity, "velocity with aerodynamic forces");
m.def("dynamics_velocity_NoAir", &dynamics_velocity_NoAir, "velocity without aerodynamic forces");
m.def("dynamics_quaternion", &dynamics_quaternion, "quaternion dynamics");
}
9 changes: 9 additions & 0 deletions src/pybind_utils.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,9 @@
#include "wrapper_utils.hpp"

PYBIND11_MODULE(utils_c, m) {
m.def("haversine", &haversine, "Haversine formula");
m.def("wind_ned", &wind_ned, "Wind in NED frame");
m.def("angle_of_attack_all_rad", &angle_of_attack_all_rad, "Angle of attack in radians");
m.def("angle_of_attack_ab_rad", &angle_of_attack_ab_rad, "Angle of attack in radians");
m.def("dynamic_pressure_pa", &dynamic_pressure_pa, "Dynamic pressure in Pascals");
}
34 changes: 34 additions & 0 deletions src/wrapper_air.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,34 @@
#include <pybind11/eigen.h>
#include <pybind11/numpy.h>
#include <pybind11/pybind11.h>

#include <Eigen/Core>
#include <cmath>

#include "Air.hpp"

namespace py = pybind11;

#ifndef SRC_WRAPPER_AIR_HPP_
#define SRC_WRAPPER_AIR_HPP_

using vec3d = Eigen::Matrix<double, 3, 1>;
using vec4d = Eigen::Matrix<double, 4, 1>;
using vecXd = Eigen::Matrix<double, -1, 1>;
using matXd = Eigen::Matrix<double, -1, -1, Eigen::RowMajor>;

double geopotential_altitude(double z) { return Air::geopotential_altitude(z); }

double airtemperature_at(double altitude_m) {
return Air::temperature(altitude_m);
}

double airpressure_at(double altitude_m) { return Air::pressure(altitude_m); }

double airdensity_at(double altitude_m) { return Air::density(altitude_m); }

double speed_of_sound(double altitude_m) {
return Air::speed_of_sound(altitude_m);
}

#endif // SRC_WRAPPER_AIR_HPP_
Loading

0 comments on commit 38b3ae2

Please sign in to comment.