COMMENT ⊗   VALID 00004 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	EULER  - PROGRAMS FOR CONVERTING TRANSFORMS ↔ EULER ANGLES
C00003 00003	"EULER" COMPUTES THE EULER ANGLES DESCRIBING AN ARM POSITION GIVEN
C00006 00004	"UNEUL" COMPUTES A TRANSFORM MATRIX REPRESENTING HAND POSITION AND 
C00010 ENDMK
C⊗;
;EULER  - PROGRAMS FOR CONVERTING TRANSFORMS ↔ EULER ANGLES

.TITLE EULER

;DEFINITIONS

T21==4
T31==10
T12==20
T22==24
T32==30
T13==40
T23==44
T33==50
T14==60
T24==64
T34==70
T44==74

;CONSTANTS

C01:	.WORD	 36443,153412	; .100@-1 
TODEG:	.WORD	 41545, 27341	; 57.2958
;"EULER" COMPUTES THE EULER ANGLES DESCRIBING AN ARM POSITION GIVEN
;A TRANSFORM IN TABLE COORDINATES.  A SAMPLE CALLING SEQUENCE FOLLOWS:
;
;		MOV	#T,R0		;LOAD ADDR. OF TRANSFORM "T"
;		MOV	#E,R1		;LOAD ADDR. OF ARRAY TO 
;					;  RETURN THE EULER ANGLES
;		JSR	PC,EULER	;CALLED USING THE PC
;
;THE TRANSFORM "T" MUST BE STORED BY COLUMNS AND THE ARRAY "E" MUST
;BE AT LEAST 6 ELEMENTS LONG.  ALL NUMBERS SHOULD BE IN SINGLE
;PRECISION FLOATING POINT.

;EXECUTION TIME:

;REGISTERS USED:
;
;	R0, R1 PASS ARGUMENTS AND R1 IS ALTERED BY "EULER"
;	AC0,AC1,AC2,AC3,AC4 ARE GARBAGED

;START OF "EULER"

EULER:	MOV	T14(R0),(R1)+	;COPY X LOCATION INTO E(1)
	MOV	T14+2(R0),(R1)+
	MOV	T24(R0),(R1)+	;COPY Y LOCATION INTO E(2)
	MOV	T24+2(R0),(R1)+
	MOV	T34(R0),(R1)+	;COPY Z LOCATION INTO E(3)
	MOV	T34+2(R0),(R1)+
	LDF	T13(R0),AC0	;COMPUTE CO2←SQRT(T[1,3]↑2+T[2,3]↑2)
	MULF	AC0,AC0
	LDF	T23(R0),AC1
	MULF	AC1,AC1
	ADDF	AC1,AC0
	JSR	PC,SQRTF
	STF	AC0,AC4		;SAVE CO2 FOR LATER
	STF	AC0,AC1		;COMPUTE ATAN(-T[3,3]/CO2)
	LDF	T33(R0),AC0
	NEGF	AC0
	JSR	PC,ATAN2
	MULF	TODEG,AC0	;CONVERT TO DEGREES
	STF	AC0,4(R1)	;THIS IS E(5)
	LDF	AC4,AC0		;CHECK IF CO2 ALMOST ZERO
	CMPF	C01,AC0
	CFCC
	BLE	CO2BIG		;IF CO2<0.01
	LDF	T22(R0),AC0	;THEN E(4)←ATAN(T[2,2]/T[1,2])
	LDF	T12(R0),AC1
	JSR	PC,ATAN2
	MULF	TODEG,AC0	;CONVERT TO DEGREES AND STORE
	STF	AC0,(R1)
	CLR	10(R1)		;AND E(6)←0
	CLR	12(R1)
	RTS	PC		;RETURN
CO2BIG:	LDF	T13(R0),AC0	;ELSE E[4]←ATAN(T[1,3]/-T[2,3])
	LDF	T23(R0),AC1
	NEGF	AC1
	JSR	PC,ATAN2
	MULF	TODEG,AC0	;CONVERT TO DEGREES AND STORE
	STF	AC0,(R1)
	LDF	T32(R0),AC0	;AND E[6]←ATAN(T[3,2]/-[3,1])
	LDF	T31(R0),AC1
	NEGF	AC1
	JSR	PC,ATAN2
	MULF	TODEG,AC0	;CONVERT TO DEGREES AND STORE
	STF	AC0,10(R1)
	RTS	PC


