summaryrefslogtreecommitdiff
path: root/fw/fe310/eos/eve/eve_phy.c
blob: cfddc803841263ad065a8f7cbfb6858b75608be9 (plain)
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
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
#include <stdlib.h>
#include <math.h>

#include "eve_platform.h"
#include "eve_phy.h"

/* Constant accelerator */
void eve_phy_acc_init(EVEPhyAcc *param, int a) {
    param->a = a;
}

int eve_phy_acc_start(EVEPhyAcc *param, int x0, int y0, uint32_t t0, int v0x, int v0y) {
    double v0;
    uint32_t dt = eve_get_tick() - t0;

    v0 = sqrt(v0x * v0x + v0y * v0y);
    param->k = 2 * v0 / param->a * EVE_RTC_FREQ;
    if ((param->k < 0) && (dt >= -param->k / 2)) {
        return 0;
    }
    param->x0 = x0;
    param->y0 = y0;
    param->v0x = v0x;
    param->v0y = v0y;

    return 1;
}

int eve_phy_acc_tick(EVEPhyAcc *param, uint32_t dt, int *x, int *y) {
    int k = param->k;
    int x0 = param->x0;
    int y0 = param->y0;
    int v0x = param->v0x;
    int v0y = param->v0y;
    int _dt = dt;
    int more = 1;

    if (k == 0) {
        if (x) *x = x0;
        if (y) *y = y0;
        return 0;
    }

    if ((k < 0) && (dt >= -k / 2)) {
        dt = -k / 2;
        more = 0;
    }
    if (x) *x = x0 + (v0x * _dt + v0x * _dt / k * _dt) / EVE_RTC_FREQ;
    if (y) *y = y0 + (v0y * _dt + v0y * _dt / k * _dt) / EVE_RTC_FREQ;

    return more;
}

/* Linear harmonic oscillator */
void eve_phy_lho_init(EVEPhyLHO *param, int x, int y, uint32_t T, double d, uint32_t t_max) {
    double f0 = 2 * M_PI / (T * EVE_RTC_FREQ / 1000);

    if (d < 0) d = 0;
    if (d > 1) d = 1;
    param->x = x;
    param->y = y;
    param->f = f0 * sqrt(1 - d * d);
    param->a = -d * f0;
    param->t_max = t_max * EVE_RTC_FREQ / 1000;
}

int eve_phy_lho_start(EVEPhyLHO *param, int x0, int y0) {
    param->x0 = x0;
    param->y0 = y0;

    return 1;
}

int eve_phy_lho_tick(EVEPhyLHO *param, uint32_t dt, int *x, int *y) {
    int ax = param->x0 - param->x;
    int ay = param->y0 - param->y;
    int more = 1;

    if (param->t_max && (dt >= param->t_max)) {
        dt = param->t_max;
        more = 0;
    }
    if (param->a) {
        double e = exp(param->a * dt);
        ax = ax * e;
        ay = ay * e;
        if ((ax == 0) && (ay == 0)) more = 0;
    }
    if (x) *x = param->x + ax * cos(param->f * dt);
    if (y) *y = param->y + ay * cos(param->f * dt);

    return more;
}