33#include <math.h>
44#include <stdlib.h>
55#include "c4.h"
6- #include "memmap .h"
6+ #include "port .h"
77
88int16_t C4WFXVal ;
99int16_t C4WFYVal ;
@@ -13,70 +13,174 @@ int16_t C4WFY2Val;
1313int16_t C4WFDist ;
1414int16_t C4WFScale ;
1515
16- static double tanval ;
17- static double c4x , c4y , c4z ;
18- static double c4x2 , c4y2 , c4z2 ;
16+ int32_t tanval ;
17+ static int32_t c4x , c4y , c4z ;
18+ static int32_t c4x2 , c4y2 , c4z2 ;
19+
20+ const int16_t C4MulTable [256 ] =
21+ {
22+ 0x0000 , 0x0003 , 0x0006 , 0x0009 , 0x000c , 0x000f , 0x0012 , 0x0015 ,
23+ 0x0019 , 0x001c , 0x001f , 0x0022 , 0x0025 , 0x0028 , 0x002b , 0x002f ,
24+ 0x0032 , 0x0035 , 0x0038 , 0x003b , 0x003e , 0x0041 , 0x0045 , 0x0048 ,
25+ 0x004b , 0x004e , 0x0051 , 0x0054 , 0x0057 , 0x005b , 0x005e , 0x0061 ,
26+ 0x0064 , 0x0067 , 0x006a , 0x006d , 0x0071 , 0x0074 , 0x0077 , 0x007a ,
27+ 0x007d , 0x0080 , 0x0083 , 0x0087 , 0x008a , 0x008d , 0x0090 , 0x0093 ,
28+ 0x0096 , 0x0099 , 0x009d , 0x00a0 , 0x00a3 , 0x00a6 , 0x00a9 , 0x00ac ,
29+ 0x00af , 0x00b3 , 0x00b6 , 0x00b9 , 0x00bc , 0x00bf , 0x00c2 , 0x00c5 ,
30+ 0x00c9 , 0x00cc , 0x00cf , 0x00d2 , 0x00d5 , 0x00d8 , 0x00db , 0x00df ,
31+ 0x00e2 , 0x00e5 , 0x00e8 , 0x00eb , 0x00ee , 0x00f1 , 0x00f5 , 0x00f8 ,
32+ 0x00fb , 0x00fe , 0x0101 , 0x0104 , 0x0107 , 0x010b , 0x010e , 0x0111 ,
33+ 0x0114 , 0x0117 , 0x011a , 0x011d , 0x0121 , 0x0124 , 0x0127 , 0x012a ,
34+ 0x012d , 0x0130 , 0x0133 , 0x0137 , 0x013a , 0x013d , 0x0140 , 0x0143 ,
35+ 0x0146 , 0x0149 , 0x014d , 0x0150 , 0x0153 , 0x0156 , 0x0159 , 0x015c ,
36+ 0x015f , 0x0163 , 0x0166 , 0x0169 , 0x016c , 0x016f , 0x0172 , 0x0175 ,
37+ 0x0178 , 0x017c , 0x017f , 0x0182 , 0x0185 , 0x0188 , 0x018b , 0x018e ,
38+ 0x0192 , 0x0195 , 0x0198 , 0x019b , 0x019e , 0x01a1 , 0x01a4 , 0x01a8 ,
39+ 0x01ab , 0x01ae , 0x01b1 , 0x01b4 , 0x01b7 , 0x01ba , 0x01be , 0x01c1 ,
40+ 0x01c4 , 0x01c7 , 0x01ca , 0x01cd , 0x01d0 , 0x01d4 , 0x01d7 , 0x01da ,
41+ 0x01dd , 0x01e0 , 0x01e3 , 0x01e6 , 0x01ea , 0x01ed , 0x01f0 , 0x01f3 ,
42+ 0x01f6 , 0x01f9 , 0x01fc , 0x0200 , 0x0203 , 0x0206 , 0x0209 , 0x020c ,
43+ 0x020f , 0x0212 , 0x0216 , 0x0219 , 0x021c , 0x021f , 0x0222 , 0x0225 ,
44+ 0x0228 , 0x022c , 0x022f , 0x0232 , 0x0235 , 0x0238 , 0x023b , 0x023e ,
45+ 0x0242 , 0x0245 , 0x0248 , 0x024b , 0x024e , 0x0251 , 0x0254 , 0x0258 ,
46+ 0x025b , 0x025e , 0x0261 , 0x0264 , 0x0267 , 0x026a , 0x026e , 0x0271 ,
47+ 0x0274 , 0x0277 , 0x027a , 0x027d , 0x0280 , 0x0284 , 0x0287 , 0x028a ,
48+ 0x028d , 0x0290 , 0x0293 , 0x0296 , 0x029a , 0x029d , 0x02a0 , 0x02a3 ,
49+ 0x02a6 , 0x02a9 , 0x02ac , 0x02b0 , 0x02b3 , 0x02b6 , 0x02b9 , 0x02bc ,
50+ 0x02bf , 0x02c2 , 0x02c6 , 0x02c9 , 0x02cc , 0x02cf , 0x02d2 , 0x02d5 ,
51+ 0x02d8 , 0x02db , 0x02df , 0x02e2 , 0x02e5 , 0x02e8 , 0x02eb , 0x02ee ,
52+ 0x02f1 , 0x02f5 , 0x02f8 , 0x02fb , 0x02fe , 0x0301 , 0x0304 , 0x0307 ,
53+ 0x030b , 0x030e , 0x0311 , 0x0314 , 0x0317 , 0x031a , 0x031d , 0x0321
54+ };
55+
56+ int16_t C4_Sin (int16_t Angle )
57+ {
58+ if (Angle < 0 )
59+ {
60+ if (Angle == -32768 )
61+ return 0 ;
62+ return - C4_Sin (- Angle );
63+ }
64+ int16_t AngleS7 = Angle >> 7 ;
65+ int32_t S = C4SinTable [AngleS7 ] + (C4MulTable [Angle & 0xff ] * C4SinTable [0x80 + AngleS7 ] >> 15 );
66+ if (S > 32767 )
67+ S = 32767 ;
68+ return (int16_t ) S ;
69+ }
70+
71+ int16_t C4_Cos (int16_t Angle )
72+ {
73+ if (Angle < 0 )
74+ {
75+ if (Angle == -32768 )
76+ return -32768 ;
77+ Angle = - Angle ;
78+ }
79+ int16_t AngleS7 = Angle >> 7 ;
80+ int32_t S = C4SinTable [0x80 + AngleS7 ] - (C4MulTable [Angle & 0xff ] * C4SinTable [AngleS7 ] >> 15 );
81+ if (S < -32768 )
82+ S = -32767 ;
83+ return (int16_t ) S ;
84+ }
85+
86+ const int16_t atantbl [256 ] = {
87+ 0 , 1 , 1 , 2 , 3 , 3 , 4 , 4 , 5 , 6 , 6 , 7 , 8 , 8 , 9 , 10 ,
88+ 10 , 11 , 11 , 12 , 13 , 13 , 14 , 15 , 15 , 16 , 16 , 17 , 18 , 18 , 19 , 20 ,
89+ 20 , 21 , 21 , 22 , 23 , 23 , 24 , 25 , 25 , 26 , 26 , 27 , 28 , 28 , 29 , 29 ,
90+ 30 , 31 , 31 , 32 , 33 , 33 , 34 , 34 , 35 , 36 , 36 , 37 , 37 , 38 , 39 , 39 ,
91+ 40 , 40 , 41 , 42 , 42 , 43 , 43 , 44 , 44 , 45 , 46 , 46 , 47 , 47 , 48 , 49 ,
92+ 49 , 50 , 50 , 51 , 51 , 52 , 53 , 53 , 54 , 54 , 55 , 55 , 56 , 57 , 57 , 58 ,
93+ 58 , 59 , 59 , 60 , 60 , 61 , 62 , 62 , 63 , 63 , 64 , 64 , 65 , 65 , 66 , 66 ,
94+ 67 , 67 , 68 , 69 , 69 , 70 , 70 , 71 , 71 , 72 , 72 , 73 , 73 , 74 , 74 , 75 ,
95+ 75 , 76 , 76 , 77 , 77 , 78 , 78 , 79 , 79 , 80 , 80 , 81 , 81 , 82 , 82 , 83 ,
96+ 83 , 84 , 84 , 85 , 85 , 86 , 86 , 86 , 87 , 87 , 88 , 88 , 89 , 89 , 90 , 90 ,
97+ 91 , 91 , 92 , 92 , 92 , 93 , 93 , 94 , 94 , 95 , 95 , 96 , 96 , 96 , 97 , 97 ,
98+ 98 , 98 , 99 , 99 , 99 , 100 , 100 , 101 , 101 , 101 , 102 , 102 , 103 , 103 , 104 , 104 ,
99+ 104 , 105 , 105 , 106 , 106 , 106 , 107 , 107 , 108 , 108 , 108 , 109 , 109 , 109 , 110 , 110 ,
100+ 111 , 111 , 111 , 112 , 112 , 113 , 113 , 113 , 114 , 114 , 114 , 115 , 115 , 115 , 116 , 116 ,
101+ 117 , 117 , 117 , 118 , 118 , 118 , 119 , 119 , 119 , 120 , 120 , 120 , 121 , 121 , 121 , 122 ,
102+ 122 , 122 , 123 , 123 , 123 , 124 , 124 , 124 , 125 , 125 , 125 , 126 , 126 , 126 , 127 , 127
103+ };
104+
105+ int16_t _atan2 (int16_t x , int16_t y )
106+ {
107+ if (x == 0 )
108+ return 0 ;
109+
110+ int32_t x1 = ABS (x ), y1 = ABS (y );
111+ int32_t absAtan ;
112+
113+ if (x1 > y1 )
114+ absAtan = atantbl [(unsigned char )((y1 << 8 ) / x1 )];
115+ else
116+ absAtan = atantbl [(unsigned char )((x1 << 8 ) / y1 )];
117+
118+ if ((x >= 0 ) ^ (y >= 0 ))
119+ return - absAtan ;
120+
121+ return absAtan ;
122+ }
19123
20124void C4TransfWireFrame ()
21125{
22- c4x = ( double ) C4WFXVal ;
23- c4y = ( double ) C4WFYVal ;
24- c4z = ( double ) C4WFZVal - 0x95 ;
126+ c4x = C4WFXVal ;
127+ c4y = C4WFYVal ;
128+ c4z = C4WFZVal - 0x95 ;
25129
26130 // Rotate X
27- tanval = - ( double ) C4WFX2Val * 3.14159265 * 2 / 128 ;
28- c4y2 = c4y * cos (tanval ) - c4z * sin (tanval );
29- c4z2 = c4y * sin (tanval ) + c4z * cos (tanval );
131+ tanval = - C4WFX2Val << 9 ;
132+ c4y2 = ( c4y * C4_Cos (tanval ) - c4z * C4_Sin (tanval )) >> 15 ;
133+ c4z2 = ( c4y * C4_Sin (tanval ) + c4z * C4_Cos (tanval )) >> 15 ;
30134
31135 // Rotate Y
32- tanval = - ( double ) C4WFY2Val * 3.14159265 * 2 / 128 ;
33- c4x2 = c4x * cos (tanval ) + c4z2 * sin (tanval );
34- c4z = c4x * - sin (tanval ) + c4z2 * cos (tanval );
136+ tanval = - C4WFY2Val << 9 ;
137+ c4x2 = ( c4x * C4_Cos (tanval ) + c4z2 * C4_Sin (tanval )) >> 15 ;
138+ c4z = ( c4x * - C4_Sin (tanval ) + c4z2 * C4_Cos (tanval )) >> 15 ;
35139
36140 // Rotate Z
37- tanval = - ( double ) C4WFDist * 3.14159265 * 2 / 128 ;
38- c4x = c4x2 * cos (tanval ) - c4y2 * sin (tanval );
39- c4y = c4x2 * sin (tanval ) + c4y2 * cos (tanval );
141+ tanval = - C4WFDist << 9 ;
142+ c4x = ( c4x2 * C4_Cos (tanval ) - c4y2 * C4_Sin (tanval )) >> 15 ;
143+ c4y = ( c4x2 * C4_Sin (tanval ) + c4y2 * C4_Cos (tanval )) >> 15 ;
40144
41145 // Scale
42- C4WFXVal = (int16_t )(c4x * ( double ) C4WFScale / (0x90 * (c4z + 0x95 )) * 0x95 );
43- C4WFYVal = (int16_t )(c4y * ( double ) C4WFScale / (0x90 * (c4z + 0x95 )) * 0x95 );
146+ C4WFXVal = (int16_t )((( int32_t ) c4x * C4WFScale * 0x95 ) / (0x90 * (c4z + 0x95 )));
147+ C4WFYVal = (int16_t )((( int32_t ) c4y * C4WFScale * 0x95 ) / (0x90 * (c4z + 0x95 )));
44148}
45149
46150void C4TransfWireFrame2 ()
47151{
48- c4x = ( double ) C4WFXVal ;
49- c4y = ( double ) C4WFYVal ;
50- c4z = ( double ) C4WFZVal ;
152+ c4x = C4WFXVal ;
153+ c4y = C4WFYVal ;
154+ c4z = C4WFZVal ;
51155
52156 // Rotate X
53- tanval = - ( double ) C4WFX2Val * 3.14159265 * 2 / 128 ;
54- c4y2 = c4y * cos (tanval ) - c4z * sin (tanval );
55- c4z2 = c4y * sin (tanval ) + c4z * cos (tanval );
157+ tanval = - C4WFX2Val << 9 ;
158+ c4y2 = ( c4y * C4_Cos (tanval ) - c4z * C4_Sin (tanval )) >> 15 ;
159+ c4z2 = ( c4y * C4_Sin (tanval ) + c4z * C4_Cos (tanval )) >> 15 ;
56160
57161 // Rotate Y
58- tanval = - ( double ) C4WFY2Val * 3.14159265 * 2 / 128 ;
59- c4x2 = c4x * cos (tanval ) + c4z2 * sin (tanval );
60- c4z = c4x * - sin (tanval ) + c4z2 * cos (tanval );
162+ tanval = - C4WFY2Val << 9 ;
163+ c4x2 = ( c4x * C4_Cos (tanval ) + c4z2 * C4_Sin (tanval )) >> 15 ;
164+ c4z = ( c4x * - C4_Sin (tanval ) + c4z2 * C4_Cos (tanval )) >> 15 ;
61165
62166 // Rotate Z
63- tanval = - ( double ) C4WFDist * 3.14159265 * 2 / 128 ;
64- c4x = c4x2 * cos (tanval ) - c4y2 * sin (tanval );
65- c4y = c4x2 * sin (tanval ) + c4y2 * cos (tanval );
167+ tanval = - C4WFDist << 9 ;
168+ c4x = ( c4x2 * C4_Cos (tanval ) - c4y2 * C4_Sin (tanval )) >> 15 ;
169+ c4y = ( c4x2 * C4_Sin (tanval ) + c4y2 * C4_Cos (tanval )) >> 15 ;
66170
67171 // Scale
68- C4WFXVal = (int16_t )(c4x * ( double ) C4WFScale / 0x100 );
69- C4WFYVal = (int16_t )(c4y * ( double ) C4WFScale / 0x100 );
172+ C4WFXVal = (int16_t )((( int32_t ) c4x * C4WFScale ) / 0x100 );
173+ C4WFYVal = (int16_t )((( int32_t ) c4y * C4WFScale ) / 0x100 );
70174}
71175
72176void C4CalcWireFrame ()
73177{
74178 C4WFXVal = C4WFX2Val - C4WFXVal ;
75179 C4WFYVal = C4WFY2Val - C4WFYVal ;
76- if (abs (C4WFXVal ) > abs (C4WFYVal ))
180+ if (ABS (C4WFXVal ) > ABS (C4WFYVal ))
77181 {
78- C4WFDist = abs (C4WFXVal ) + 1 ;
79- C4WFYVal = (int16_t )(256 * ( double ) C4WFYVal / abs (C4WFXVal ));
182+ C4WFDist = ABS (C4WFXVal ) + 1 ;
183+ C4WFYVal = (int16_t )((( int32_t ) C4WFYVal << 8 ) / ABS (C4WFXVal ));
80184 if (C4WFXVal < 0 )
81185 C4WFXVal = -256 ;
82186 else
@@ -86,8 +190,8 @@ void C4CalcWireFrame()
86190 {
87191 if (C4WFYVal != 0 )
88192 {
89- C4WFDist = abs (C4WFYVal ) + 1 ;
90- C4WFXVal = (int16_t )(256 * ( double )C4WFXVal / abs (C4WFYVal ));
193+ C4WFDist = ABS (C4WFYVal ) + 1 ;
194+ C4WFXVal = (int16_t )((( int32_t )C4WFXVal << 8 ) / ABS (C4WFYVal ));
91195 if (C4WFYVal < 0 )
92196 C4WFYVal = -256 ;
93197 else
@@ -103,36 +207,3 @@ int16_t C41FYVal;
103207int16_t C41FAngleRes ;
104208int16_t C41FDist ;
105209int16_t C41FDistVal ;
106-
107- void C4Op1F ()
108- {
109- if (C41FXVal == 0 )
110- {
111- if (C41FYVal > 0 )
112- C41FAngleRes = 0x80 ;
113- else
114- C41FAngleRes = 0x180 ;
115- }
116- else
117- {
118- tanval = (double ) C41FYVal / C41FXVal ;
119- C41FAngleRes = (int16_t )(atan (tanval ) / (3.141592675 * 2 ) * 512 );
120- if (C41FXVal < 0 )
121- C41FAngleRes += 0x100 ;
122- C41FAngleRes &= 0x1FF ;
123- }
124- }
125-
126- void C4Op15 ()
127- {
128- tanval = sqrt ((double ) C41FYVal * C41FYVal + (double ) C41FXVal * C41FXVal );
129- C41FDist = (int16_t ) tanval ;
130- }
131-
132- void C4Op0D ()
133- {
134- tanval = sqrt ((double ) C41FYVal * C41FYVal + (double ) C41FXVal * C41FXVal );
135- tanval = C41FDistVal / tanval ;
136- C41FYVal = (int16_t )(C41FYVal * tanval * 0.99 );
137- C41FXVal = (int16_t )(C41FXVal * tanval * 0.98 );
138- }
0 commit comments