-
Notifications
You must be signed in to change notification settings - Fork 0
/
math.inc
303 lines (268 loc) · 6.12 KB
/
math.inc
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
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
; General Math Functions
; I don't remember the exact sources for the original log, exp, sin, cos and acos algorithms, apart from being available somewhere on the web
; If you know of the sources, let me know so that I may give credit
; returns 1/sqrt(x)
; Inputs:
; AHL = x
; adapted from: https://en.wikipedia.org/wiki/Fast_inverse_square_root
InvSqrt:
ld c,a
call fDiv2CDE ;dec c ; CHL / 2
push hl ; CHL / 2
ld de,$6EB5
rl h \ srl a \ rr h ; (HL << 1), AHL >> 1
cpl ; -a
ld b,a \ call NegHL ; -HL, preserve a
ld a,h \ or l ; hl == 0 ? -- carried
jr nz,InvSqrtNC
inc b ; add carry
InvSqrtNC:
ld a,b ; restore A
add hl,de \ adc a,$5E ; $5E6EB5 - *(int*)AHL
scf \ rr h \ rr l ; y = *(float*)i
pop de \ push hl \ push af ; pop x/2, push y
push de \ push bc ; push x/2
call fSquare ; y*y
pop bc \ pop de ; x/2
call fMult ; x/2*y*y
xor $80 ; -(x/2*y*y)
ld c,$3F \ ld de,$C000 ; CDE = 1.5
call fAdd ; 1.5 - (x/2*y*y)
pop bc \ pop de \ ld c,b ; y
call fMult ; y * (1.5 - (x/2*y*y))
ret
; returns sqrt(1+x)
; Inputs:
; AHL = x
Sqrt1plusX:
ld c,$3F \ ld de,$8000 ; CDE = 1.0
call fAdd ; x + 1
;jr Sqrt
; fall through to sqrt
; returns sqrt(x)
; Inputs:
; AHL = x
Sqrt:
push af \ push hl ; x
call InvSqrt
pop de \ pop bc \ ld c,b
call fMult ; AHL^(-1/2+1)
ret
; returns ln(x)
; Inputs:
; AHL = x
Log:
;call log2
;ld c,$3E \ ld de,$B172 ; CDE = ln(2)
;call fMult ; log2(x) * ln(2)
;ret
Log2:
ld b,a
and $7F \ sub 64
ld c,a ; log_2
push bc ; log_2
ld a,b
and $80 \ or $3F ; exp(val) = 0
push af \ push hl ; val
ld c,$C0 \ ld de,$C000 ; CDE = -3
call fDiv ; val / -3
ld c,$40 \ ld de,$8000 ; CDE = 2
call fAdd ; (val / -3) + 2
pop de \ pop bc \ ld c,b ; CDE = val
call fMult ; ((val / -3) + 2) * val
call fTrunc
ld c,$BE \ ld de,$AAAB ; CDE = -2/3
call fAdd ; ((val / -3) + 2) * val - 2/3
pop bc ; c = log_2
push af \ push hl ; val
ld a,c
call s8tof ; AHL = A
pop de \ pop bc \ ld c,b ; CDE = val
call fAdd ; val + float(log_2)
;ret
;LogCont:
ld c,$3E \ ld de,$B172 ; CDE = ln(2)
call fMult ; log2(x) * ln(2)
ret
; returns e^(x)
; Inputs:
; AHL = x
Exp:
ld c,$3F \ ld de,$B8AA ; CDE = 1.442...
call fMult ; x*1.442...
;jr pow2
Pow2: ; 2^x; AHL = x
ld c,a
and $80 ; sgn(x)
ld b,a ; offset
ex af,af' \ ld a,b \ ex af,af' ; a' = offset
;x < -126 ? -126 : x
jr z,Pow2NoClip ; x > 0
ld a,c \ xor b ; exp(x)
cp $45 ; exp - exp(-126)
jr c,Pow2NoClip ; exp(x) < exp(-126)
jr nz,Pow2Clip ; exp(x) > exp(-126)
ld a,h
cp $FC
jr nc,Pow2NoClip ; x >= -126
Pow2Clip:
ld c,$C5 ; c = exp(-126)
ld hl,$FC00 ; AHL = -126
Pow2NoClip:
ld a,c ; restore exp(AHL)
push af \ push hl ; P
call fInt ; w = int(P), no rounding
pop de \ pop bc ; P
push bc \ push de ; P
;ld c,b ; CDE = P
ld c,a
ld a,b
ex de,hl ; CDE = W, AHL = P
;pop hl \ pop af ; P
;push af \ push hl ; P
call fSub ; P-W
ex af,af' ; a = offset
or a
jr z,Pow2NoOffs ; Offset == 0
ex af,af' ; restore AHL
ld c,$3F \ ld de,$8000 ; CDE = 1.0
call fAdd ; P - W + offset
jr Pow2NoOffs+1
Pow2NoOffs:
ex af,af' ; restore AHL
;Pow2NoOffs+1:
push af \ push hl ; Z
ld c,$3F \ ld de,$BEBD ; CDE = 1.49
call fMult ; Z*1.49
call fTrunc
pop de \ pop bc \ ld c,b ; CDE = Z
push af \ push hl ; z*1.49
ld a,$41 \ ld hl,$9AF6 ; AHL = 4.842
call fSub ; 4.842-Z
ex de,hl \ ld c,a ; CDE = 4.842-Z
ld a,$43 \ ld hl,$DDD3 ; AHL = 27.73
call fDiv ; 27.73/(4.8-z)
pop de \ pop bc \ ld c,b ; CDE = z*1.49
call fSub ; 27.73/(4.8-z) - z*1.49
ld c,$45 \ ld de,$F28C ; CDE = 121.27
call fAdd ; 121.27 + 27.73/(4.8-z) - z*1.49
pop de \ pop bc \ ld c,b ; CDE = P
call fAdd ; p + 121.27 + 27.73/(4.8-z) - z*1.49
add a,15 ; AHL *= 2^15
call ftou24
sla h ; carry = bit7
adc a,a ; a = (a<<1) + carry
sub 64 ; exp(AHL) - 127 + 63
scf ; carry = 1
rr h ; bit7 = 1
ret
; -pi to pi
Sin:
ld c,$3E \ ld de,$A2FA ; CDE = 2/pi
call fMult ; AHL * 2/pi
CosFinish:
push af \ push hl ; AHL * 2/pi
or $80 ; -|x*2/pi|
ld c,$40 \ ld de,$8000 ; CDE = 2
call fAdd ; 2-|x*2/pi|
pop de \ pop bc \ ld c,b ; CDE = x*2/pi
call fMult ; (2-|x*2/pi|)(x*2/pi)
ld c,a \ ld d,h \ ld e,l ; CDE = AHL ; y
push bc \ push de ; y
push bc \ push de ; y
and $7F ; |y|
call fMult ; y*|y|
pop de \ pop bc ; y
call fSub ; y*|y|-y
ld c,$3C \ ld de,$E666 ; CDE = .225 ; p
call fMult ; .225 * (y*|y|-y)
pop de \ pop bc ; y
call fAdd ; .225 * (y*|y|-y) + y
ret
Cos:
ld c,$3E \ ld de,$A2FA ; CDE = 2/pi
call fMult ; AHL * 2/pi
or a
jp m,CosAdd+1 ; AHL < 0 < 1.00
cp $3F ; a-$3F
jr c,CosAdd+1 ; AHL < 1.00
jr nz,CosSub ; AHL > 1.00
ld b,a
xor a
cp h ; $00-h
jp m,CosAdd ; AHL < 1.00
jr nz,CosSub ; AHL > 1.00
cp l ; $00-l
jr nz,CosSub ; AHL > 1.00
;ld b,a
;ld a,$3F
;cp b ; $3F-b
;jr c,CosSub ; b > $3F
;xor a
;cp h ; $00-h
;jp p,CosSub ; h > $80
;cp l ; $00-l
;jr nz,CosSub ; l > $00
CosAdd:
ld a,b
;CosAdd+1:
ld c,$3F \ ld de,$8000 ; CDE = 1
call fAdd
jr CosFinish
CosSub:
ld c,$C0 \ ld de,$C000 ; CDE -3
ld a,b
call fAdd
jr CosFinish
; returns arccos(AHL)
ACos:
ld c,a
and $80 ; sgn(AHL)
ld b,a ; b = negate
xor c ; AHL = |AHL|
push bc ; neg
ld c,a
push bc \ push hl ; X
push bc \ push hl ; X
push bc \ push hl ; X
ld c,$B9 \ ld de,$996E ; CDE = -.018
call fMult ; |X| * -.018
ld c,$3B \ ld de,$9816 ; CDE = .074
call fAdd ; |X| * -.018 + .074
pop de \ pop bc ; BCE = X
call fMult
ld c,$BC \ ld de,$D935
call fAdd
pop de \ pop bc ; BCE = X
call fMult
ld c,$3F \ ld de,$C90E
call fAdd
pop de \ pop bc ; CDE = X
push af \ push hl ; ret
ld a,$3F \ ld hl,$8000
call fSub ; 1-X
call Sqrt ; sqrt(1-X)
pop de \ pop bc \ ld c,b ; CDE = ret
call fMult ; ret * sqrt(1-X)
pop bc ; b = sgn
ld c,a ; store
ld a,b ; a = sgn
or a ; test a
ld a,c ; restore
ret z ; sgn == 0
ex de,hl ; CDE = ret
ld a,$40 \ ld hl,$C910 ; AHL = pi
call fSub ; pi - ret
ret
; Negate DE, only destroying A
NegDE:
ld a,d \ cpl \ ld d,a
ld a,e \ cpl \ ld e,a
inc de
ret
; Negate HL, only destroying A
NegHL:
ld a,h \ cpl \ ld h,a
ld a,l \ cpl \ ld l,a
inc hl
ret