rk4_solver/examples/event-example.cpp at main · cinaral/rk4_solver · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
#include "rk4_solver/loop.hpp"

using rk4_solver::Real_T;
using rk4_solver::size_t;

constexpr size_t sample_freq = 1e5;
constexpr Real_T time_step = 1. / sample_freq;
constexpr Real_T t_init = 0.;
constexpr Real_T t_final = 2.;
constexpr size_t t_dim = sample_freq * (t_final - t_init) + 1;
constexpr size_t x_dim = 2;
constexpr Real_T x_init[x_dim] = {1., 0.};
constexpr Real_T e_restitution = .75;
constexpr Real_T gravity_const = 9.806;

struct Dynamics {
	/*
	 * Bouncing ball in one dimension:
	 * dt_x =  [x2; -g]
	 */
	void
	ode_fun(const Real_T, const Real_T (&x)[x_dim], Real_T (&dt_x)[x_dim])
	{
		dt_x[0] = x[1];
		dt_x[1] = -gravity_const;
	}
	/*
	 * Impact event:
	 * x+ = 0
	 * dt_x+ = -e * dt_x
	 */
	bool
	event_fun(const Real_T, const Real_T (&x)[x_dim], Real_T (&x_plus)[x_dim])
	{
		bool did_occur = false;

		if (x[0] <= 0) {
			x_plus[0] = 0;
			x_plus[1] = -e_restitution * x[1];
			did_occur = true;
		}
		return did_occur;
	}
};
Dynamics dynamics;

int
main()
{
	rk4_solver::Integrator<x_dim, Dynamics> integrator(dynamics, &Dynamics::ode_fun, time_step);
	rk4_solver::Event<x_dim, Dynamics> event(dynamics, &Dynamics::event_fun);
	Real_T t;
	Real_T x[x_dim];
	//* discard the intermediate values and stop at first event
	rk4_solver::loop<t_dim>(integrator, event, t_init, x_init, t, x, true);

	return 0;
}