( ------------------------------------------------
( FILE:        coords.rpl
( DESCRITION:  definitions for Coordsys operation
( PLATFORM:    AMIGA
( VERSION:     1.1
( DATE:        28-1-95  -> 10-5-95
( ------------------------------------------------
( 1995 Alessandro Tasora, Italy
( ------------------------------------------------


?& COORDS.RPL NOT ?IF
17 CONSTANT COORDS.RPL

8  CONSTANT FSIZE
24 CONSTANT VSIZE

VVARIABLE tempx
VVARIABLE tempy
VVARIABLE tempz   ( these vectorial var. will be  used later 


( alloc 3 x VTYPE and leave address on the stack,
( Coordsys type is a 3-dimensional, 2nd-order tensor 

: CVARIABLE
    <BUILDS VSIZE 3 * ALLOT DOES>
;

( store 3 vectors in CVARIABLE )
VARIABLE tempv

: CSTORE ( vVal vVal vVal aVar CSTORE )
    tempv !
    tempv @ VSIZE 2 * + V!
    tempv @ VSIZE + V!
    tempv @ V!
;

: C! CSTORE ;

( fetch 3 vectors in CVARIABLE )

: CFETCH ( aVar CFETCH vVal vVal vVal )
    tempv !
    tempv @ V@
    tempv @ VSIZE + V@
    tempv @ VSIZE 2 * + V@
;

: C@ CFETCH ;

( print coordsys tensor )

: C.     ( cVal C. - )
    9 ROLL 9 ROLL 9 ROLL V.   10 EMIT 13 EMIT
    6 ROLL 6 ROLL 6 ROLL V.   10 EMIT 13 EMIT
    V.
;

( duplicates cCoordsys on stack )

: CDUP   ( cCoor CDUP cCoor cCoor )
    9 PICK 9 PICK 9 PICK
    9 PICK 9 PICK 9 PICK
    9 PICK 9 PICK 9 PICK
;


( swap a coordsys and a vector-on stack top- )

: CVSWAP    ( cCoord, vVect  CVSWAP vVect, cCoord )
    12 ROLL 12 ROLL 12 ROLL
    12 ROLL 12 ROLL 12 ROLL
    12 ROLL 12 ROLL 12 ROLL
;

( swap a vector and a coordsys-on stack top- )  

: VCSWAP    ( vVect, cCoord  VCSWAP cCoord, vVect )
    12 ROLL 12 ROLL 12 ROLL
;   


( rotate coordsys with 3 Descartes' angles X Y Z )

: CROTATE   ( fXangle fYangle fZangle aCoordsys ) 
   ROT_COORD
;

( transposes a 3 x 3 matrix-coordsys on stack )

: CTRASP    ( cMatrix CTRASP cMatrixT )

 9 ROLL  7 ROLL 5 ROLL
 9 ROLL  8 ROLL 7 ROLL
 9 ROLL  9 ROLL 9 ROLL
;

( multiplies a 3x3 matrix -a coordsys- per a vector: results a vector )

: CMULV     ( cMatrix vVector CMULV vVectResult )

  3 PICK  13 ROLL  F*
  3 PICK  13 ROLL  F*  F+
  2 PICK  12 ROLL  F*  F+   ( x comp

  4 PICK  11 ROLL  F*
  4 PICK  11 ROLL  F*  F+
  3 PICK  10 ROLL  F*  F+   ( y comp

  5 ROLL   8 ROLL  F*
  5 ROLL   7 ROLL  F*  F+
  4 ROLL   5 ROLL  F*  F+   ( z comp
;


( TO_COORDS, a procedure to transform coords
( input  :  a Vector in absolute coords, a NewCoordsys, the Origin
(            of the new coordsys
( output :  the vector in the new coord. system
( Note that a "cNewCoordsys" is a sequence of three vectors DDIR, DDIV, DNOR 

: TO_COORDS   ( cNewCoordsys, vVector, vOrigin TO_COORDS vVector2 )

   VSUB   ( absolute coords. with the same origin of new coordsys )
   CMULV  ( matrix coordsys transformation )
;



( TO_ABS_COORDS, 
( input  :  a Vector in NewCoordsys, the NewCoordsys, the Origin
(            of the new coordsys
( output :  the vector back in the absolute coord. system 

: TO_ABS_COORDS   ( vOrigin vVector2 cNewCoordsys TO_ABS_COORDS vVector )

   CTRASP                   ( matrix transposition )
   12 ROLL 12 ROLL 12 ROLL  ( vector on top stack )
   CMULV                    ( matrix coordsys transformation )
   VADD
;

( creates a coordsys starting from its DDIR vector,
( setting the DNOR -z axis- as horizontal 

: VCREAC  ( vVectorDir VCREAC cCoordsys )
   DUP 0 F= 4 PICK 0 F= AND 
   IF 
     -1 0 0 
      0 0 1
   ELSE
    ( DROP      "drop", why? It should not exist, but I need it... )  
     VDUP
     0 1 0   ( -y versor )
     VCROSS
     VNORM
     VDUP    ( 6 PICK 6 PICK 6 PICK
     9 PICK 9 PICK 9 PICK
     VCROSS
     VSWAP
   ENDIF
;

( matrix multiplication )
( to improve, better without vect variables or with stack ? ) 

: CMULC  ( cCoords1, cCoords2 CMULC cCoordsm )
   CTRASP
   tempz V!
   tempy V!
   tempx V!   ( partitioning Coords 2 in three vertical vectors )
   CDUP 
   tempx V@ CMULV    ( column 1 of resulting matrix )
   CVSWAP CDUP
   tempy V@ CMULV    ( column 2 )
   CVSWAP
   tempz V@ CMULV    ( column 3 )
   CTRASP
;

?ENDIF