;=====================================================================
;This file contains some very important mathematical MACROS. The code
;in these macros has been tested out and can be used for several
;different projects (not just CyberMorph). Indeed, some of the macros
;are long enough for subroutines. Can be made in-line or subroutines
;by use of the parameters.
;
;---------------------------------------------------------------------
;Macros contained herein:
;
;	DISTANCE2	:	calculate dist squared 2D coordinate pairs
;	DISTANCE	:	approximate distance as above (ROOT2 ?)
;	QUICKDIST	:	very quick and inaccurate distance
;	CALCANGLE	:	calculate 2D vector angle from North
;	ROOT3		:	approximate root of a*a+b*b+c*c
;
;	SCALARMULT	:	multiply by UNIT scalar (CyberMorph specific ?)
;
;=====================================================================


;---------------------------------------------------------------------
;DISTANCE2 (meaning distance squared) macro. Calculates the square of
;the shortest distance between 2 sets of coordinates, given that world
;wraps. Coordinate pairs are passed as parameters thus:
;
;				\1.w	= X1 (corrupted)
;				\2.w	= Z1 (corrupted)
;				\3.w	= X2
;				\4.w	= Z2
;				\1.l	= output reg for distance squared

DISTANCE2		macro
				sub.w \3,\1				;dX
				sub.w \4,\2				;dZ
				muls.w \1,\1			;dX * dX
				muls.w \2,\2			;dZ * dZ
				add.l \2,\1				;dX * dX + dZ * dZ
				endm

;
;---------------------------------------------------------------------

;---------------------------------------------------------------------
;DISTANCE macro. Uses root approximation to calculate distance. Could
;have implemented as a subroutine (still can). This method give more
;flexibility and possibly more speed. As above, the distance returned
;is the shortest between 2 coordinate pairs, given that the world
;in both directions (0-65535). Parameters as follows:
;
;				\1.w	= X1 (corrupted)
;				\2.w	= Z1 (corrupted)
;				\3.w	= X2
;				\4.w	= Z2
;				\5		= terminating instruction (rts / bra.s)
;				\6		= terminating instruction for \@2 (rts only)
;				\1.w	= output reg for distance
;
;To use in-line, call with:	DISTANCE d0,d1,d2,d3,<bra.s label>
;To gen subroutine, call with: DISTANCE d0,d1,d2,d3,<rts>,<rts>

DISTANCE		macro
				sub.w \3,\1
				bmi.b \@0
				sub.w \4,\2
				bmi.b \@1
				cmp.w \2,\1
				bcs.b \@2
				lsr.w #1,\2
				add.w \2,\1				;abs(\1-\3) + 0.5 * abs(\2-\4)
				\5

\@0:			neg.w \1				;absolute value required
				sub.w \4,\2
				bmi.b \@1
				cmp.w \2,\1
				bcs.b \@2

				lsr.w #1,\2
				add.w \2,\1				;abs(\1-\3) + 0.5 * abs(\2-\4)
				\5

\@1:			neg.w \2				;absolute value required
				cmp.w \2,\1
				bcs.b \@2

				lsr.w #1,\2
				add.w \2,\1				;abs(\1-\3) + 0.5 * abs(\2-\4)
				\5

\@2:			lsr.w #1,\1
				add.w \2,\1				;0.5 * abs(\1-\3) + abs(\2-\4)
				\6
				endm

;
;---------------------------------------------------------------------

;---------------------------------------------------------------------
;Very quick (and nasty ?) test for collision / distances in 2D. Used
;as a preliminary test before more complex and expensive tests are
;performed. Similar to above with the exception that it is slightly
;faster (probably not worth using in the end).
;
;				\1.w	= X1 (corrupted)
;				\2.w	= Z1 (corrupted)
;				\3.w	= X2
;				\4.w	= Z2
;				\5		= terminating instruction (rts / bra.s)
;				\6		= terminating instruction at end (rts only)
;				\1.w	= output reg for distance

QUICKDIST		macro
				sub.w \3,\1
				bmi.b \@0
				sub.w \4,\2
				bmi.b \@1
				add.w \2,\1				;abs(\1-\3) + abs(\2-\4)
				\5

\@0:			neg.w \1				;absolute value required
				sub.w \4,\2
				bmi.b \@1
				add.w \2,\1				;abs(\1-\3) + abs(\2-\4)
				\5

\@1:			neg.w \2				;absolute value required
				add.w \2,\1				;abs(\1-\3) + abs(\2-\4)
				\6
				endm

;
;---------------------------------------------------------------------

;---------------------------------------------------------------------
;Very large macro which calculates the angle of a vector (x2-x1,z2-z1)
;with respect to North. Uses approximation that for 0-45 degrees tan
;of angle is approximately linear. Hence, we break the vector angle
;into 45 degree areas (see below). Parameters very similar to DISTANCE
;
;                 Q8 | Q1
;                \   |	 /
;              Q7  \ | /   Q2
;             <------+------>
;              Q6  / | \   Q3
;                /   |   \
;				  Q5 | Q4
;
;				\1.w	= X2 (corrupted)
;				\2.w	= Z2 (corrupted)
;				\3.w	= X1
;				\4.w	= Z1
;				\5		= terminating instruction (rts / bra.s)
;				\6		= terminating instruction for \@2 (rts only)
;				\1.w	= angle (x2-x1,z2-z1)
;
;To use in-line, call with:	CALCANGLE d0,d1,d2,d3,<bra.s label>
;To gen subroutine, call with: CALCANGLE d0,d1,d2,d3,<rts>,<rts>

