/[projet1]/users/chema/TINE/tinefuncs.s
Defence Force logotype

Contents of /users/chema/TINE/tinefuncs.s

Parent Directory Parent Directory | Revision Log Revision Log


Revision 266 - (show annotations)
Mon Feb 8 22:12:53 2010 UTC (9 years, 8 months ago) by Chema
File size: 23148 byte(s)
many small changes and bug huntings. Newest line routine, removed clearing the laser vertices from the main loop...
1 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
2 ; Some TINE functions
3 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
4
5 #include "ships.h"
6 #include "tine.h"
7 #include "oobj3d\params.h"
8
9
10 ;; Main functions
11
12 ;; Some extensions of obj3d
13 ;; some can be called from C
14
15
16 ;; Gets the pixel address and scan.
17 ;; Params X reg X, Y reg Y
18 ;; Returns
19 ;; tmp0 address of line
20 ;; reg Y scan in line (tmp0),y is the pointer to the actual scan
21 ;; reg A scan code with the pixel to ora or eor in screen...
22
23 #ifndef FILLEDPOLYS
24 pixel_address_real
25 .(
26 lda double_buff
27 beq pixel_address
28
29 lda _HiresAddrLow,y ; 4
30 sta tmp0+0 ; 3
31 lda _HiresAddrHigh,y ; 4
32 sta tmp0+1 ; 3 => Total 14 cycles
33
34 ; Put back to onscreen coordinates
35 lda #<($a000-buffer)
36 clc
37 adc tmp0
38 sta tmp0
39 lda #>($a000-buffer)
40 adc tmp0+1
41 sta tmp0+1
42
43 ldy _TableDiv6,x
44 lda _TableBit6Reverse,x ; 4
45
46 rts
47
48
49 .)
50
51 pixel_address
52 .(
53 lda _HiresAddrLow,y ; 4
54 sta tmp0+0 ; 3
55 lda _HiresAddrHigh,y ; 4
56 sta tmp0+1 ; 3 => Total 14 cycles
57
58 ldy _TableDiv6,x
59 lda _TableBit6Reverse,x ; 4
60
61 rts
62
63 .)
64 #else
65 pixel_address_real
66 pixel_address
67 .(
68
69 lda _ScreenPtrLow,y
70 sta tmp0
71 lda _ScreenPtrHigh,y
72 sta tmp0+1
73
74
75 lda _Mod6Left,x
76 and #%00111111
77 sec
78 ror
79 ora _Mod6Right,x
80 eor #$ff
81 ldy _Div6,x
82 rts
83 .)
84 #endif
85
86
87 ;; LaunchShipFromOther
88 ;; Launches a ship from current ship
89 ;; Reg A contains the new ship type
90 ;; Returns the new ship ID in reg X
91
92 LaunchShipFromOther
93 .(
94
95 pha
96 jsr GetCurOb
97 sta loop+1
98 sty loop+2
99 stx savx+1
100
101 jsr _GetShipPos
102 lda #<_PosX
103 sta tmp0
104 lda #>_PosY
105 sta tmp0+1
106
107 pla
108 jsr AddSpaceObject
109 cpx #0
110 beq failure
111
112 ; Copy orientation matrix
113
114 ldy #ObjMat
115 loop
116 lda $1234,y ; SMC
117 sta (POINT),y
118 iny
119 cpy #(ObjMat+18)
120 bne loop
121
122 ; Match speeds
123 savx
124 ldy #0 ; SMC
125 lda _speed,y
126 sta _speed,x
127
128 failure
129
130 rts
131
132 .)
133
134
135
136 ;; Adds an space object to the world
137 ;; The pointer to the center is passed in tmp0
138 ;; and the ship type in A
139 ;; Returns X=ship entry (ID), POINT contains pointer to object data
140 AddSpaceObject
141 .(
142 ; Load ship Model
143 ;txa
144 sta saveme+1
145 and #%01111111
146 tax
147 stx saveModelIndex+1
148
149
150 ; See if we have enough space...
151 lda NUMOBJS
152 cmp #(MAXSHIPS)
153 bne roomok
154 noroom
155 ldx #0
156 rts
157 roomok
158
159 lda ShipModelHi-1,x
160 tay
161 lda ShipModelLo-1,x
162
163 ; Add object to oobj3d
164 saveme
165 ldx #0 ; SMC
166
167 +AddSpaceObjectDirect
168 jsr AddObj
169
170 ; Now the position
171
172 ;lda tmp0+1
173 ;ora tmp0
174 ;beq nopos ; If pointer is zero, not a valid address
175
176 sta POINT ;Object pointer
177 sty POINT+1
178 ldy #5 ;Set center
179 l1 lda (tmp0),y
180 sta (POINT),y
181 dey
182 bpl l1
183
184 saveModelIndex
185 ldy #0 ; SMC, get model index
186
187 ; Set all fields
188
189 ; Initial energy... default for ship
190 lda ShipEnergy-1,y
191 sta _energy,x
192
193 ; Number of missiles and strength of lasers
194 lda ShipAmmo-1,y
195 ;and #%11111000
196 sta _missiles,x
197
198 ; Set rest of fields to default
199 lda #0
200 sta _rotx,x
201 sta _roty,x
202 sta _rotz,x
203 sta _speed,x
204 sta _accel,x
205 sta _target,x
206 sta _flags,x
207 sta _ttl,x
208 sta _ai_state,x
209
210 ;nopos
211 rts
212 .)
213
214 _SetCurrentObject
215 .(
216 ldy #0
217 lda (sp),y
218 ;sta _curr_ship
219 ;tax
220 ;lda _ids,x
221 tax
222 jmp SetCurOb
223 .)
224
225
226
227 _GetFrontVector
228 .(
229 jsr GetFrontVec
230 stx _VX
231 sty _VY
232 sta _VZ
233 rts
234
235 .)
236
237
238 _GetDownVector
239 .(
240 jsr GetDownVec
241 stx _VX
242 sty _VY
243 sta _VZ
244 rts
245 .)
246
247
248 _GetSideVector
249 .(
250 jsr GetSideVec
251 stx _VX
252 sty _VY
253 sta _VZ
254 rts
255 .)
256
257
258 _GetShipPos
259 .(
260 jsr GetCurOb
261 jmp GetShipPos
262
263 .)
264
265
266 ; Gets the position of ship
267 ; given in reg X
268 ; Stores it in _PosX,Y and Z
269 ; all 16 bit signed.
270 GetShipPos
271 .(
272 jsr GetObj
273
274 sta POINT ;Object pointer
275 sty POINT+1
276
277 ldy #0
278 lda (POINT),y
279 sta _PosX
280 iny
281 lda (POINT),y
282 sta _PosX+1
283 iny
284
285
286 lda (POINT),y
287 sta _PosY
288 iny
289 lda (POINT),y
290 sta _PosY+1
291 iny
292
293 lda (POINT),y
294 sta _PosZ
295 iny
296 lda (POINT),y
297 sta _PosZ+1
298
299 rts
300 .)
301
302
303 ; Gets the type of ship
304 ; given in reg X
305 ; and returns it in reg A
306 GetShipType
307 .(
308 jsr GetObj
309 ; Check ship ID byte...
310 sta POINT ;Object pointer
311 sty POINT+1
312 ldy #ObjID
313 lda (POINT),y
314 rts
315
316 .)
317
318
319
320 ; Gets the user byte (equipment)
321 ; given in reg X
322 ; and returns it in reg A
323 GetShipEquip
324 .(
325 jsr GetObj
326 ; Check ship ID byte...
327 sta POINT ;Object pointer
328 sty POINT+1
329 ldy #ObjUser
330 lda (POINT),y
331 rts
332 .)
333
334
335 ; Sets the user byte (equipment)
336 ; of the ship given in reg X
337 ; as the value given in reg a
338 SetShipEquip
339 .(
340 pha
341 jsr GetObj
342 ; Check ship ID byte...
343 sta POINT ;Object pointer
344 sty POINT+1
345 ldy #ObjUser
346 pla
347 sta (POINT),y
348 rts
349 .)
350
351
352
353 ; Basic stats for a new ship for the player
354 NewPlayerShip
355 .(
356
357 ; Initializations for first-time usage
358 ldx _ship_type
359 lda ShipAmmo-1,x
360 and #%111
361 sta _p_maxmissiles
362 sec
363 sbc #1
364 sta _missiles_left
365 +PreInit
366 ;lda ShipMaxSpeed-1,x
367 lda #23
368 sta _p_maxspeed
369 ;lda ShipEnergy-1,x
370 lda #72
371 sta _p_maxenergy
372
373 ldx #8
374 loop
375 lda tab_rolls_slow,x
376 sta tab_rolls,x
377 dex
378 bpl loop
379
380 rts
381
382 .)
383
384 ;; Get the initial stats for player's ship
385 InitPlayerShip
386 .(
387
388 jsr PreInit
389
390 ; Depending on ship's equipment...
391
392 /* lda _equip
393 and #%10 ; Large cargo bay
394 beq nocargo
395 lda _holdspace
396 clc
397 adc #10
398 sta _holdspace
399 nocargo
400
401 */
402 lda _equip
403 ror
404 bcc nopulse
405 ;Pulse laser
406 ldx #PULSE_LASER
407 nopulse
408 lda _equip+1
409 ror
410 bcc nobeam
411 ldx #BEAM_LASER
412 nobeam
413 ror
414 bcc nomil
415 ldx #MILITARY_LASER
416 nomil
417
418 afterlaser
419 stx _p_laserdamage
420
421 ror
422 bcc nospeed
423 tax
424 lda _p_maxspeed
425 clc
426 adc #5
427 sta _p_maxspeed
428 txa
429 nospeed
430
431 ror
432 bcc noman
433
434 ldx #8
435 loop
436 lda tab_rolls_fast,x
437 sta tab_rolls,x
438 dex
439 bpl loop
440 noman
441
442 lda _equip
443 and #%01000000 ; Extra energy
444 beq noenergy
445 clc
446 lda _p_maxenergy
447 adc #24
448 sta _p_maxenergy
449 noenergy
450
451 ; Shields
452 lda #22
453 sta _front_shield
454 sta _rear_shield
455
456 ; Energy, speed,...
457 lda _p_maxenergy
458 sta _energy+1
459 lda _p_maxspeed
460 lsr
461 sta _speed+1
462
463 ; Missile armed
464 lda #0
465 sta _missile_armed
466 sta _ptla
467 sta _ptsh
468
469 rts
470 .)
471
472
473
474 ;; For moving ships
475
476 _MoveShips
477 .(
478 jsr MovePlayer ; Start moving player
479
480 ; Now iterate through object list moving the rest
481
482 ; Avoid fixed objects
483 ldx fixed_objects
484 dex
485 ;jsr SetCurOb
486 stx CUROBJ
487 jsr GetNextOb
488
489 ; Carry shoubd be clear, unless error
490 ;bcs end
491 ; Check if we got back to object 0 (radar)
492 cpx #0
493 beq end
494
495 loop
496 sta POINT ;Object pointer
497 sty POINT+1
498
499 ; Get ship ID byte...
500 ldy #ObjID
501 lda (POINT),y
502
503 jsr MoveCurrent
504
505 nomove
506 jsr GetNextOb
507 ;bcc loop
508 ; Carry shoubd be clear, unless error
509 ; Check if we got back to object 0
510 cpx #0
511 bne loop
512
513 end
514 rts
515
516 .)
517
518 ;; Routines to perform movement of ships. They are
519 ;; separated in two, one for the player and other
520 ;; for the rest of ships, as different things should be performed
521 ;; and we avoid filling them with comparisons...
522
523 ;; These globals are used by stars...
524
525 g_alpha .byt 0 ; Total rotation in X (Pitch)
526 g_beta .byt 0 ; Total rotation in Y (Yaw)
527 g_delta .byt 0 ; Total rotation in Z (Roll)
528 g_theta .byt 0 ; Total speed?
529
530 #define mancount tmp
531
532 MovePlayer
533 .(
534 ; Patch ACCROTX, etc.
535 lda #<ROTXY2
536 sta patch_rot1+1
537 sta patch_rot2+1
538
539 lda #>ROTXY2
540 sta patch_rot1+2
541 sta patch_rot2+2
542
543 ldx #1
544 jsr SetCurOb
545
546 lda _rotx+1
547 cmp #$80
548 php
549 and #$7f
550 beq nadax
551 sta mancount
552 lda #0
553 sta _rotx+1
554
555 lda #0
556 plp
557 php
558 ror
559 ora mancount
560 sta g_alpha
561
562 loopx
563 plp
564 php
565 jsr Pitch
566 dec mancount
567 bne loopx
568 nadax
569 plp
570
571 lda _roty+1
572 cmp #$80
573 php
574 and #$7f
575 beq naday
576 sta mancount
577 lda #0
578 sta _roty+1
579
580 lda #0
581 plp
582 php
583 ror
584 ora mancount
585 sta g_beta
586
587 loopy
588 plp
589 php
590 jsr Yaw
591 dec mancount
592 bne loopy
593 naday
594 plp
595
596 lda _rotz+1
597 cmp #$80
598 php
599 and #$7f
600 beq nadaz
601 sta mancount
602 lda #0
603 sta _rotz+1
604
605 lda #0
606 plp
607 php
608 ror
609 ora mancount
610 sta g_delta
611
612 loopz
613 plp
614 php
615 jsr Roll
616 dec mancount
617 bne loopz
618 nadaz
619 plp
620
621 lda _accel+1
622 clc
623 adc _speed+1
624 bpl noneg
625 lda #0
626 beq nomax
627 noneg
628 cmp _p_maxspeed
629 bcc nomax
630 lda _p_maxspeed
631 nomax
632 sta _speed+1
633 sta g_theta
634 beq end
635
636 ; Move forwards actually moves 4 times the amount in A
637 ; Original Elite moves 3/2 (96*4/256) times this amount.
638 ; Let's see if halving down this is enough...
639 lsr
640 lsr
641 jsr MoveForwards
642
643 lda #0
644 sta _accel+1
645
646 end
647
648 ; Patch ACCROTX, etc.
649 lda #<ROTXY
650 sta patch_rot1+1
651 sta patch_rot2+1
652
653 lda #>ROTXY
654 sta patch_rot1+2
655 sta patch_rot2+2
656 +globalrts
657 rts
658
659 .)
660
661
662 maxspeed .byt 0
663
664 ; Needs reg A loaded with field ID of obj3D record
665 MoveCurrent
666 .(
667 and #%01111111
668 tax
669 beq globalrts ; This is a planet or a moon...
670
671 lda ShipMaxSpeed-1,x
672 sta maxspeed
673
674 ldx CUROBJ
675 lda _rotx,x
676 tay
677 and #%01111111
678 beq nadax
679 cpy #$80
680 jsr Pitch
681 nadax
682 ldx CUROBJ
683 lda _roty,x
684 tay
685 and #%01111111
686 beq naday
687 cpy #$80
688 jsr Yaw
689 naday
690 ldx CUROBJ
691 lda _rotz,x
692 tay
693 and #%01111111
694 beq nadaz
695 cpy #$80
696 jsr Roll
697 nadaz
698
699 forwards
700 ldx CUROBJ
701 lda _accel,x
702 beq move
703 clc
704 adc _speed,x
705 bpl noneg
706 lda #0
707 beq nomax ; allways branch
708 noneg
709 cmp maxspeed
710 bcc nomax
711 lda maxspeed
712 nomax
713 sta _speed,x
714 move
715 lda _speed,x
716 ; Move forwards actually moves 4 times the amount in A
717 ; Original Elite moves 3/2 (96*4/256) times this amount.
718 ; Let's see if halving down this is enough...
719
720 ; Speed should allways be possitive
721 ;cmp #$80
722 ;ror
723 ;cmp #$80
724 ;ror
725 lsr
726 lsr
727 jsr MoveForwards
728 end
729 rts
730 .)
731
732
733
734
735
736
737 ;; fly_to_vector
738 ;; function that implements
739 ;; the AI to make a ship follow a given vector
740 ;; The thing is calculating dot products for the
741 ;; three direction vectors of the ship and the objective
742 ;; vector and trying to make down and lateral values 0
743 ;; it fills the rotx, roty and rotz components for the ship
744 ;; data, as well as the acceleration, so MoveShip will perform
745 ;; rotations and advances as necessary.
746 ;;
747 ;; Convention for rotation values is that bit 7 set means negative
748 ;; BUT lower 7 bits are NOT 2-complemented.
749
750 #define ROTATE_AMOUNT 3; 5
751
752
753 ;; First some variables needed for all this...
754
755 ; int our_ang0;
756 ; int our_ang1;
757 ; int our_ang2;
758 ; int ang_lim;
759
760 ; The three results for the dot product
761 our_ang0 .word 00
762 our_ang1 .word 00
763 our_ang2 .word 00
764
765 ;; The angle limit to decide a ship should rotate
766 ;; It is compared with the high byte of angle
767 ang_lim .byt 00
768
769
770 _fly_to_vector
771 .(
772 jsr get_attack_angle
773 jmp fly_to_vector_final
774 .)
775
776
777
778 get_attack_angle
779 .(
780
781 ; norm_big();
782 ; GetFrontVector();
783 ; our_ang0=dot_product();
784
785 jsr _norm_big
786 jsr _GetFrontVector
787 jsr dot_product
788 lda op1+1
789 ldx op1
790 sta our_ang0+1
791 stx our_ang0
792
793 ; If very behind us, make sure we rotate...
794 ; if (our_ang0 < -0xa38)//-0x1700/2)
795 ; ang_lim=0;
796 ; else
797 ; ang_lim=3;
798 ; already in op1
799 ;sta op1+1
800 ;stx op1
801 lda #<(-$a38)
802 sta op2
803 lda #>(-$a38)
804 sta op2+1
805 jsr cmp16
806 bpl limnorm
807 lda #0
808 beq store ; allways jump
809 limnorm
810 lda #50 ; Force 0 for aggresive following... maybe when ANGRY?
811 store
812 sta ang_lim
813 rts
814 .)
815
816 fly_to_vector_final
817 .(
818 ; // Fly_to_vector_final
819
820 ; // NES addition
821 ;if (our_ang0 < -0xdc7){//-0x1000) {
822 ; rotx[curr_ship] |= 2;
823 ; if (our_ang0 < 0)
824 ; rotx[curr_ship] |= 0x80;
825 ; rotz[curr_ship] = 0;
826 ; return;
827 ;}
828
829 ;jmp nonessadd
830 lda our_ang0
831 sta op1
832 lda our_ang0+1
833 sta op1+1
834 lda #<(-$dc7)
835 sta op2
836 lda #>(-$dc7)
837 sta op2+1
838 jsr cmp16
839 bpl nonessadd
840
841 ldx CUROBJ;_curr_ship
842 lda _rotx,x
843 ora #7
844 sta _rotx,x
845 lda our_ang0+1
846 bpl noneg
847 lda _rotx,x
848 ora #$80
849 sta _rotx,x
850 noneg
851 lda #0
852 sta _roty,x
853 rts
854 nonessadd
855
856 ;; Now start with checks for angles.
857 ;; We want to make ang1 and ang2 0 and
858 ;; ang0 the max possible (64*64=4096 i.e. $1000)
859
860
861 ; GetDownVector();
862 ; our_ang1=dot_product();
863 jsr _GetDownVector
864 jsr dot_product
865 lda op1+1
866 ldx op1
867 stx our_ang1
868 sta our_ang1+1
869
870 ;rotz[curr_ship]=0;
871 ;if (abs(our_ang1)/255>=ang_lim) {
872 ; rotz[curr_ship]=ROTATE_AMOUNT;
873 ;}
874 ;if(our_ang1 >=0)
875 ; rotz[curr_ship] |= 0x80;
876
877 lda #0
878 ldx CUROBJ;_curr_ship
879 sta _roty,x
880
881 lda our_ang1
882 sta op1
883 lda our_ang1+1
884 sta op1+1
885 jsr abs
886
887 ldx CUROBJ ;_curr_ship
888 lda op1+1
889 bne rotatez
890 lda op1
891 cmp ang_lim
892 bcc nothingtodo
893 ; lda #4
894 rotatez
895 lda #ROTATE_AMOUNT
896 ;lsr
897 sta _roty,x
898 nothingtodo
899 lda our_ang1+1
900 bpl neg ; bmi neg ; BEWARE rotz seems to be interpreted differently so sign should be inverted!
901 lda _roty,x
902 ora #$80
903 sta _roty,x
904 neg
905
906
907 ; GetSideVector();
908 ; our_ang2=dot_product();
909
910 jsr _GetSideVector
911 jsr dot_product
912 lda op1+1
913 ldx op1
914 stx our_ang2
915 sta our_ang2+1
916
917 ; rotx[curr_ship]=0;
918 ; if( abs(our_ang2)/255 >=ang_lim){
919 ; rotx[curr_ship]=ROTATE_AMOUNT;
920 ; }
921
922 ; if(our_ang2>=0)
923 ; rotx[curr_ship]|=0x80;
924
925 lda #0
926 ldx CUROBJ;_curr_ship
927 sta _rotx,x
928
929 ; already in op1
930 ;lda our_ang2
931 ;sta op1
932 ;lda our_ang2+1
933 ;sta op1+1
934 jsr abs
935
936 ldx CUROBJ;_curr_ship
937 lda op1+1
938 bne rotatex
939 lda op1
940 cmp ang_lim
941 bcc nothingtodo2
942 lda #4
943 rotatex
944 ;lda #ROTATE_AMOUNT
945 lsr
946 sta _rotx,x
947 nothingtodo2
948 lda our_ang2+1
949 bmi neg2
950 lda _rotx,x
951 ora #$80
952 sta _rotx,x
953 neg2
954
955 ; Now the acceleration
956 ; This was my own addition, trying to avoid a ship
957 ; stopping, but should not be necessary...
958
959 ; if ((rotx[curr_ship] & 0x7f | roty[curr_ship] & 0x7f | rotz[curr_ship] & 0x7f)==0)
960 ; accel[curr_ship]=2;
961 ; else
962
963 #ifdef 0
964 lda _rotx,x
965 ora _roty,x
966 ora _rotz,x
967 and #$7f
968 bne cont
969 lda #2
970 sta _accel,x
971 rts
972 cont
973 #endif
974
975 ; // Check if dot product of front vector and destination
976 ; // is near the max. Max result is 64*64=4096 (0x1000).
977 ; // The original elite-agb used this max*0.63 and max*0.5
978 ; // for the bottom thresholds... so we can use 0x9c0 and 0x500..
979 ; //
980
981 ; // i.e. negative or small...
982 ; // for tactics this should be 1600
983
984 ; This means that if the angle of front vector is negative or small
985 ; we are in the wrong direction, so better stop... The elite-agb sources
986 ; state that for tactics this should be higher, but will add it later
987 ; {
988 ; if (our_ang0<0x9c0){ //0x9ff){//0x1600 ) { // this is 60%the max vector
989 ; our_ang0 = abs(our_ang0);
990 ; if (our_ang0 >= 0x500){//0x100){//0x1200) { // This is half the max vector
991 ; accel[curr_ship] = -1;
992 ; }
993 ; } else {
994 ; accel[curr_ship] = 2;
995 ; }
996 ; };
997 ;}
998 lda our_ang0
999 sta op1
1000 lda our_ang0+1
1001 sta op1+1
1002 lda #<$9c0
1003 sta op2
1004 lda #>$9c0
1005 sta op2+1
1006 jsr cmp16
1007 bpl notsmall
1008
1009 jsr abs
1010
1011 ;; Storing this is really necessary? Maybe for later functions...
1012 ;lda op1
1013 ;sta our_ang0
1014 ;lda op1+1
1015 ;sta our_ang0+1
1016
1017 lda #<$500
1018 sta op2
1019 lda #>$500
1020 sta op2+1
1021 jsr cmp16
1022 bmi notdec
1023
1024 ldx CUROBJ;_curr_ship
1025 lda #$ff
1026 sta _accel,x
1027
1028 notdec
1029 ;; That is all
1030 rts
1031 notsmall
1032 ldx CUROBJ;_curr_ship
1033 lda #2
1034 sta _accel,x
1035
1036 ;; And that is all folks
1037 ;lda #0
1038 ;sta _rotz,x
1039 rts
1040
1041 .)
1042
1043
1044
1045
1046
1047
1048
1049 ;; Normalize a vector
1050
1051
1052 ;void norm()
1053 ;{
1054 ; int x,y,z;
1055 ; int RQ,Q;
1056 ;
1057 ;
1058 ; x=VectX/256;
1059 ; y=VectY/256;
1060 ; z=VectZ/256;
1061 ;
1062 ; RQ=(x*x)+(y*y)+(z*z);
1063 ; Q=SqRoot(RQ);
1064 ;
1065 ; if (Q){
1066 ; VectX=(VectX)/Q*64;
1067 ; VectY=(VectY)/Q*64;
1068 ; VectZ=(VectZ)/Q*64;
1069 ; }
1070 ;
1071 ;}
1072
1073
1074 ;; Removes the sign and adds it back
1075 ;; for a 16-bit number stored in op1,op1+1
1076 ;; used in the above division...
1077
1078 unsgn
1079 .(
1080 lda op1+1
1081 sta resgn+1
1082 bpl end
1083 jmp abs
1084 end
1085 rts
1086 .)
1087
1088 resgn
1089 .(
1090 lda #00 ; SMC
1091 bpl end
1092 sec
1093 lda #0
1094 sbc op1
1095 sta op1
1096 lda #0
1097 sbc op1+1
1098 sta op1+1
1099 end
1100 rts
1101 .)
1102
1103 ;; Square of the contents of reg A. Accumulate over op1
1104 do_square
1105 .(
1106 beq end
1107 bpl cont
1108 eor #$ff
1109 ; Can't just add 1 as this is the HIGH byte
1110 ; so we may get an error of 1 unit. Who cares...
1111 +do_square_nosign
1112 beq end ; Just in case... -1 ($ff) becomes $00 !
1113 cont
1114 tay
1115 MULTAY(tmp)
1116 sta tmp+1
1117 clc
1118 lda tmp
1119 adc op1
1120 sta op1
1121 lda tmp+1
1122 adc op1+1
1123 sta op1+1
1124
1125 end
1126 rts
1127 .)
1128
1129 ;; Calculates the high byte of
1130 ;; the result of op2, op2+1 (a 16-bit signed number)
1131 ;; divided by Q and mutiplied by 64
1132 ;; op2/Q*64.
1133 ;; That is not the order in which it is done, or
1134 ;; floating arithmetic should be needed
1135 ;; (results of the division are between 0 and 1)
1136
1137 divQ
1138 .(
1139 lda #0 ; SMC
1140 sta op2
1141 lda #0
1142 sta op2+1
1143
1144 jsr unsgn
1145 jsr divu ;DIVXY
1146 ; X low resultado, A hi resultado
1147
1148 ; Multiplicar por 64 y quedarse con el byte alto
1149 ; En _VectX y poner el signo correcto en _VectX+1
1150 ; O sea coger el byte rotar a la dcha 2 veces y ponerlo
1151 ; Como byte alto. Luego arreglar el signo.
1152 stx tmp
1153
1154 lsr
1155 ror tmp
1156 lsr
1157 ror tmp
1158
1159 lda tmp
1160 sta op1+1
1161 lda #0
1162 sta op1
1163 jsr resgn
1164
1165 lda op1+1
1166 rts
1167 .)
1168
1169 ;; Normalize the vector, so it gets a total
1170 ;; length of 64 (more or less). It uses only
1171 ;; the higher bytes, so we must be sure of this
1172 ;; or call _norm_big.
1173
1174 norm
1175 .(
1176 lda #0
1177 sta op1
1178 sta op1+1
1179
1180 lda _VectX+1
1181 jsr do_square
1182 lda _VectY+1
1183 jsr do_square
1184 lda _VectZ+1
1185 jsr do_square
1186
1187 jsr SqRoot ; Resultado en op2, listo para usarse.
1188 lda op2
1189 beq end
1190
1191 sta divQ+1
1192
1193 ;; OJO ESTO HAY QUE TRATARLO CON SIGNO!!!
1194
1195 lda _VectX
1196 sta op1
1197 lda _VectX+1
1198 sta op1+1
1199 jsr divQ
1200 sta _VectX+1
1201 lda #0
1202 sta _VectX
1203
1204 lda _VectY
1205 sta op1
1206 lda _VectY+1
1207 sta op1+1
1208 jsr divQ
1209 sta _VectY+1
1210 lda #0
1211 sta _VectY
1212
1213 lda _VectZ
1214 sta op1
1215 lda _VectZ+1
1216 sta op1+1
1217 jsr divQ
1218 sta _VectZ+1
1219 lda #0
1220 sta _VectZ
1221 end
1222 rts
1223
1224 .)
1225
1226
1227
1228
1229 ;; Calculates the value of abs(VectX)|abs(Vecty)|abs(VectZ)
1230 ;; Stores result in op1 (16-bit)
1231 getnorm
1232 .(
1233
1234 lda _VectX
1235 sta op1
1236 lda _VectX+1
1237 sta op1+1
1238 jsr abs
1239
1240 lda op1
1241 sta tmp
1242 lda op1+1
1243 sta tmp+1
1244
1245 lda _VectY
1246 sta op1
1247 lda _VectY+1
1248 sta op1+1
1249 jsr abs
1250
1251 lda op1
1252 ora tmp
1253 sta tmp
1254 lda op1+1
1255 ora tmp+1
1256 sta tmp+1
1257
1258 lda _VectZ
1259 sta op1
1260 lda _VectZ+1
1261 sta op1+1
1262 jsr abs
1263
1264 lda op1
1265 ora tmp
1266 sta tmp
1267 lda op1+1
1268 ora tmp+1
1269 sta tmp+1
1270
1271 lda tmp
1272 sta op1
1273 lda tmp+1
1274 sta op1+1
1275
1276 rts
1277
1278 .)
1279
1280 ;; Normalize the vector to length 64, but
1281 ;; being sure it is large enough as to use higher
1282 ;; bytes...
1283
1284 _norm_big
1285 .(
1286 jsr getnorm ; Result in op1
1287
1288 ; This makes no sense, just some deffensive programming
1289 lda op1
1290 ora op1+1
1291 bne nozero
1292 inc op1
1293 nozero
1294
1295 lda #>$2000
1296 sta op2+1
1297 lda #<$2000
1298 sta op2
1299 jsr cmp16
1300 bcs finloop
1301
1302 lda #>$4000
1303 sta op2+1
1304 ; Low byte is already 00
1305
1306 loopw
1307 asl _VectX
1308 rol _VectX+1
1309 asl _VectY
1310 rol _VectY+1
1311 asl _VectZ
1312 rol _VectZ+1
1313 asl op1
1314 rol op1+1
1315
1316 jsr cmp16
1317 bcc loopw
1318
1319 finloop
1320
1321 lda _VectX+1
1322 cmp #$80
1323 ror _VectX+1
1324 ror _VectX
1325
1326 lda _VectY+1
1327 cmp #$80
1328 ror _VectY+1
1329 ror _VectY
1330
1331 lda _VectZ+1
1332 cmp #$80
1333 ror _VectZ+1
1334 ror _VectZ
1335
1336 jsr norm
1337
1338 lda _VectX+1
1339 sta _VectX
1340 sec
1341 bpl n1
1342 clc
1343 n1
1344 lda #0
1345 sbc #0
1346 sta _VectX+1
1347
1348 lda _VectY+1
1349 sta _VectY
1350 sec
1351 bpl n2
1352 clc
1353 n2
1354 lda #0
1355 sbc #0
1356 sta _VectY+1
1357
1358 lda _VectZ+1
1359 sta _VectZ
1360 sec
1361 bpl n3
1362 clc
1363 n3
1364 lda #0
1365 sbc #0
1366 sta _VectZ+1
1367
1368 end
1369 rts
1370
1371
1372 .)
1373
1374
1375 ;; Calculates the dot product between vectors specified in
1376 ;; <VX,VY,VZ> and <VectX,VectY,VectZ>, BUT considering both
1377 ;; of coordinates of 8-bit maximum, despite the fact that
1378 ;; VectX etc. are 16-bit, so they should be normalized first...
1379 ;; the operation is VX*VectX+VY*VectY+VZ*VectZ
1380
1381 ;; This helper calculates Vx*Vectx, being x=X,Y or Z and considering
1382 ;; them as being 8-bit. The component number is passed in reg X (0,1 or 2 for
1383 ;; X, Y or Z). Result is _added_ to op1,op1+1.
1384 #define savsign tmp
1385 dotpcomp
1386 .(
1387 ; First unsign each component
1388 lda #0
1389 sta savsign
1390
1391 lda _VX,x
1392 beq end ; zero result
1393 bpl nothing
1394 eor savsign
1395 sta savsign
1396 sec
1397 lda #0
1398 sbc _VX,x
1399 nothing
1400 tay ; Save a for multiplication
1401
1402 txa
1403 asl ; Vectx are 16-bit
1404 tax
1405 lda _VectX,x ; Get only lower byte (assumed high=0)
1406 beq end ; zero result
1407 bpl nothing2
1408 eor savsign
1409 sta savsign
1410 sec
1411 lda #0
1412 sbc _VectX,x
1413 nothing2
1414 MULTAY(op2)
1415 sta op2+1
1416
1417 ;; Resign result
1418 lda savsign
1419 bpl nothing3
1420 sec
1421 lda #0
1422 sbc op2
1423 sta op2
1424 lda #0
1425 sbc op2+1
1426 sta op2+1
1427
1428 nothing3
1429 ;; Add result to op1
1430 lda op1
1431 clc
1432 adc op2
1433 sta op1
1434 lda op1+1
1435 adc op2+1
1436 sta op1+1
1437 end
1438 rts
1439
1440
1441 .)
1442
1443 dot_product
1444 .(
1445 ; Prepare result. Initialize at 0
1446 lda #0
1447 sta op1
1448 sta op1+1
1449 ldx #0
1450 jsr dotpcomp
1451 ldx #1
1452 jsr dotpcomp
1453 ldx #2
1454 jmp dotpcomp ; This is jsr/rts
1455
1456 .)
1457
1458
1459
1460
1461
1462
1463
1464

  ViewVC Help
Powered by ViewVC 1.1.26