Subversion Repositories NaviCtrl

Rev

Rev 293 | Rev 491 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 293 Rev 330
1 
#include "91x_lib.h"
 1 
#include "91x_lib.h"
2 
#include "mymath.h"
 2 
#include "mymath.h"
3 
 
 3 
 
4 
// discrete mathematics
 4 
// discrete mathematics
5 
 
 5 
 
6 
// sinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit.
 6 
// sinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit.
7 
const s16 sinlookup[91] = {0, 143, 286, 429, 571, 714, 856, 998, 1140, 1282, 1423, 1563, 1703, 1843, 1982, 2120, 2258, 2395, 2531, 2667, 2802, 2936, 3069, 3201, 3332, 3462, 3591, 3719, 3846, 3972, 4096, 4219, 4341, 4462, 4581, 4699, 4815, 4930, 5043, 5155, 5266, 5374, 5482, 5587, 5691, 5793, 5893, 5991, 6088, 6183, 6275, 6366, 6455, 6542, 6627, 6710, 6791, 6870, 6947, 7022, 7094, 7165, 7233, 7299, 7363, 7424, 7484, 7541, 7595, 7648, 7698, 7746, 7791, 7834, 7875, 7913, 7949, 7982, 8013, 8041, 8068, 8091, 8112, 8131, 8147, 8161, 8172, 8181, 8187, 8191, 8192};
 7 
const s16 sinlookup[91] = {0, 143, 286, 429, 571, 714, 856, 998, 1140, 1282, 1423, 1563, 1703, 1843, 1982, 2120, 2258, 2395, 2531, 2667, 2802, 2936, 3069, 3201, 3332, 3462, 3591, 3719, 3846, 3972, 4096, 4219, 4341, 4462, 4581, 4699, 4815, 4930, 5043, 5155, 5266, 5374, 5482, 5587, 5691, 5793, 5893, 5991, 6088, 6183, 6275, 6366, 6455, 6542, 6627, 6710, 6791, 6870, 6947, 7022, 7094, 7165, 7233, 7299, 7363, 7424, 7484, 7541, 7595, 7648, 7698, 7746, 7791, 7834, 7875, 7913, 7949, 7982, 8013, 8041, 8068, 8091, 8112, 8131, 8147, 8161, 8172, 8181, 8187, 8191, 8192};
- 
 
 8 
                                                //0  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  38  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
- 
 
 9 
const s16 arccos64[65] = {90,89,88,87,86, 85, 84, 83, 83, 82, 81, 80, 79, 78, 77, 76, 75, 74, 73, 72, 71, 71, 70, 69, 68, 67, 66, 65, 64, 63, 62, 61, 60, 59, 58, 57, 56, 55, 54, 53, 51, 50, 49, 48, 47, 45, 44, 43, 41, 40, 39, 37, 36, 34, 32, 31, 29, 27, 25, 23, 20, 18, 14, 10, 0};
8 
 
 10 
 
9 
s16 c_sin_8192(s16 angle)
 11 
s16 c_sin_8192(s16 angle)
10 
{
 12 
{
11 
        s8 m,n;
 13 
        s8 m,n;
12 
        s16 sinus;
 14 
        s16 sinus;
13 
 
 15 
 
14 
        // avoid negative angles
 16 
        // avoid negative angles
15 
        if (angle < 0)
 17 
        if (angle < 0)
16 
        {
 18 
        {
17 
                m = -1;
 19 
                m = -1;
18 
                angle = -angle;
 20 
                angle = -angle;
19 
        }
 21 
        }
20 
        else m = +1;
 22 
        else m = +1;
21 
 
 23 
 
22 
        // fold angle to interval 0 to 359
 24 
        // fold angle to interval 0 to 359
23 
        angle %= 360;
 25 
        angle %= 360;
24 
 
 26 
 
25 
        // check quadrant
 27 
        // check quadrant
26 
        if (angle <= 90) n = 1; // first quadrant
 28 
        if (angle <= 90) n = 1; // first quadrant
27 
        else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second quadrant
 29 
        else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second quadrant
28 
        else if ((angle > 180) && (angle <= 270)) {angle = angle - 180; n = -1;} // third quadrant
 30 
        else if ((angle > 180) && (angle <= 270)) {angle = angle - 180; n = -1;} // third quadrant
29 
        else {angle = 360 - angle; n = -1;}     //fourth quadrant
 31 
        else {angle = 360 - angle; n = -1;}     //fourth quadrant
30 
        // get lookup value
 32 
        // get lookup value
31 
        sinus = sinlookup[angle];
 33 
        sinus = sinlookup[angle];
32 
        // calculate sinus value
 34 
        // calculate sinus value
33 
        return (sinus * m * n);
 35 
        return (sinus * m * n);
34 
}
 36 
}
- 
 
 37 
 
- 
 
 38 
s16 c_arccos2(s32 a,s32 b)
- 
 
 39 
{
- 
 
 40 
 if(a>b) return(0);
- 
 
 41 
 return(arccos64[64 * a / b]);
- 
 
 42 
}
35 
 
 43 
 
36 
// cosinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit.
 44 
// cosinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit.
37 
s16 c_cos_8192(s16 angle)
 45 
s16 c_cos_8192(s16 angle)
38 
{
 46 
{
39 
        return (c_sin_8192(90 - angle));
 47 
        return (c_sin_8192(90 - angle));
40 
}
 48 
}
41 
 
 49 
 
42 
// higher resolution angle in deg is arg/div
 50 
