-
-
Notifications
You must be signed in to change notification settings - Fork 6
Expand file tree
/
Copy pathmaths.asm
More file actions
482 lines (431 loc) · 9.41 KB
/
maths.asm
File metadata and controls
482 lines (431 loc) · 9.41 KB
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
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
;
; Title: Fast 3D Maths Routines
; Author: Dean Belfield
; Created: 20/08/2025
; Last Updated: 24/11/2025
;
; Modinfo:
; 20/11/2025: Added fastDiv16
; 21/11/2025 Improved performance of project3D
; 22/11/2025: Refactored multiply and divide routines
; 23/11/2025: Added more multiply routines, refactored sin and cos routines
; 24/11/2025: Minor performance tweak in sin and cos routines
;
SECTION KERNEL_CODE
INCLUDE "globals.inc"
EXTERN sin_table ; In ram.inc
EXTERN div_table ; In ram.inc
; These are declared here
; https://github.com/z88dk/z88dk/tree/master/libsrc/_DEVELOPMENT/math/integer/z80n
EXTERN l_divu_32_32x16 ; DEHL = DEHL / BC (unsigned)
PUBLIC _pd ; int pd
PUBLIC sin8, sin16
PUBLIC cos8, cos16
PUBLIC negHL, negDE, negBC, negDEHL
PUBLIC absHL, absDE, absBC, absDEHL
PUBLIC muldivs16_16x16
PUBLIC muldivs32_16x16
PUBLIC muls32_16x16
PUBLIC mulu32_16x16
PUBLIC muls16_16x16
PUBLIC mulu16_16x16
PUBLIC divs16_16x16
PUBLIC divu16_16x16
_pd: DW 256 ; Perspective distance
; Negate HL and ABS(HL)
;
PUBLIC negHL, absHL
absHL: BIT 7,H
RET P
negHL: XOR A
SUB L
LD L,A
SBC A,A
SUB H
LD H,A
RET
; Negate DE and ABS(DE)
;
PUBLIC negDE, absDE
absDE: BIT 7,D
RET P
negDE: XOR A
SUB E
LD E,A
SBC A,A
SUB D
LD D,A
RET
; Negate BC and ABS(BC)
;
PUBLIC negBC, absDE
absBC: BIT 7,B
RET P
negBC: XOR A
SUB C
LD C,A
SBC A,A
SUB B
LD B,A
RET
; Negate DEHL and ABS(DEHL)
;
absDEHL: BIT 7,D
RET P
negDEHL: LD A,L
CPL
LD L,A
LD A,H
CPL
LD H,A
LD A,E
CPL
LD E,A
LD A,D
CPL
LD D,A
INC L
RET NZ
INC H
RET NZ
INC DE
RET
; extern int8_t sin8(uint8_t a, int8_t m) __z88dk_callee;
; extern int8_t cos8(uint8_t a, int8_t m) __z88dk_callee;
;
PUBLIC _sin8
PUBLIC _cos8
_sin8: POP BC
POP DE ; D: Angle, E: Multiplier
LD A,D ; A: Angle
CALL sin8
LD L,D
PUSH BC
RET
_cos8: POP BC
POP DE ; D: Angle, E: Multiplier
LD A,D
CALL cos8
LD L,D
PUSH BC
RET
; D=COS(A)*E/256
; D=SIN(A)*E/256
;
cos8: ADD A,64 ; Cosine is a quarter revolution copy of the sin wave
sin8: LD H,sin_table >> 8 ; The sin table is a 128 byte table on a page boundary
LD L,A ; Index into the table
RES 7,L ; It's only a 128 byte table, so clear the top bit
LD D,(HL) ; Fetch the value from the sin table
RLCA ; Get the sign of the angle
BIT 7,E ; And the sign of the multiplicand
JR C,sin8_neg_angle ; Skip to the case where the sin angle is negative
;
; Here, the angle (D) is positive
;
sin8_pos_angle: JR Z,sin8_res_pos ; If the multiplicand is positive, then positive result
XOR A ; Negate the multiplicand
SUB E
LD E,A ; And drop through to return a negative result
;
; Here, the result will be negative
;
sin8_res_neg: MUL D,E ; A = -(D*A/256)
XOR A ; Negate the result
SUB D
LD D,A
RET
;
; Here the angle (D) is negative
;
sin8_neg_angle: JR Z,sin8_res_neg ; If the multiplicand is positive, then negative result
XOR A ; Otherwise negate the multiplicand
SUB E
LD E,A
;
; Here, the result will be positive
;
sin8_res_pos: MUL D,E ; A = +(D*A/256)
RET
; extern int16_t sin16(uint8_t a, int16_t m);
; extern int16_t cos16(uint8_t a, int16_t m);
;
PUBLIC _sin16
PUBLIC _cos16
_sin16: LD HL,2
ADD HL,SP ; Skip over return address
LD A,(HL) ; A: Angle
INC HL
LD E,(HL)
INC HL
LD D,(HL) ; DE: Multiplier
JR sin16
_cos16: LD HL,2
ADD HL,SP ; Skip over return address
LD A,(HL) ; A: Angle
INC HL
LD E,(HL)
INC HL
LD D,(HL) ; DE: Multiplier
JR cos16
; HL=COS(A)*DE/256
; HL=SIN(A)*DE/256
;
cos16: ADD A,64 ; Cosine is a quarter revolution copy of the sin wave
sin16: LD H,sin_table >> 8 ; The sin table is a 128 byte table on a page boundary
LD L,A ; Index into the table
RES 7,L ; It's only a 128 byte table, so clear the top bit
LD L,(HL) ; Fetch the value from the sin table
RLCA ; Get the sign of the angle
BIT 7,D ; And the sign of the multiplicand
JR C,sin16_neg_angle ; Skip to the case where the sin angle is negative
;
sin16_pos_angle: JR Z,sin16_mul
CALL negDE ; Otherwise negate the multiplicand
sin16_mul_neg: CALL sin16_mul
JP negHL ; And return a negative result
;
sin16_neg_angle: JR Z,sin16_mul_neg
CALL negDE ; Otherwise negate the multiplicand
;
; HL = L * DE / 256
;
sin16_mul: LD H,E
LD D,D
LD E,L
MUL D,E ; DE: YH*X
EX DE,HL
MUL D,E ; DE: YL*X
LD A,D
ADD A,L
LD L,A
LD A,H
ADC A,0
LD H,A
RET
; extern int16_t muldivs32_16x16(int16_t a, int16_t b, int16_t c) __z88dk_callee
; Calculates a * b / c, with the internal calculation done in 32-bits
;
PUBLIC _muldivs32_16x16, muldivs32_16x16
_muldivs32_16x16: POP IY
POP HL ; a
POP DE ; b
POP BC ; c
PUSH IY
; HL = HL * DE / BC
;
muldivs32_16x16: PUSH BC ; Save this somewhere
CALL muls32_16x16 ; DEHL: 32-bit signed product
POP BC
LD A,B ; Get the sign
XOR D
PUSH AF
CALL absDEHL
CALL absBC
CALL l_divu_32_32x16
POP AF
RET P ; Answer is positive
JP negHL ; Answer is negative so negate it
; extern int16_t muldivs16_16x16(int16_t a, int16_t b, int16_t c) __z88dk_callee
; Calculates a * b / c, with the internal calculation done in 16-bits
;
PUBLIC _muldivs16_16x16, muldivs16_16x16
_muldivs16_16x16: POP IY
POP HL ; a
POP DE ; b
POP BC ; c
PUSH IY
; HL = HL * DE / BC
;
muldivs16_16x16: LD A,H
XOR D
XOR B
PUSH AF ; Work out the final sign
CALL absHL
CALL absDE
CALL absBC
PUSH BC ; BC: Save the divisor
CALL mulu16_16x16 ; HL: 16-bit product (HL*DE)
POP DE ; DE: The divisor
CALL divu16_16x16 ; HL: HL*DE/BC
POP AF ; F: The sign
RET P ; Answer is positive
JP negHL
; extern uint16_t mulu16_16x16(uint16_t a, uint16_t b) __z88dk_callee;
; Calculates a * b (unsigned)
;
PUBLIC _mulu16_16x16, mulu16_16x16
_mulu16_16x16: POP IY
POP HL
POP DE
PUSH IY
; HL: Multiplicand
; DE: Multiplier
; Returns
; HL: Result (HL*DE)
;
mulu16_16x16: LD A,D
LD D,H
LD H,A
LD C,E
LD B,L
MUL D,E
EX DE,HL ; HL: H*E
MUL D,E ; DE: D*L
ADD HL,DE ; HL: H*E+D*L - Adding the cross products
LD E,C
LD D,B
MUL D,E ; DE: E*L
LD A,L ; A: Cross product LSB
ADD A,D
LD H,A
LD L,E
RET
; extern int16_t muls16_16x16(int16_t a, int16_t b) __z88dk_callee;
; Calculates a * b (signed)
;
PUBLIC _muls16_16x16, muls16_16x16
_muls16_16x16: POP IY
POP HL
POP DE
PUSH IY
; HL: Multiplicand
; DE: Multiplier
; Returns
; HL: Result (HL*DE)
;
muls16_16x16: LD A,H
XOR D
PUSH AF
CALL absHL
CALL absDE
CALL mulu16_16x16
POP AF
RET P
JP negHL
; extern int16_t mulu32_16x16(int16_t a, int16_t b) __z88dk_callee;
; Calculates a * b (signed)
;
PUBLIC _mulu32_16x16, mulu32_16x16
_mulu32_16x16: POP IY
POP HL
POP DE
PUSH IY
; HL: Multiplicand
; DE: Multiplier
; Returns
; DEHL: Result (HL*DE)
;
mulu32_16x16: LD B,L ; x0
LD C,E ; y0
LD E,L ; x0
LD L,D
PUSH HL ; x1 y1
LD L,C ; y0
MUL DE ; y1*x0
EX DE,HL
MUL DE ; x1*y0
XOR A
ADD HL,DE ; sum cross products p2 p1
ADC A,A ; capture carry p3
LD E,C ; x0
LD D,B ; y0
MUL DE ; y0*x0
LD B,A ; carry from cross products
LD C,H ; LSB of MSW from cross products
LD A,D
ADD A,L
LD H,A
LD L,E ; LSW in HL p1 p0
POP DE
MUL DE ; x1*y1
EX DE,HL
ADC HL,BC
EX DE,HL ; DE = final MSW
RET
; extern int32_t muls32_16x16(int16_t a, int16_t b) __z88dk_callee;
; Calculates a * b (signed)
;
PUBLIC _muls32_16x16, muls32_16x16
_muls32_16x16: POP IY
POP HL
POP DE
PUSH IY
; HL: Multiplicand
; DE: Multiplier
; Returns
; DEHL: Result (HL*DE)
;
muls32_16x16: LD A,H
XOR D
PUSH AF
CALL absHL
CALL absDE
CALL mulu32_16x16
POP AF
RET P
JP negDEHL
; extern uint16_t divu16_16x16(uint16_t a, uint16_t b) __z88dk_callee;
; Calculates an estimate of a / b (unsigned)
; Inspired by https://blog.segger.com/algorithms-for-division-part-3-using-multiplication/
;
PUBLIC _divu16_16x16, divu16_16x16
_divu16_16x16: POP IY
POP HL ; HL: Dividend
POP DE ; DE: Divisor
PUSH IY
; HL: Dividend
; DE: Divisor
; Returns:
; HL: The result (HL/DEV)
;
divu16_16x16: LD A,D ; Check for divide by zero
OR E
RET Z
EX DE,HL ; Swap the dividend and divisor
LD A,15 ; The bit counter
@L1: BIT 7,H ; Check high byte of HL
JR NZ,@S1
ADD HL,HL ; Normalise HL by shifting it right
DEC A ; Increment the bit counter
JR @L1
@S1: EX AF,AF ; Store the bit counter for later
;
; Look up the reciprocal in the table
;
LD A,H ; A = HL >> 8
ADD A,A ; Now index into the table
LD L,A
LD H,div_table >> 8
LD A,(HL)
INC L
LD H,(HL)
LD L,A ; Not needed, use A instead of L in next block
CALL mulu32_16x16 ;
EX AF,AF ; The bit counter
LD B,A
BSRL DE,B ; Undo the normalisation by shifting right B times
EX DE,HL
RET
; extern int16_t divs16_16x16(int16_t a, int16_t b) __z88dk_callee;
; Calculates an estimate of a / b (signed)
;
PUBLIC _divs16_16x16, divs16_16x16
_divs16_16x16: POP IY
POP HL ; HL: Dividend
POP DE ; DE: Divisor
PUSH IY
; HL: Dividend
; DE: Divisor
; Returns:
; HL: The result (HL/DEV)
;
divs16_16x16: LD A,H
XOR D
PUSH AF
CALL absHL
CALL absDE
CALL divu16_16x16
POP AF
RET P
JP negHL