;END OF "EULER"
;"UNEUL" COMPUTES A TRANSFORM MATRIX REPRESENTING HAND POSITION AND 
;ORIENTATION IN TABLE COORDINATES GIVEN THE EULER ANGLES OF THE 
;LOCATION.  A SAMPLE CALLING SEQUENCE FOLLOWS:
;
;		MOV	#T,R0		;LOAD ADDR. OF TRANSFORM "T"
;		MOV	#E,R1		;LOAD ADDR. OF EULER ANGLE ARRAY
;		JSR	PC,UNEUL	;CALLED USING THE PC
;
;THE TRANSFORM "T" WILL BE STORED BY COLUMNS AND THE ARRAY "E" MUST
;CONTAIN THE 6 EULER ANGLES.  ALL NUMBERS SHOULD BE IN SINGLE
;PRECISION FLOATING POINT.

;EXECUTION TIME:

;REGISTERS USED:
;
;	R0, R1 PASS ARGUMENTS AND R1 IS ALTERED BY "UNEUL"
;	AC0,AC1,AC2,AC3,AC4 ARE GARBAGED

;START OF "UNEUL"

UNEUL:	MOV	(R1)+,T14(R0)	;COPY X LOCATION INTO T[1,4]
	MOV	(R1)+,T14+2(R0)
	MOV	(R1)+,T24(R0)	;COPY Y LOCATION INTO T[2,4]
	MOV	(R1)+,T24+2(R0)
	MOV	(R1)+,T34(R0)	;COPY Z LOCATION INTO T[3,4]
	MOV	(R1)+,T34+2(R0)
	LDF	(R1)+,AC0	;COMPUTE THE SINE/COSINE OF E[4]
	JSR	PC,SNCOSD
	STF	AC0,SI1
	STF	AC1,CO1
	LDF	(R1)+,AC0	;COMPUTE THE SINE/COSINE OF E[5]
	JSR	PC,SNCOSD
	STF	AC0,SI2
	STF	AC1,CO2
	LDF	(R1),AC0	;COMPUTE THE SINE/COSINE OF E[6]
	JSR	PC,SNCOSD
	LDF	SI1,AC2		;T[1,1]←-SI1*SI2*CO3+CO1*SI3
	MULF	SI2,AC2
	STF	AC2,AC4		;SAVE SI1*SI2
	MULF	AC1,AC2
	LDF	CO1,AC3
	MULF	AC0,AC3
	SUBF	AC2,AC3
	STF	AC3,(R0)
	LDF	AC4,AC2		;T[1,2]←SI1*SI2*SI3+CO1*CO3
	MULF	AC0,AC2
	LDF	CO1,AC3
	MULF	AC1,AC3
	ADDF	AC3,AC2
	STF	AC2,T12(R0)
	MULF	SI2,AC3		;T[2,1]←CO1*CO3*SI2+SI1*SI3
	LDF	SI1,AC2
	MULF	AC0,AC2
	ADDF	AC3,AC2
	STF	AC2,T21(R0)
	LDF	CO1,AC2		;T[2,2]←-CO1*SI2*SI3+SI1*CO3
	MULF	SI2,AC2
	MULF	AC0,AC2
	LDF	SI1,AC3
	MULF	AC1,AC3
	SUBF	AC2,AC3
	STF	AC3,T22(R0)
	LDF	CO2,AC2		;T[1,3]←SI1*CO2
	LDF	SI1,AC3
	MULF	AC2,AC3
	STF	AC3,T13(R0)
	MULF	AC2,AC0		;T[3,2]←CO2*SI3
	STF	AC0,T32(R0)
	NEGF	AC2
	LDF	CO1,AC3		;T[2,3]←-CO1*CO2
	MULF	AC2,AC3
	STF	AC3,T23(R0)
	MULF	AC2,AC1		;T[3,1]←-CO2*CO3
	STF	AC1,T31(R0)
	LDF	SI2,AC0		;T[3,3]←-SI2
	NEGF	AC0
	STF	AC0,T33(R0)
	LDF	#40200,AC0	;T[4,4]←1.0
	STF	AC0,T44(R0)
	RTS	PC		;RETURN

SI1:	.WORD	0,0
CO1:	.WORD	0,0
SI2:	.WORD	0,0
CO2:	.WORD	0,0

;END OF "UNEUL"