// higher resolution angle in deg is arg/div
43 
s16 c_sin_8192_res(s16 arg, s16 div)
 51 
s16 c_sin_8192_res(s16 arg, s16 div)
44 
{
 52 
{
45 
        s16 angle, rest;
 53 
        s16 angle, rest;
46 
        s32 tmp;
 54 
        s32 tmp;
47 
 
 55 
 
48 
        angle = arg/div;
 56 
        angle = arg/div;
49 
        rest  = arg%div;
 57 
        rest  = arg%div;
50 
 
 58 
 
51 
        if(rest>0)
 59 
        if(rest>0)
52 
        {
 60 
        {
53 
                tmp = (div-rest)*(s32)c_sin_8192(angle);
 61 
                tmp = (div-rest)*(s32)c_sin_8192(angle);
54 
                tmp += rest * (s32)c_sin_8192(angle+1);
 62 
                tmp += rest * (s32)c_sin_8192(angle+1);
55 
                tmp /= div;
 63 
                tmp /= div;
56 
                return(tmp);
 64 
                return(tmp);
57 
        }
 65 
        }
58 
        else if(rest<0)
 66 
        else if(rest<0)
59 
        {
 67 
        {
60 
                tmp = (div+rest)*(s32)c_sin_8192(angle);
 68 
                tmp = (div+rest)*(s32)c_sin_8192(angle);
61 
                tmp -= rest * (s32)c_sin_8192(angle-1);
 69 
                tmp -= rest * (s32)c_sin_8192(angle-1);
62 
                tmp /= div;
 70 
                tmp /= div;
63 
                return(tmp);
 71 
                return(tmp);
64 
        }
 72 
        }
65 
        else
 73 
        else
66 
        {
 74 
        {
67 
                return(c_sin_8192(angle));
 75 
                return(c_sin_8192(angle));
68 
        }
 76 
        }
69 
 
 77 
 
70 
}
 78 
}
71 
 
 79 
 
72 
s16 c_cos_8192_res(s16 arg, s16 div)
 80 
s16 c_cos_8192_res(s16 arg, s16 div)
73 
{
 81 
{
74 
        return(c_sin_8192_res(90*div - arg, div));
 82 
        return(c_sin_8192_res(90*div - arg, div));
75 
}
 83 
}
76 
 
 84 
 
77 
 
 85 
 
78 
// integer based atan2 that returns angle in counts of 1/546.13°  
 86 
// integer based atan2 that returns angle in counts of 1/546.13°  
79 
s32 c_atan2_546(s32 y, s32 x)
 87 
s32 c_atan2_546(s32 y, s32 x)
80 
{
 88 
{
81 
        s32 qx, qy, q;
 89 
        s32 qx, qy, q;
82 
       
 90 
       
83 
        if( x < 0) qx = -x;
 91 
        if( x < 0) qx = -x;
84 
        else qx = x;
 92 
        else qx = x;
85 
        if( y < 0) qy = -y;
 93 
        if( y < 0) qy = -y;
86 
        else qy = y;
 94 
        else qy = y;
87 
        if(qy <= qx)
 95 
        if(qy <= qx)
88 
        {       // scale down to avoid overflow in quadratic interpolation
 96 
        {       // scale down to avoid overflow in quadratic interpolation
89 
                while(qy > (1<<15))
 97 
                while(qy > (1<<15))
90 
                {
 98 
                {
91 
                        qy/=2;
 99 
                        qy/=2;
92 
                        qx/=2;
 100 
                        qx/=2;
93 
                }
 101 
                }
94 
                // calculate the quatratic interpolation
 102 
                // calculate the quatratic interpolation
95 
                q = (((qy<<13)/qx)*qy)/qx;
 103 
                q = (((qy<<13)/qx)*qy)/qx;
96 
                q = (qy<<15)/qx-q;
 104 
                q = (qy<<15)/qx-q;
97 
        }
 105 
        }
98 
        else
 106 
        else
99 
        {   // scale down to avoid overflow in quadratic interpolation
 107 
        {   // scale down to avoid overflow in quadratic interpolation
100 
                while(qx>(1<<15))
 108 
                while(qx>(1<<15))
101 
                {
 109 
                {
102 
                        qy/=2;
 110 
                        qy/=2;
103 
                        qx/=2;
 111 
                        qx/=2;
104 
                }
 112 
                }
105 
                // calculate the quatratic interpolation
 113 
                // calculate the quatratic interpolation
106 
                q = (((qx<<13)/qy)*qx)/qy;
 114 
                q = (((qx<<13)/qy)*qx)/qy;
107 
                q = (qx<<15)/qy-q;
 115 
                q = (qx<<15)/qy-q;
108 
                q = 2*((1<<15)-(1<<13)) - q;
 116 
                q = 2*((1<<15)-(1<<13)) - q;
109 
        }
 117 
        }
110 
        if(y < 0)
 118 
        if(y < 0)
111 
        {
 119 
        {
112 
                if(x < 0)  q = q - 4*((1<<15)-(1<<13));
 120 
                if(x < 0)  q = q - 4*((1<<15)-(1<<13));
113 
                else q = -q;
 121 
                else q = -q;
114 
        }
 122 
        }
115 
        else if( x < 0) q = 4*((1<<15)-(1<<13)) - q;
 123 
        else if( x < 0) q = 4*((1<<15)-(1<<13)) - q;
116 
        return(q);
 124 
        return(q);
117 
}
 125 
}
118 
 
 126