812fd23adcc4bfa378baa701728f35ee92f9b496
[h316.git] / lib / hachti / src / matrix.asm
1 * MATRIX - AFFINE TRANSFORM SUPPORT PACKAGE
2 *
3 *
4 * AUTHOR:
5 *
6 * PHILIPP HACHTMANN
7 *
8 * VERSIONS:
9 * 0.1 - INITIAL REVISION (22.12.2007
10 *
11 *
12 * PURPOSE:
13 *
14 * THIS LIBRARY PROVIDES AFFINE TRANSFORMATION ROUTINES TO
15 * PLOTTING ROUTINES AND OTHER SOFTWARE.
16 *
17 *
18 * DATA REPRESENTATION:
19 *
20 *
21 * MATRIX FORMAT: | A11 A12 |
22 * M= | |
23 * | A21 A22 |
24 *
25 * A11-A22 ARE SINGLE PRECISION FLOAT VALUES COMPLIANT TO THE
26 * HONEYWELL MATHEMATICAL LIBARAY. EVERY VALUE USES TWO
27 * 16 BIT MACHINE WORDS.
28 * IF A MATRIX IS USED AS A DAC ARGUMENT, A POINTER TO THE FIRST
29 * ELEMENT, A11, HAS TO BE USED.
30 *
31 *
32 * VECTOR FORMAT: | A1 |
33 * V= | |
34 * | A2 |
35 *
36 * A1 AND A2 ARE SIGNED INTEGER VALUES. EVERY VALUE USES ONE
37 * 16 BIT MACHINE WORD.
38 * IF A VECTOR IS USED AS A DAC ARGUMENT, A POINTER TO THE FIRST
39 * ELEMENT, A1, HAS TO BE USED.
40 *
41 *
42 * AFFINE TRANSFORM FORMAT:
43 *
44 * AN AFFINE TRANSFORM CONSISTS OF A MATRIX FOR ROTATING AND SCALING
45 * AND A VECTOR VOR RELOCATION OF THE RESULT.
46 * A VECTOR IS TRANSFORMED BY FIRST MULTIPLYING THE MATRIX WITH IT
47 * AND THEN ADDING THE RELOCATION VECTOR:
48 *
49 * | A11 A12 | | VI1 | | VT1 |
50 * VO = MT * VI + VT = | | * | | + | |
51 * | A21 A22 | | VI2 | | VT2 |
52 *
53 * | VI1*A11 + VI2*A12 + VT1 |
54 * = | |
55 * | VI1*A21 + VI2*A22 + VT2 |
56 *
57 * MT AND VT ARE THE TRANSFORMATION MATRIX AND VECTOR, VI THE INPUT
58 * VECTOR, VO THE RESULT VECTOR.
59 *
60 * AN AFFINE TRANSFORM IS STORED AS A CONCATENATION OF A MATRIX AND
61 * A VECTOR. HERE IS THE MEMORY LAYOUT:
62 *
63 * '000 : MT11 UPPER
64 * '001 : MT11 LOWER
65 * '002 : MT12 UPPER
66 * '003 : MT12 LOWER
67 * '004 : MT21 UPPER
68 * '005 : MT21 LOWER
69 * '006 : MT22 UPPER
70 * '007 : MT22 LOWER
71 * '010 : VT1
72 * '011 : VT2
73 *
74 * FOR EVERY TRANSFORMATION, '12 WORDS HAVE TO BE RESERVED.
75 * IN AN APPLICATION, A TRANFORMATION VARIABLE COULD BE
76 * DECLARED WITH:
77 *
78 * TRANS BSS '12
79 *
80 *
81 *
82 *********************************************************************************
83 *
84 *
85 *
86 * M$INIT: INITIALISE MATRIX TO IDENTITY
87 *
88 * THE MATRIX ARGUMENT IS SET TO
89 *
90 * | 1.0 0.0 |
91 * M= | |
92 * | 0.0 1.0 |
93 *
94 * WICH RESULTS TO THE IDENTITY TRANSFORMATION.
95 *
96 * JST M$INIT
97 * DAC MATRIX POINTER TO A MATRIX
98 *
99 *
100 *
101 * M$MUL: MATRIX MULTIPLICATION
102 *
103 * JST M$MUL
104 * DAC TARGET POINTER TO TARGET MATRIX
105 * DAC MATRIX1 POINTER TO LEFT MATRIX
106 * DAC MATRIX2 POINTER TO RIGHT MATRIX
107 * DAC 0 FOR FORTRAN IV COMPATIBILITY
108 *
109 *
110 *
111 * M$APLI: APPLY MATRIX TO VECTOR
112 *
113 * THIS ROUTINE CONVERTS THE VECTOR ELEMENTS TO FLOATING POINT VALUES,
114 * APPLIES THE TRANSFORMATION TO THEM AND ROUNDS THE RESULTS BACK TO
115 * INTEGER VALUES. THEN IT SAVES THE NEW VECTOR IN THE PLACE OF THE
116 * OLD VECTOR.
117 *
118 * JST M$APLI
119 * DAC MATRIX MATRIX TO APPLY
120 * DAC VECTOR VECTOR TO TRANSFORM
121 * DAC 0 FOR FORTRAN IV COMPATIBILITY
122 *
123 *
124 * M$APII: APPLY MATRIX TO PAIR OF INTEGERS AS VECTOR
125 *
126 * THIS ROUTINE USES TWO DISTINCT INTEGER POINTERS INSTEAD OF ONE VECTOR
127 * POINTER. THE REST OF THE BEHAVIOR IS EXACTLY LIKE M$APL.
128 *
129 * JST M$APLI
130 * DAC MATRIX MATRIX TO APPLY
131 * DAC X X COORDINATE OF ARGUMENT VECTOR
132 * DAC Y Y COORDINATE OF ARGUMENT VECTOR
133 *
134 *
135 * M$ROT: ROTATE MATRIX
136 *
137 * THIS ROUTINE TAKES A MATRIX AND ADDS A ROTATION TO IT.
138 * INTERNALLY, THE ROUTINE CREATES A ROTATION MATRIX AND THEN
139 * MULTIPLIES IT WITH THE ARGUMENT MATRIX. THE ROTATION IS SPECIFIED
140 * COUNTERCLOCKWISE FORWARD, ANGLE IN RADIANT.
141 * THE ANGLE ARGUMENT IS A SINGLE PRECISION FLOATING POINT NUMER
142 * TAKING TWO WORDS TO STORE.
143 *
144 * JST M$ROT
145 * DAC MATRIX MATRIX TO MODIFY
146 * DAC ANGLE RADIANT ANGLE
147 * DAC 0 FOR FORTRAN IV COMPATIBILITY
148 *
149 * M$SCLE: SCALE MATRIX
150 *
151 * THIS ROUTINE WORKS SIMILAR TO M$ROT BUT SCALES THE ARGUMENT MATRIX.
152 * THE SCALE FACTOR IS A FLOATING POINT NUMBER. LIKE THE ROTATION ANGLE.
153 *
154 * JST M$SCLE
155 * DAC MATRIX MATRIX TO MODIFY
156 * DAC SCALE SCALE FACTOR
157 * DAC 0 FOR FORTRAN IV COMPATIBILITY
158 *
159 *
160 ********************************************************************************
161 *
162 **** EXPORTED SYMBOLS
163 *
164 SUBR MATRIX,INIT JUST A FANCY LABEL
165 SUBR M$INIT,INIT INITIALISE MATRIX
166 SUBR M$MUL,MUL MATRIX MULTIPLICATION
167 SUBR M$APLI,APLI APPLY MATRIX TO INTEGER VECTOR
168 SUBR M$APII,APII APPLY MATRIX TO PAIR OF INTEGERS
169 SUBR M$ROT,ROT ADD ROTATION TO MATRIX
170 SUBR M$SCLE,SCLE SCALE MATRIX
171 *
172 SUBR A$INIT,AFIN INITIALISE AFFINE TRANSFORMATION
173 *
174 *
175 ********************************************************************************
176 *
177 *
178 REL RELOCATEABLE MODE
179 *
180 *
181 ********************************************************************************
182 *
183 *
184 **** INITIALIZE AFFINE TRANSFORMATION
185 *
186 AFIN DAC **
187 LDA* AFIN
188 STA AFI1 STORE ARGUMENT POINTER
189 LDX AFIN LOAD INTO INDEX REGISTER, TOO
190 IRS AFIN TALLY RETURN ADDRESS
191 *
192 JST INIT MATRIX INIT
193 AFI1 DAC **
194 *
195 CRA
196 STA 8,1 CLEAR FIRST VECTOR ELEMENT
197 STA 9,1 CLEAR SECOND VECTOR ELEMENT
198 *
199 JMP* AFIN RETURN TO CALLER
200 *
201 *
202 ********************************************************************************
203 *
204 *
205 **** INITIALIZE MATRIX
206 *
207 * THIS ROUTINE SHOULD BE IMPROVED BY SUPPLYING
208 * A FLOATING POINT 1.0 CONSTANT!
209 *
210 ****************************************
211 *
212 INIT DAC **
213 LDX* INIT LOAD INDEX REGISTER WITH ADDRESS OF MATRIX
214 LDA* INIT LOAD MATRIX ADDRESS
215 STA IM11 STORE POINTER TO FIRST ELEMENT (A11)
216 ADD =6 IM12,IM21 ARE NOT TO BE INITIALISED WITH FP DATA
217 STA IM22 STORE POINTER TO FOURTH ELEMENT (A22)
218 IRS INIT CORRECT RETURN ADDRESS
219 *
220 CRA INITIALISE
221 STA 2,1 A12
222 STA 3,1
223 STA 4,1 A21
224 STA 5,1
225 CALL FLOAT GENERATE FLOATING POINT 1.0
226 DAC ONE CONSTANT INTEGER 1
227 CALL H$22 STORE FLOATING POINT
228 IM11 DEC 0
229 CALL H$22
230 IM22 DEC 0
231 *
232 JMP* INIT RETURN.
233 *
234 *
235 ********************************************************************************
236 *
237 *
238 **** MATRIX MULTIPLICATION
239 *
240 * C = A * B
241 *
242 * | a11 a12 | | b11 b12 |
243 * = | | * | |
244 * | a21 a22 | | b21 b22 |
245 *
246 * | (a11*b11) (a21*b12) |
247 * = | |
248 * | (a12*b21) (a22*b22) |
249 *
250 * CALL:
251 * JST MUL
252 * DAC MC
253 * DAC MA
254 * DAC MB
255 *
256 ****************************************
257 *
258 MUL DAC **
259 LDX* MUL
260 *
261 LDA* MUL
262 STA PC11
263 ADD =2
264 STA PC12
265 ADD =2
266 STA PC21
267 ADD =2
268 STA PC22
269 IRS MUL
270 *
271 LDA* MUL
272 STA PA11
273 ADD =2
274 STA PA12
275 ADD =2
276 STA PA21
277 ADD =2
278 STA PA22
279 ADD =2
280 *
281 IRS MUL
282 *
283 LDA* MUL
284 STA PB11
285 ADD =2
286 STA PB12
287 ADD =2
288 STA PB21
289 ADD =2
290 STA PB22
291 ADD =2
292 *
293 IRS MUL
294 IRS MUL
295 *
296 * a11 a12 b11 b12 a11*b11 a21*b12
297 * a21 a22 b21 b22 a12*b21 a22*b22
298 *
299 CALL L$22 LOAD REAL
300 PA11 DAC 0
301 CALL M$22 MULTIPLY
302 PB11 DAC 0
303 CALL H$22 STORE
304 PC11 DEC 0
305 *
306 CALL L$22
307 PA21 DEC 0
308 CALL M$22
309 PB12 DEC 0
310 CALL H$22
311 PC12 DEC 0
312 *
313 CALL L$22
314 PA12 DEC 0
315 CALL M$22
316 PB21 DEC 0
317 CALL H$22
318 PC21 DEC 0
319 *
320 CALL L$22
321 PA22 DEC 0
322 CALL M$22
323 PB22 DEC 0
324 CALL H$22
325 PC22 DEC 0
326 *
327 *
328 JMP* MUL RETURN.
329 *
330 *
331 ********************************************************************************
332 *
333 *
334 **** SCALE MATRIX
335 *
336 SCLE DAC ** SCALE MATRIX
337 LDX* SCLE
338 *
339 LDA* SCLE GET MATRIX BASE ADDRESS
340 STA SM11
341 STA TM11
342 ADD =6
343 STA SM22
344 STA TM22
345 IRS SCLE
346 LDA* SCLE
347 STA SX
348 STA SY
349 IRS SCLE TALLY RETURN ADDRESS
350 IRS SCLE AGAIN
351 *
352 CALL L$22
353 SM11 DAC 0
354 CALL M$22
355 SX DAC 0
356 CALL H$22
357 TM11 DAC 0
358 *
359 CALL L$22
360 SM22 DAC 0
361 CALL M$22
362 SY DAC 0
363 CALL H$22
364 TM22 DAC 0
365 *
366 JMP* SCLE
367 *
368 *
369 ********************************************************************************
370 *
371 *
372 **** ADD ROTATION TO MATRIX
373 *
374 *
375 * M = M * MROT
376 *
377 * | M11 M12 | | COS(X) -SIN(X)|
378 * = | | * | |
379 * | M21 M22 | | SIN( X) COS(X)|
380 *
381 * | M11*COS(X)+M12*SIN(X) M12*COS(X)-M11*SIN(X) |
382 * = | |
383 * | M21*COS(X)+M22*SIN(X) M22*COS(X)-M21*SIN(X) |
384 *
385 * CALL:
386 * JST ROT
387 * DAC MATRIX
388 * DAC ANGLE
389 * DAC 0 DON'T FORGET!
390 *
391 ****************************************
392 *
393 ROT DAC ** ENTRY
394 *
395 LDA* ROT GET MATRIX POINTER
396 STA R111 M11, FIRST COPY
397 STA R211 M11, SECOND COPY
398 STA R311 M11, THIRD COPY
399 ADD =2
400 STA R112
401 STA R212
402 STA R312
403 STA R412
404 ADD =2
405 STA R121
406 STA R221
407 STA R321
408 ADD =2
409 STA R122
410 STA R222
411 STA R322
412 IRS ROT
413 LDA* ROT
414 STA RA1
415 STA RA2
416 IRS ROT
417 IRS ROT
418 *
419 *
420 **** M11 CALCULATION
421 *
422 CALL SIN FLOATING POINT SINE
423 RA1 DAC ** POINTER TO ANGLE
424 CALL H$22 SAVE TO TMP1
425 DAC TMP1
426 * CALL L$22
427 * DAC TMP1
428 CALL M$22 MULTIPLY
429 R112 DAC ** M12
430 CALL H$22 STORE TO TMP3
431 DAC TMP3
432 *
433 *************************************
434 *
435 CALL COS FLOATING POINT COSINE
436 RA2 DAC ** POINTER TO ANGLE
437 CALL H$22 SAVE TO TMP2
438 DAC TMP2
439 CALL M$22 MULTIPLY
440 R111 DAC ** M11
441 CALL A$22 ADD TMP3
442 DAC TMP3
443 CALL H$22 SAVE NEW M11 TO TMP3
444 DAC TMP3
445 *
446 *
447 * M12 CALCULATION
448 *
449 * M12 = M12*COS(X)-M11*SIN(X)
450 *
451 *
452 CALL L$22 LOAD SINE
453 DAC TMP1
454 CALL M$22 MULTIPLY
455 R211 DAC ** M11
456 CALL H$22 STORE TO TMP4
457 DAC TMP4
458 CALL L$22 LOAD COSINE
459 DAC TMP2
460 CALL M$22 MULTIPLY
461 R212 DAC **
462 *
463 CALL S$22 SUBSTRACT !!
464 DAC TMP4
465 *
466 CALL H$22 SAVE TO NEW M12
467 R312 DAC **
468 *
469 CALL L$22 LOAD NEW M11 FROM TMP3
470 DAC TMP3
471 CALL H$22 AND SAVE TO NEW M11
472 R311 DAC **
473 *
474 *
475 * ******************************************
476 *
477 * M21 CALCULATION
478 *
479 * M21*COS(X)+M22*SIN(X)
480 *
481 * M22*SIN(X) -> TMP3
482 * M21*COS(X) - TMP3
483 *
484 *
485 *
486 CALL L$22 LOAD SINE
487 DAC TMP1
488 CALL M$22 MULTIPLY
489 R122 DAC ** M22
490 CALL H$22 STORE TO TMP3
491 DAC TMP3
492 CALL L$22 LOAD COSINE
493 DAC TMP2
494 CALL M$22 MULTIPLY
495 R121 DAC ** M11
496 CALL A$22 ADD TMP3
497 DAC TMP3
498 CALL H$22 SAVE NEW M21 TO TMP3
499 DAC TMP3
500 *
501 * M22 CALCULATION
502 *
503 * M22*COS(X)-M21*SIN(X)
504 *
505 *
506 * JMP NN
507 CALL L$22 LOAD SINE
508 DAC TMP1
509 CALL M$22 MULTIPLY
510 R221 DAC ** M21
511 CALL H$22 STORE TO TMP4
512 DAC TMP4
513 CALL L$22 LOAD COSINE
514 DAC TMP2
515 CALL M$22 MULTIPLY
516 R222 DAC **
517 CALL S$22 SUBSTRACT
518 DAC TMP4
519 CALL H$22 SAVE TO NEW M22
520 R322 DAC **
521 CALL L$22 LOAD NEW M21 FROM TMP3
522 DAC TMP3
523 CALL H$22 AND SAVE TO NEW M21
524 R321 DAC **
525 *
526 *
527 JMP* ROT RETURN.
528 *
529 R412 DAC **
530 *
531 ********************************************************************************
532 *
533 *
534 **** APPLY MATRIX TO PAIR OF INTEGERS
535 *
536 * SETS UP MATRIX POINTERS AND VECTOR POINTERS.
537 * THEN IT CALLS APL, THE REAL WORKING ROUTINE.
538 *
539 * CALL:
540 * JST M$APII
541 * DAC MATRIX
542 * DAC X
543 * DAC Y
544 * DAC 0 DON'T FORGET!
545 *
546 ****************************************
547 *
548 APII DAC **
549 *
550 LDA* APII POINTER TO MATRIX
551 STA MP11 STORE M11
552 ADD =2 ADD 2
553 STA MP12 STORE M12
554 ADD =2 ADD 2
555 STA MP21 STORE M21
556 ADD =2 ADD 2
557 STA MP22 STORE M22
558 IRS APII JUMP TO NEXT ARGUMENT (X)
559 *
560 LDA* APII LOAD X VALUE
561 STA XP1 STORE TO X-POINTER
562 STA XP2 STORE TO X-POINTER
563 IRS APII JUMP TO NEXT ARGUMENT (Y)
564 LDA* APII LOAD Y VALUE
565 STA YP1 STORE TO Y-POINTER
566 STA YP2 STORE TO Y-POINTER
567 IRS APII CORRECT RETURN ADDRESS
568 IRS APII FOR FORTRANIV COMPATIBILITY
569 JST APL CALL REAL ROUTINE
570 JMP* APII
571 *
572 ********************************************************************************
573 *
574 *
575 **** APPLY MATRIX TO VECTOR
576 *
577 * SETS UP MATRIX POINTERS AND VECTOR POINTERS. THEN IT CALLS APL,
578 * THE REAL WORKING ROUTINE.
579 *
580 * CALL:
581 * JST M$APLI
582 * DAC MATRIX
583 * DAC VECTOR
584 * DAC 0 DON'T FORGET!
585 *
586 ****************************************
587 *
588 APLI DAC **
589 *
590 LDA* APLI
591 STA MP11
592 ADD =2
593 STA MP12
594 ADD =2
595 STA MP21
596 ADD =2
597 STA MP22
598 IRS APLI
599 *
600 LDA* APLI
601 STA XP1
602 STA XP2
603 AOA
604 STA YP1
605 STA YP2
606 IRS APLI
607 IRS APLI
608 JST APL CALL INTERNAL ROUTINE
609 JMP* APLI RETURN.
610 *
611 *
612 ********************************************************************************
613 *
614 *
615 **** INTERNAL ROUTINE OF M$APL AND M$APII.
616 *
617 * ALL DATA IS SET UP BY THE BOTH USER ROUTINES ABOVE.
618 *
619 ****************************************
620 *
621 APL DAC **
622 *
623 CALL FLOAT LOAD SINGLE PRECISION FLOAT FROM 1-WORD INTEGER
624 XP1 DAC 0
625 CALL M$22 MULTIPLY FLOAT*FLOAT
626 MP11 DAC 0
627 CALL H$22 STORE FLOAT
628 DAC TMP1
629 CALL FLOAT
630 YP1 DAC 0
631 CALL M$22
632 MP12 DAC 0
633 CALL A$22
634 DAC TMP1
635 JST RND ROUND AND CONVERT TO INTEGER
636 STA PA21 STORE NEW X VALUE INTO TEMPORARY LOCATION
637 ****
638 CALL FLOAT
639 XP2 DAC 0
640 CALL M$22
641 MP21 DAC 0
642 CALL H$22
643 DAC TMP1
644 *
645 CALL FLOAT
646 YP2 DAC 0
647 CALL M$22
648 MP22 DAC 0
649 CALL A$22
650 DAC TMP1
651 JST RND NOW INTEGER IN AC
652 STA* YP1 STORE NEW Y VALUE
653 *
654 LDA PA21
655 STA* XP1
656 JMP* APL RETURN TO CALLER.
657 *
658 *
659 ********************************************************************************
660 *
661 *
662 **** ROUND FLOAT TO INTEGER ROUTINE
663 *
664 * THERE IS NO CORRECTLY WORKING ROUNDING ROUTINE IN THE LIBRARY.
665 * SO THIS IS A WORKAROUND. ADDS 0.5 TO THE VALUE AND USES ONE
666 * ONE OF THE TRUNCATE AND CONVERT ROUTINES.
667 * THE ARGUMENT IS IN REGISTERS A/B, THE RESULT IS PUT INTO A.
668 *
669 ****************************************
670 *
671 RND DAC **
672 CALL A$22 ADD
673 DAC HLF 0.5
674 CALL C$21 TRUNCATE TO INTEGER
675 NOP
676 JMP* RND
677 *
678 *
679 ********************************************************************************
680 *
681 *
682 **** CONSTANTS
683 *
684 ONE DEC 1
685 HLF OCT '040100 CONSTANT 0.5
686 OCT '000000
687 *
688 *
689 ********************************************************************************
690 *
691 **** VARIABLES
692 *
693 TMP1 BSS '2 TEMPORARY 2-WORD VARIABLE
694 TMP2 BSS '2 " " "
695 TMP3 BSS '2 " " "
696 TMP4 BSS '2 " " "
697 *
698 *
699 ********************************************************************************
700 *
701 *
702 **** END OF THE LINE
703 *
704 END
705 *
706 *
707 ********************************************************************************