CALCANGLE		macro
				sub.w \3,\1
				bmi.b \@0
				sub.w \4,\2				;Q1, Q2, Q3 or Q4
				bmi.b \@1
				cmp.w \2,\1				;Q1 or Q2
				bcs.b \@2
				tst.w \1				;ensure no DIVU errors
				beq.b \@7
				ext.l \2				;\2 cannot be > $7fff here
				lsl.l #SHIFT45,\2		;Q2
				divu \1,\2
				move.w #ANGLE90,\1
				sub.w \2,\1				;angle is 90-theta
				\5

\@2:			tst.w \2				;ensure no DIVU errors
				beq.b \@7
				ext.l \1				;\1 cannot be > $7fff here
				lsl.l #SHIFT45,\1		;Q1
				divu \2,\1				;angle is theta
				\5

\@1:			neg.w \2				;Q3 or Q4
				cmp.w \2,\1
				bcs.b \@3
				tst.w \1				;ensure no DIVU errors
				beq.b \@7
				and.l #$0000ffff,\2		;cannot use EXT (\2 might be $8000)
				lsl.l #SHIFT45,\2		;Q3
				divu \1,\2
				move.w #ANGLE90,\1
				add.w \2,\1				;angle is 90+theta
				\5

\@3:			tst.w \2				;ensure no DIVU errors
				beq.b \@7
				ext.l \1				;\1 cannot be > $7fff here
				lsl.l #SHIFT45,\1		;Q4
				divu \2,\1
				neg.w \1
				add.w #ANGLE180,\1		;angle is 180-theta
				\5

\@7:			move.w #ANGLE0,\1		;x1==x2, z1==z2 (avoid DIVU errors)
				\5

\@0:			neg.w \1				;Q5, Q6, Q7 or Q8
				sub.w \4,\2
				bmi.b \@4
				cmp.w \2,\1				;Q7 or Q8
				bcs.b \@5
				tst.w \1				;ensure no DIVU errors
				beq.b \@7
				ext.l \2				;\2 cannot be > $7fff here
				lsl.l #SHIFT45,\2		;Q7
				divu \1,\2
				move.w #ANGLE270,\1
				add.w \2,\1				;angle is 270+theta
				\5

\@5:			tst.w \2				;ensure no DIVU errors
				beq.b \@7
				and.l #$0000ffff,\1		;cannot use EXT (\1 might be $8000)
				lsl.l #SHIFT45,\1		;Q8
				divu \2,\1
				neg.w \1
				add.w #ANGLE360,\1		;angle is 360-theta
				\5

\@4:			neg.w \2				;Q5 or Q6
				cmp.w \2,\1
				bcs.b \@6
				tst.w \1				;ensure no DIVU errors
				beq.b \@7
				and.l #$0000ffff,\2		;cannot use EXT (\2 might be $8000)
				lsl.l #SHIFT45,\2		;Q6
				divu \1,\2
				move.w #ANGLE270,\1
				sub.w \2,\1				;angle is 270-theta
				\5

\@6:			tst.w \2				;ensure no DIVU errors
				beq.b \@7
				and.l #$0000ffff,\1		;cannot use EXT (\1 might be $8000)
				lsl.l #SHIFT45,\1		;Q5
				divu \2,\1
				add.w #ANGLE180,\1		;angle is 180+theta
				\6
				endm

;
;---------------------------------------------------------------------

;---------------------------------------------------------------------
;Calculate the approximate root of a*a+b*b+c*c. Used for generating
;unit normals and possibly distances in 3D. This code, unlike its 2D
;equivalent DISTANCE, is NOT optimised for speed (optimised for code
;size). Approximation is in Graphic Gems.
;
;				\1.w	= A
;				\2.w	= B
;				\3.w	= C
;				\4.w	= |A|
;				\5.w	= |B|
;				\6.w	= |C|
;				\7.w	= result (\7.l used for calcs)
;				\8		= terminating instruction (rts ?)
;
;\7 can be the same as \1, \2 or \3 (but not \4, \5 or \6)

ROOT3			macro
				move.w \1,\4
				bpl.b \@0
				neg.w \4				;need modulus
\@0:			move.w \2,\5
				bpl.b \@1
				neg.w \5				;need modulus
\@1:			move.w \3,\6
				bpl.b \@2
				neg.w \6				;need modulus

\@2:			cmp.w \5,\6
				bcs.b \@3
				exg \5,\6
\@3:			cmp.w \4,\5				;\5 contains larger of \5 and \6
				bcs.b \@4
				exg \4,\5
\@4:			cmp.w \5,\6				;\4 contains larger of \4, \5 and \6
				bcs.b \@5
				exg \5,\6

\@5:			move.w \5,\7			;need 11/32 of middle value
				add.w \5,\5
				add.w \7,\5				;*3 (hence max is 21845)
				lsr #3,\5				;3/8
				add.w \5,\7				;1+3/8
				add.w \6,\7
				lsr #2,\7
				add.w \4,\7				;max + (med*11)/32 + min/4
				\8
				endm

;
;---------------------------------------------------------------------

;---------------------------------------------------------------------
;Multiply by the UNIT scalar. Used by various routines. Much quicker
;than using the 680x0 multiply instructions. Is this product specific?

SCALARMULT		macro
				swap \1
				sub.w \1,\1
				asr.l #4,\1				;signed multiply by 4096
				endm

;
;---------------------------------------------------------------------
ÿ
