liba 0.1.15

An algorithm library based on C/C++
Documentation 
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
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139

#include "a/trajtrap.h"
#include "a/math.h"

#if A_PREREQ_GNUC(3, 0) || __has_warning("-Wfloat-equal")
#pragma GCC diagnostic ignored "-Wfloat-equal"
#endif /* -Wfloat-equal */

a_float a_trajtrap_gen(a_trajtrap *ctx, a_float vm, a_float ac, a_float de,
                       a_float p0, a_float p1, a_float v0, a_float v1)
{
    a_float v02, v12, vc2;
    a_float const p = p1 - p0;
    a_float const _2p = 2 * p;
    a_bool const reversed = p < 0;
    if (ac == de) { return 0; }
    if (vm < 0) { vm = -vm; }
    v0 = A_SAT(v0, -vm, +vm);
    v1 = A_SAT(v1, -vm, +vm);
    ctx->p0 = p0;
    ctx->p1 = p1;
    ctx->v0 = v0;
    ctx->v1 = v1;
    v02 = v0 * v0;
    v12 = v1 * v1;
    vc2 = (v12 * ac - v02 * de - _2p * ac * de) / (ac - de);
    if (vc2 <= 0) { return 0; }
    if (vc2 > vm * vm) /* acceleration, constant velocity, deceleration */
    {
        ctx->vc = reversed ? -vm : vm;
        ctx->ta = (ctx->vc - v0) / ac;
        ctx->t = (v1 - ctx->vc) / de;
        ctx->pa = p0 + ctx->v0 * ctx->ta + A_FLOAT_C(0.5) * ac * ctx->ta * ctx->ta;
        ctx->pd = p1 - ctx->vc * ctx->t - A_FLOAT_C(0.5) * de * ctx->t * ctx->t;
        ctx->td = ctx->ta + (ctx->pd - ctx->pa) / ctx->vc;
        ctx->t += ctx->td;
    }
    else if (vc2 > v02 && vc2 <= v12) /* acceleration */
    {
        v12 = v02 + _2p * ac;
        if (v12 < 0) { return 0; }
        ctx->v1 = a_float_sqrt(v12);
        if (reversed) { ctx->v1 = -ctx->v1; }
        ctx->vc = ctx->v1;
        ctx->t = (ctx->v1 - v0) / ac;
        ctx->ta = ctx->t;
        ctx->td = ctx->t;
        ctx->pa = p0 + ctx->v0 * ctx->t + A_FLOAT_C(0.5) * ac * ctx->t * ctx->t;
        ctx->pd = p1;
    }
    else if (vc2 <= v02 && vc2 > v12) /* deceleration */
    {
        v12 = v02 + _2p * de;
        if (v12 < 0) { return 0; }
        ctx->v1 = a_float_sqrt(v12);
        if (reversed) { ctx->v1 = -ctx->v1; }
        ctx->vc = ctx->v0;
        ctx->t = (ctx->v1 - v0) / de;
        ctx->ta = 0;
        ctx->td = 0;
        ctx->pa = p0;
        ctx->pd = p0;
    }
    else /* acceleration, deceleration */
    {
        ctx->vc = a_float_sqrt(vc2);
        if (reversed) { ctx->vc = -ctx->vc; }
        ctx->t = (ctx->vc - v0) / ac;
        ctx->ta = ctx->t;
        ctx->td = ctx->t;
        ctx->pa = p0 + ctx->v0 * ctx->t + A_FLOAT_C(0.5) * ac * ctx->t * ctx->t;
        ctx->t += (v1 - ctx->vc) / de;
        ctx->pd = ctx->pa;
    }
    ctx->ac = ac;
    ctx->de = de;
    return ctx->t;
}

a_float a_trajtrap_pos(a_trajtrap const *ctx, a_float x)
{
    if (x >= ctx->ta)
    {
        if (x < ctx->td) /* linear motion */
        {
            return ctx->pa + ctx->vc * (x - ctx->ta);
        }
        if (x < ctx->t) /* final blend */
        {
            x -= ctx->td;
            return ctx->pd + ctx->vc * x + A_FLOAT_C(0.5) * ctx->de * x * x;
        }
        return ctx->p1;
    }
    if (x > 0) /* initial blend */
    {
        return ctx->p0 + ctx->v0 * x + A_FLOAT_C(0.5) * ctx->ac * x * x;
    }
    return ctx->p0;
}

a_float a_trajtrap_vel(a_trajtrap const *ctx, a_float x)
{
    if (x >= ctx->ta)
    {
        if (x < ctx->td) /* linear motion */
        {
            return ctx->vc;
        }
        if (x < ctx->t) /* final blend */
        {
            return ctx->vc + ctx->de * (x - ctx->td);
        }
        return ctx->v1;
    }
    if (x > 0) /* initial blend */
    {
        return ctx->v0 + ctx->ac * x;
    }
    return ctx->v0;
}

a_float a_trajtrap_acc(a_trajtrap const *ctx, a_float x)
{
    if (x < ctx->ta)
    {
        if (x >= 0) /* initial blend */
        {
            return ctx->ac;
        }
    }
    else if (x >= ctx->td)
    {
        if (x <= ctx->t) /* final blend */
        {
            return ctx->de;
        }
    }
    return 0; /* linear motion */
}