!S7Motion7.F95       Group ID: #7
!2025.05.24.1840cdt- Modelview Matrix control using the mouse
!                 - 7DoF Point-of-Interest(PoI)
!                 - Standard Flight Simulation (FS) coordinates, i.e.:
!                       +X:forward, +y:right, +Z:down

!            Author- Jeffrey M. Setterholm, Lakeville,MN 55044 USA
!         IP Status- Free source code (e.g.: post copyright)
!
!          Computer- "T3"/Dell Precision T3500/Intel i5 E5520/Win10Pro-21H2
!                     ^name ^Mfgr.Id           ^chipset       ^OS 
!                    /Absoft Pro Fortran 21.0.2/GeForce GTX 1050/f90gl~Glut3.7
!                     ^compiler ~Fortran 95     ^graphics card   ^graphics

!    f90gl bindings- public domain; see "https://math.nist.gov/f90gl/"

!Disclaimer:
! ***************************************************************
! **********   Individual cognition is always flawed,  **********
! **********         including yours and mine.         **********
! **********                 - So: -                   **********
! **********       Use this code at your own risk.     **********
! ***************************************************************

!Table of Contents: ...use to search...
! Subroutine Motion7(nM7,iEye,iP)
! Subroutine hPoIGen(hPoI,XyzRpyM,Mode,iP)
! Subroutine hPoIDecode(hPoI,PoI7,iP)

! Subroutine hFS7Gen(hFS7,XyzRpyM,iP)
! Subroutine hFS7Decode(hFS7,FS7,iP)

! Subroutine DCfromRPY(DC,RPY,iP)
! Subroutine OddEven7DoF(ioe,ioein,ioeu,DoFsInUse,iP)

! Subroutine PackM7u
! Subroutine UnPackM7u

! Subroutine CubeGrid(nCol)
! Subroutine WirePlane(Sizeu,nCol)
! Subroutine VecFont7(XYZRPYwC,LineWidth,iCol,Label)

!--Quaternions:
! Subroutine QFromRpy(Q,Rpyh)
! Subroutine QoutFromQoQi(Qout,Qo,Qi)
! Subroutine QtoRpy(Q,Rpy)
! Subroutine QToQinverse(Q,Qinverse)
! Subroutine QtoVhatAngle(Q,Vhat,Angle,iP)      <-"JeffTernions"

!---------------------------------------------------------------------------7-9

Subroutine Motion7(nM7,iEye,iPin)
!2020.06.30.0805cdt JMS- Updates Motion 7DoF's.
!//////////////////////////////////////////////////
!--Globals
 use ioDef       ,only: Us,Ut,UpVerb,NowView             !Files,Units,etc.
 use ScreenDef   ,only: nCharMaxY,nCharMaxXS,Side     &  !screen & colors
                       ,lCharX,lCharY,nCharMaxX,nCharCenX
 use KeyboardDef ,only: KbdKey,ArrowKey,NumLockOn        !Keyboard
 use MouseDef    ,only: MChan,MouseDirect,MXY,MXYdel,iMouse7DoF
 use ModelDef    ,only: c7DoFID,iColors,cDoFID        &  !Modelview matrix
       ,M7,M7zero,hPoIRotate,M7u,Motion7Rec           &
       ,DoFL,H,DoFreset,Sens,nM7L,ID,iType,Locked,iec,ioe,DoFsInUse,M7Init
 use simdef      ,only: IterTotal
 use ViewDef     ,only: Key

!--End Globals
 implicit none
 !--Arguments- ^global variables in S1ModDef.f95:
  integer(4)::nM7          !Index of the 7DoF object being controlled
  integer(4)::iEye
  integer(4)::iPin         ! >5: Printing enable, use: write(iP,...)
 !--Internals
  integer(4)::iP           ! >5: Printing enable, use: write(iP,...)
  real(8)   ::DoF(7,0:3)   !(/ X,Y,Z,Roll,Pitch,Yaw,Mag /)
  integer(4)::ioein(0:1)   !Axis change: Odd-or-even & DoF#
  integer(4)::ioeu         !=ioe(ioe(0))
  integer(4)::ie
  real(8)   ::Mag
  integer(4)::Ku,Au,i,j,i7u,IntExtMod(2),iDelta,iDeltaL,iUnlock(0:7)=1
  integer(4)::iRepeat
  !Screen reporting:
  integer(4)::nRow,nCol,nCol2,iColor,McharXY(2),jPrev
  character(len=80):: PText
  !--EndDefs-----------------------------------------------
   iP=iPin !; if(UpVerb==0) iP=0
   if(iP>5) write(iP,"('Motion7:  7DoF',36('\'))")
   M7u = M7(nM7); call UnPackM7u; ie=iec
   if(M7Init==0) then
    !--Initialize DoF's & Transform:
     M7u                 = M7zero
     nM7L                  = nM7
     if(nM7L==0) iType =0 !Point-of-Interest control: <-Translate<-Rotate
     if(nM7L> 0) iType =1 !Flight Simulation control: <-Rotate<-Translate
     write(ID,"('M7(',i3,')')") nM7
     DoFreset              =(/0.d0,0.d0,0.d0,0.d0,0.d0, 0.d0,+1.d0/)
     DoFL                  = DoFReset
     if(iType==0) call hPoIGen(H,DoFL,0,0)
     if(iType==1) call hFS7Gen(H,DoFL,0,0)
     Sens                  =(/.05d0,.05d0,.05d0,1.0d0,1.0d0,1.0d0,.01d0/)

    !--Initialize control axes:
     ioe            = (/2,5,6/)
     MChan(3)%i7DoF = ioe(2) !Mouse channel assignment- lateral
     MChan(4)%i7DoF = ioe(1) !                        - vertical
     DoFsInUse      = (/0,0,0,0,0,2,1,0/)
     Locked         =   0        !unlocked
     ie             =   1        !interior control
     M7Init         =   1
     iec=ie; call PackM7u; M7(nM7) = M7u
   endif! M7Init=0
   if(iEye>0)                                                          goto 100
  !--Each iteration's initialization:
   DoF        = 0.d0
   DoF(1:7,1) = DoFL
   MChan(4)%i7DoF = ioe(1)
   MChan(3)%i7DoF = ioe(2)

   ioein      = 0
   Mag        = DoF(7,1)
   if (Mag<.000001d0) Mag=.001d0
  !--Save the incoming 7DoF state (for locking):
   DoF(1:7,0)=DoF(1:7,1)
  !--exterior inputs, if any, are incremental:
   DoF(1:7,2)=(/0.d0,0.d0,0.d0,0.d0,0.d0,0.d0,1.d0/)
  !---------------------------------------------------
  !ASCII NumLocked Key ID's:                 operators
  !       /     *    -  |     k47  k42  k45      * k42
  !  7    8     9    +  |k55  k56  k57  K43      + k43
  !  4    5     6    "  |k52  k53  k54   "       - k45
  !  1    2     3  Enter|k49  k50  k51  K13      / k47
  !  0-----     .    "  |k48-----  k46   "
  !---------------------------------------------------
               IntExtMod=0;  iDelta=0
  !--Numeric keypad DoF selection:
               ku=kbdkey;KbdKey=0; ioeu=ioe(ioe(0))
   select case(Ku)
    case(46); Locked(ioeu)=1-Locked(ioeu) !'.' Lock
    case(48); if(  ioeu >0) then         !'0' zero ~ reset one
                DoF(ioeu,1) = DoFreset(ioeu)
                DoF(ioeu,0) = DoF(ioeu,1)
                IntExtMod(1)=1
              endif!ioeu>0
    case(49,51,53,55)         !Odds: '1':X, '3':Z, '5':Pitch, '7':Mag
     ioein(0) = 1
     ioein(1) = Ku-48
     call OddEven7DoF(ioe,ioein,ioeu,DoFsInUse,0)
     MChan(4)%i7DoF = ioe(1)
     MChan(3)%i7DoF = ioe(2)
     NumLockOn=1
    case(50,52,54)            !Evens: '2':Y, '4':Roll, '6':Yaw
     ioein(0) = 2
     ioein(1) = Ku-48
     call OddEven7DoF(ioe,ioein,ioeu,DoFsInUse,0)
     MChan(4)%i7DoF = ioe(1)
     MChan(3)%i7DoF = ioe(2)
     NumLockOn=1
    case(56)                  !'8':Reset modelview 7DoF entirely:
     DoF(1:7,1) = DoFreset
     IntExtMod(1)= 1
     if(Ip>5) write(Ip,"('Reset all: X,Y,Z,Roll,Pitch,Yaw,& Mag:')")
    case(57); ie=3-ie         !'9':INTERIOR|exterior flag:
    case default; KbdKey=ku   !Keypresses ignored, e.g.:non-numeric:
   end select !ku
   i7u=ioeu

   !--ArrowKey manual 7DoF control:
               Au=ArrowKey; ArrowKey=0
   select case(Au)
   !--Arrowkeys- lateral:
    case(100); IntExtMod(ie)=1;  iDelta=-1 !Arrow-left:
     i7u=MChan(3)%i7DoF
    case(102); IntExtMod(ie)=1;  iDelta=+1 !Arrow-right:
     i7u=MChan(3)%i7DoF
   !--Arrowkeys- into:
    case(101); IntExtMod(ie)=1;  iDelta=+1 !Arrow-forward:
     i7u=MChan(4)%i7DoF
    case(103); IntExtMod(ie)=1;  iDelta=-1 !Arrow-back:
     i7u=MChan(4)%i7DoF
   !--NumLock off warning:
    case(104:108); write(6,"(a1)") char(7) !; ArrowKey=Au                 !Beep
     NumLockOn=0
   !--ArrowKeys ignored, e.g.:not arrows :
    case default; ArrowKey=Au
   end select!(Au)

   iRepeat=0
50 continue
  !--Increment/Decrement individual Point-of-Interest values:
   if(iDelta/=0) then
     select case(i7u)
      case( 0 ) !
      case(1:3) !X,Y,Z:
       iDeltaL=iDelta; if(i7u==3) iDeltaL=-iDelta
       if(ie==1) DoF(i7u,ie)=DoF(i7u,ie)+.05d0*iDeltaL/Mag
       if(ie==2) DoF(i7u,ie)=DoF(i7u,ie)+.05d0*iDeltaL
      case(4:6 ) !Roll.Pitch,Yaw:
       DoF(i7u,ie)=DoF(i7u,ie)+1.0d0*iDelta
       if(DoF(i7u,ie)>   180.d0) DoF(i7u,ie) = DoF(i7u,ie)-360.d0
       if(DoF(i7u,ie)<= -180.d0) DoF(i7u,ie) = DoF(i7u,ie)+360.d0
      case( 7 ) !Mag
       if(abs(iDelta)>5) iDelta=sign(5,iDelta)
       if(iDelta>0) DoF(i7u,ie) = DoF(i7u,ie)*1.010d0     **abs(iDelta)
       if(iDelta<0) DoF(i7u,ie) = DoF(i7u,ie)*0.99009901d0**abs(iDelta)
       if(dabs(     DoF(i7u,ie)  -1.d0) < +.005d0) &
                    DoF(i7u,ie) = 1.d0
      !--Bound magnitude:
       if(          DoF(i7u,ie) <   0.001d0) &
                    DoF(i7u,ie) =   0.001d0
       if(          DoF(i7u,ie) > 1000.0d0 ) &
                    DoF(i7u,ie) = 1000.0d0
      case default; write(6,"('i7u=',i2,' ie=',i1,': out of range @L143.')") &
                               i7u     ,  ie
     end select! i7
   endif!iDelta/=0

  !--LMB-pressed + Mouse motion manual 7DoF control:
   if(MXYDel(3)/=2) then; iRepeat        =3 !LMB- not pressed
    else;                 iRepeat=iRepeat+1 !   -     pressed
     select case(iRepeat)
      case(1) !Mouse X-axis:
       i7u=MChan(3)%i7DoF;   iDelta = MXYDel(1)
       if(iDelta/=0) IntExtMod(ie)=1; MXYDel(1)= 0
       goto 50
      case(2) !Mouse Y-axis:
       i7u=MChan(4)%i7DoF;   iDelta =-MXYDel(2)
       if(iDelta/=0) IntExtMod(ie)=1; MXYDel(2)= 0
       goto 50
     end select !(iRepeat)
   endif !MXYDel(3)/=2

   if(MXY(3,1)==2) then !Left mouse button- pressed:
    !--LMB-pressed - check for mouse-screen-based DoF changes:
     nRow  = nCharMaxY-17; nCol = nCharMaxXS(Side)-33
     McharXY(1)=MXY(1,1)/lCharX
    !--Interpret mouse X location in the split screen, if necessary:
     if((Key%SplitScreen/=0).and.(McharXY(1)>nCharCenX)) &
     McharXY(1)=McharXY(1)-(nCharCenX-1)
     McharXY(1)=McharXY(1)-nCol
     McharXY(2)=MXY(2,1)/lCharY-nRow; j=McharXY(2)-2
     if(j/=jPrev) then
       select case(j)
        case(-1) !Change the controlled object
         if(McharXY(1)>0) then
           iMouse7Dof=1-iMouse7Dof                                  ;jPrev=j
         endif!McharXY(1)>0
        case(1:7)
         select case(McharXY(1))
          case(16 ); if(IterTotal-MXY(4,1)<2) Locked(j)=1-Locked(j) ;jPrev=j
          case(18:28)                                               ;jPrev=j
!          if(Ut>5) write(Ut,"('LMB DoF update:',4i5)") nM7,j,McharXY
           select case(j)
            case(1,3,5,7); ioein = (/1,j/)
            case(2,4,6  ); ioein = (/2,j/)
           end select!(J)
           call OddEven7DoF(ioe,ioein,ioeu,DoFsInUse,0)
           MChan(4)%i7DoF= ioe(1)
           MChan(3)%i7DoF= ioe(2)
         end select!(MpiXY(1))
       end select!j
     endif!j/=jPrev
    else; jPrev=0
   endif!MXY(3,1)==2

   if(nM7>0) then   !For Flight Simulation 6DoF:
     Locked(7)   =1 !  No magnification
     ie          =1 !  & internal control only.
     IntExtMod(2)=0
   endif!nM7>0

   i7u=ioeu
   if((IntExtMod(1)/=0).or.((iType==0).and.(IntExtMod(2)/=0)) ) then
    !--Check for locked DoF's
     Locked(0)=0; do i=1,7; if(Locked(i)>0) Locked(0)=1; enddo!i
    !--Point-of-Interest transform update:
    !--Update ModelView's interior 7DoF Transform:
     if(IntExtMod(1)>0) then
       do i=1,7; if(Locked(i)==1) DoF(i,1)=DoF(i,0); enddo!i
!      if(iP>5) write(iP,"('INTERIOR 7DoF transform update:' )")
       if(iType==0)call hPoIGen(H,DoF(1,1),0,iP)
       if(iType==1)call hFS7Gen(H,DoF(1,1),iP)
     endif!IntExtMod(1)>0
     if(IntExtMod(2)>0) then
    !--Update ModelView's exterior 7DoF Transform:
       DoF(1:7,1)=DoF(1:7,2)
       if(iType==0)call hPoIGen(H,DoF(1,1),1,iP)
       if(Locked(0)>0) then
         do i=1,7; if(Locked(i)==1) DoF(i,1)=DoF(i,0); enddo!i
         if(iP>5) write(iP,"('& with locked state(s):' )")
       if(iType==0)call hPoIGen(H,DoF(1,1),0,iP)
       endif!Locked(0)>0
     endif!IntExtMod()>0                   !Use DoF(_,1) for screen reporting.
  !--Update the PoI rotation-only transform:
     if(iType==0) then
       DoF(1:7,3) =(/    0.d0     ,   0.d0     ,   0.d0      &
                         ,DoF(4,1),DoF(5,1),DoF(6,1) &
                         ,   1.d0                                 /)
       if(iP>5) write(iP,"('PoI rotation only:' )")
       call hPoIGen(hPoIRotate,DoF(1,3),0,iP)
     endif!iType==0
    else
     if(iP>5) then
       write(iP,"('M7(',i3,') transform:' )") nM7
       if(iType==0)call hPoIGen(H,DoF(1,1),0,iP)
       if(iType==1)call hFS7Gen(H,DoF(1,1),iP)
      else
       if(iType==0)call hPoIGen(H,DoF(1,1),0,0)   !WAG add 2021.03.08
       if(iType==1)call hFS7Gen(H,DoF(1,1),0)     ! "   "       "
     endif!iP>1\5
   endif! IntExtMod(1)/=0 or IntExtMod(2)/=0 

  !--Identify the controlled 7DoF's:
   MChan(3)%ID = cDoFID(ioe(2))
   MChan(4)%ID = cDoFID(ioe(1))
   DoFL=DoF(1:7,1); iec=ie; call PackM7u; M7(nM7) = M7u

100 continue
    if(NowView/=1)                                                       return
  !--On-screen report:
                         nRow =nCharMaxY-16;           nCol=nCharMaxXS(Side)-33
  !write(Ptext,"('Units=    ',a12)") S.cFrustumUnits           !LGreen
     !                    nRow=nRow+1; call PrntOrtho(nRow,nCol,10, 0,PText)

   do i=1,15; PText=char(0); iColor=iColors(i)
     select case(i)
      case(1:2,10,12:15) !7DoF generic info:
                      write(Ptext,"(a32)") c7DoFID(i)
       if(i==1) then
         if(iType==0) write(Ptext,"('Seven DoF Point-of-Interest Ctl:')")
         if(iType >0) write(Ptext,"('Seven DoF Object#',i3,' 6DoF Ctl: ')") nM7
       endif;           nRow=nRow+1; call PrntOrtho(nRow,nCol,iColor,0,PText)
      case(3:9); j=i-2   !7DoF states info:
       if(j==ioe(1)) iColor= 1
       if(j==ioe(2)) iColor= 1
       write(Ptext,"(a32)") c7DoFID(i)
                         nRow=nRow+1; call PrntOrtho(nRow,nCol,iColor,0,PText)
       write(PText,"(f16.6)") DoFL(j)
                                      call PrntOrtho(nRow,nCol,iColor,0,PText)
       PText="L"                                                 !White
       if(Locked(j)>0)                call PrntOrtho(nRow,nCol+17, 1 ,0,PText)
       if(iColor==1) then
         if(j==ioe(ioe(0))) then;  PText="1"
                            else;  PText="2"
         endif!j==ioe(io1e2)
                         nCol2=nCol+27
         if(mod(j,2)==1) nCol2=nCol+29
                                      call PrntOrtho(nRow,nCol2,iColor,0,PText)
       endif!iColor==1 
      case(11)           !Numlock& INT|EXT
       if(NumLockOn==0) then; PText="NumLock?" ;                iColor=7!Red
        else;                 PText="NumLocked";                iColor=3!LLWhit
       endif;             nRow=nRow+1;;call PrntOrtho(nRow,nCol,iColor,0,PText)
       if(ie==1) Ptext="INTERIOR control"
       if(ie==2) Ptext="EXTERIOR control"                         !White
                                       call PrntOrtho(nRow,nCol+15, 1 ,0,PText)
     end select!(i)
   enddo!i
   if(Ip>5) write(Ip,"(14x,'Display on-screen M7(',i3,') 7DoF controls.'))")nM7
   if(Ip>5) write(Ip,"('End Motion7:  ',36('/'))")                       ;return
End Subroutine Motion7
!---------------------------------------------------------------------------7-9

Subroutine hPoIGen(hPoI,XyzRpyM,Mode,iP)
!2020.06.18.1210cdt JMS- Generates the Point-of-Interest control matrix
!                          in the INTERIOR or the exterior concatenated form.
 implicit none
 !--Arguments:
  real(8)   ::hPoI(4,4)  !<-- Rotate+Magnify <-- Translate <--
  real(8)   ::XyzRpyM(7) !7DoF rigid body motion & magnification:
 !                      !X    Y    Z  Roll Pitch Yaw  Mag
 !                      !1    2    3    4    5    6    7
  integer(4)::Mode       !=0: no incoming matrix - defining the INTERIOR matrix.
                        !=1: hPoIin defined; concat. the exterior matrix.
  integer(4)::iP         !>5: Printing enable, use: write(iP,...)
 !--Internals
  real(8)   ::V(7)       !internal copy of XyzRpyM(7)
  real(8)   ::Hin(4,4)   !internal copy of hPoI input
  real(8)   ::H(4,4),Hout(4,4)
  real(8)   ::DC(3,3)                   !Direction cosines of (Roll,Pitch,Yaw)
  real(8)   ::XYZ(3)                    !rotated translations
  integer(4)::i,j
  real(8)   ::XyzRpyML(7)
  !--EndDefs-----------------------------------------------
   V   = XyzRpyM
   Hin = hPoI !Used by the exterior PoI computation
  !--Compute the direction cosines:
   call DCfromRPY(DC,V(4:6),iP)
  !--Rotate(X,Y,Z)
   do i=1,3; XYZ(i)=DC(i,1)*V(1)+DC(i,2)*V(2)+DC(i,3)*V(3); enddo!i
  !--Compute the PoI transform of XyzRpyM:
   H(1,1:4) = (/ DC(1,1), DC(1,2), DC(1,3),   XYZ(1) /)
   H(2,1:4) = (/ DC(2,1), DC(2,2), DC(2,3),   XYZ(2) /)
   H(3,1:4) = (/ DC(3,1), DC(3,2), DC(3,3),   XYZ(3) /)
   H(4,1:4) = (/  0.d0  ,  0.d0  ,  0.d0  ,+1.d0/V(7)/)
   select case(Mode)
    case(0) !INTERIOR PoI:
     if(iP>5) then !FYI printout:
       write(iP,"('hPoIGen: INTERIOR.  XyzRpyM=')")
       write(iP,"(7f10.4,'     :DoF')") V
       write(iP,"( '         yields hPoI:')")
       write(iP,"( 4f20.9)") ((H(i,j),j=1,4),i=1,4)
     endif!iP>5
     hPoI = H                                                           ;return

    case(1) !Concatenate the exterior PoI:
     do i=1,4; do j=1,4; Hout(i,j)=  H(i,1)*Hin(1,j) &
                                   + H(i,2)*Hin(2,j) &
                                   + H(i,3)*Hin(3,j) &
                                   + H(i,4)*Hin(4,j)   ;enddo; enddo!i
     hPoI   = Hout
     if(iP>5) then !FYI printout:
       write(iP,"('hPoIGen: EXTERIOR. Incoming XyzRpyM= :')")
       write(iP,"(7f10.4,'     :DoF')") V
       write(iP,"('     & incoming hPoI:')")
       write(iP,"(4f20.9)") ((Hin(i,j),j=1,4),i=1,4)
       call hPoIDecode(Hin,XyzRpyML,iP)
       write(iP,"( '      exterior tweak:')")
       write(iP,"( 4f20.9)") ((H(i,j),j=1,4),i=1,4)
       write(iP,"( '      Resulting hPoI:')")
       write(iP,"( 4f20.9)") ((Hout(i,j),j=1,4),i=1,4)
       write(iP,"('  & DoF XyzRpyM=')")
     endif!iP>5
     call hPoIDecode(Hout,XyzRpyM,iP)                                   ;return
    !--Decode hPoI's 7DoF states:
   end select!Mode
End Subroutine hPoIGen
!---------------------------------------------------------------------------7-9

Subroutine hPoIDecode(hPoI,PoI7,iP)
!2020.06.10.0500cdt JMS- !Decodes hPoI7's 7DoF states.
 implicit none
 !--Arguments:
  real(8)   ::hPoI(4,4) !Point-of-Interest- homogeneous transform
  real(8)   ::PoI7(7)   !                 - seven degrees of freedom:
                       !                  (X,Y,Z,Roll,Pitch,Yaw,Magnification)
  integer(4)::iP        !>5: Printing enable, use: write(iP,...)
 !--Internals
  real(8)   ::H(4,4)    !internal copy of hPoI7
  real(8)   ::XYZ(3)
  real(8)   ::X,Y,Z,Roll,Pitch,Yaw,Mag  !Individual 7DoF states
  real(8)   ::sP        !sine of Pitch
  integer(4)::i
  !--EndDefs-----------------------------------------------
   H=hPoI
  !--Decode Hout's 7DoF states:
   do i=1,3; XYZ(i)=H(1,i)*H(1,4)+H(2,i)*H(2,4)+H(3,i)*H(3,4); enddo!i
   Mag = 1.d0/H(4,4)
   X   = XYZ(1)
   Y   = XYZ(2);             sP    = -H(3,1)
   Z   = XYZ(3);             Pitch =  dasind(sP)
     if(dabs(sP)<1.d0) then; Yaw   =  datan2d(H(2,1),H(1,1))
                             Roll  =  datan2d(H(3,2),H(3,3))
                       else; Roll  =  0.d0
                             Yaw   =  datan2d(-H(1,2),H(2,2))
     endif!|sP|<1.
   PoI7=(/ X,Y,Z,Roll,Pitch,Yaw,Mag /)
   if(iP>5) write(iP,"(7f10.4,'     :PoI7')") PoI7                          ;return
End Subroutine hPoIDecode
!---------------------------------------------------------------------------7-9

Subroutine hFS7Gen(hFS7,XyzRpyM,iP)
!2020.06.10.0705cdt JMS- Generates 6DoF rigid body motion (+magnification)
!                          using Standard Flight Simulation coordinates.
 implicit none
 !--Arguments:
  real(8)   ::hFS7(4,4)  !<-- Translate <-- Magnify+Rotate <--
                        ! C11 , C12 , C13 ,    +X/Mag
                        ! C21 , C22 , C23 ,    +Y/Mag
                        ! C31 , C32 , C33 ,    +Z/Mag
                        ! 0.d0, 0.d0, 0.d0, +1.d0/Mag

  real(8)   ::XyzRpyM(7) !7DoF rigid body motion & magnification:
 !                      !X    Y    Z  Roll Pitch Yaw  Mag
 !                      !1    2    3    4    5    6    7
  integer(4)::iP         !>5: Printing enable, use: write(iP,...)
 !--Internals
  real(8)   ::V(7)       !internal copy of XyzRpyM(7)
  real(8)   ::H(4,4)     !internal copy of hFS input
  real(8)   ::DC(3,3)    !Direction cosines of (Roll,Pitch,Yaw)
  integer(4)::i,j
  !--EndDefs-----------------------------------------------
   V=XyzRpyM
   if(iP>5) write(iP,"('hFS7Gen: XyzRpyM=')")
   if(iP>5) write(iP,"(7f10.4,'      :FS7')") V
  !--Compute the direction cosines:
   call DCfromRPY(DC,V(4:6),iP)
  !--Compute the FS transform of XyzRpyM:
   H(1,1:4) = (/ DC(1,1), DC(1,2), DC(1,3), V(1)/V(7) /)
   H(2,1:4) = (/ DC(2,1), DC(2,2), DC(2,3), V(2)/V(7) /)
   H(3,1:4) = (/ DC(3,1), DC(3,2), DC(3,3), V(3)/V(7) /)
   H(4,1:4) = (/  0.d0  ,  0.d0  ,  0.d0  ,+1.d0/V(7) /)
   hFS7     = H                                                ;if(iP<6) return
   write(iP,"( '  yields hFS7:')")
   write(iP,"( 4f20.9)") ((H(i,j),j=1,4),i=1,4)                         ;return
End Subroutine hFS7Gen
!---------------------------------------------------------------------------7-9

Subroutine hFS7Decode(hFS7,FS7,iP)
!2020.06.10.0700cdt JMS- !Decodes hFS7's 7DoF states.
 implicit none
 !--Arguments:
  real(8)   ::hFS7(4,4)  !<-- Translate <-- Magnify+Rotate <--
  real(8)   ::FS7(7)     !Decoded seven degrees of freedom:
                        !  (X,Y,Z,Roll,Pitch,Yaw,Magnification)
  integer(4)::iP         !>5: Printing enable, use: write(iP,...)
 !--Internals
  real(8)   ::H(4,4)     !internal copy of hFS7
  real(8)   ::X,Y,Z,Roll,Pitch,Yaw,Mag  !Individual 7DoF states
  real(8)   ::sP         !sine of Pitch
  !--EndDefs-----------------------------------------------
   H=hFS7
  !--Decode hFS7's 7DoF states:
                Mag = 1.d0/H(4,4)
   X   = H(1,4)*Mag
   Y   = H(2,4)*Mag;         sP    = -H(3,1)
   Z   = H(3,4)*Mag;         Pitch =  dasind(sP)
     if(dabs(sP)<1.d0) then; Yaw   =  datan2d(H(2,1),H(1,1))
                             Roll  =  datan2d(H(3,2),H(3,3))
                       else; Roll  =  0.d0
                             Yaw   =  datan2d(-H(1,2),H(2,2))
     endif!|sP|<1.
   FS7 = (/ X,Y,Z,Roll,Pitch,Yaw,Mag /)
   if(iP>5) write(iP,"(7f10.4,'      :FS7')") FS7                       ;return
End Subroutine hFS7Decode
!---------------------------------------------------------------------------7-9

Subroutine DCfromRPY(DC,RPY,iP)
!2020.06.10.0420cdt JMS- !Computes Direction Cosines from FS (Roll,Pitch,Yaw).
 implicit none
 !--Arguments:
  real(8)   ::DC(3,3)    !Direction Cosine output.
  real(8)   ::RPY(3)     !(Roll,Pitch,Yaw) - in Flight Simulation coordinates.
  integer(4)::iP         !>5: Printing enable, use: write(iP,...)
 !--Internals
  real(8)   ::Roll,Pitch,Yaw      !Individual rotation angles
  real(8)   ::sr,cr, sp,cp, sy,cy !Sines & Cosines of Roll,Pitch,& Yaw
  real(8)   ::C11,C21,C31,C12,C22,C32,C13,C23,C33 !DC coefficients;
  !--EndDefs-----------------------------------------------
   Roll  = RPY(1); sR=dsind(Roll ); cR=dcosd(Roll )
   Pitch = RPY(2); sP=dsind(Pitch); cP=dcosd(Pitch)
   Yaw   = RPY(3); sY=dsind(Yaw  ); cY=dcosd(Yaw  )
   C11 = cY*cP ; C12 = (cY*sP*sR-sY*cR) ; C13 = (cY*sP*cR+sY*sR)
   C21 = sY*cP ; C22 = (sY*sP*sR+cY*cR) ; C23 = (sY*sP*cR-cY*sR)
   C31 =   -sP ; C32 =     cP*sR        ; C33 =     cP*cR
   DC(1,1:3)=(/ C11,C12,C13 /)
   DC(2,1:3)=(/ C21,C22,C23 /)
   DC(3,1:3)=(/ C31,C32,C33 /)                                          ;return
End Subroutine DCfromRPY
!---------------------------------------------------------------------------7-9

Subroutine OddEven7DoF(ioe,ioein,ioeu,DoFsInUse,iP)
!2020.06.20.1530cdt JMS- Odd|Even DoF management
 implicit none
 !--Arguments
  integer(4)::ioe(0:2)      !(0)- =1: Primary control- vertical- X,Z,Pitch,Mag
                           !     =2:                - lateral - Y,Roll,Yaw
                           !(1)    :Controlled DoF- vertical
                           !(2)    :              - lateral
  integer(4)::ioein(0:1)   !Axis change: Odd-or-even & DoF#
  integer(4)::ioeu         !=ioe(ioe(0))
  integer(4)::DoFsInUse(0:7)!=0 not under control
                           !=1 primary   #1 -onscreen
                           !=2 secondary #2 -
  integer(4)::iP           ! >5: Printing enable, use: write(iP,...)
 !--Internals
  !--EndDefs-----------------------------------------------
   if(iP>5) write(iP,"('OddEven7DoF: ioein=',2i2)") ioein
                 ioe(0)     = ioein(0)
             ioe(ioe(0))    = ioein(1) !Activate the DoF 
   DoFsInUse                = 0
   DoFsInUse(ioe(  ioe(0))) = 1
   DoFsInUse(ioe(3-ioe(0))) = 2
             ioeu           = ioe(0)
             ioein          = 0
   if(iP>5) write(iP,"(13x,         'ioe  =',3i2,3x,8i2)") ioe,DoFsInUse
   return
End Subroutine OddEven7DoF
!---------------------------------------------------------------------------7-9

Subroutine PackM7u
!2020.06.21.0900cdt JMS- Unpacks a Unit Cube vector into its 14 components.
!--Globals
 use ModelDef    ,only: &
   M7u,DoFL,H,DoFreset,Sens,nM7L,ID,iType,Locked,iec,ioe,DoFsInUse,M7Init
 implicit none
  !--EndDefs-----------------------------------------------
   M7u%DoF       =DoFL
   M7u%H         =H
   M7u%DoFreset  =DoFreset
   M7u%Sens      =Sens
   M7u%n         =nM7L
   M7u%ID        =ID
   M7u%iType     =iType
   M7u%Locked    =Locked
   M7u%ie        =iec
   M7u%ioe       =ioe
   M7u%DoFsInUse =DoFsInUse
   M7u%M7Init    =M7Init                                                ;return
End Subroutine PackM7u
!---------------------------------------------------------------------------7-9

Subroutine UnPackM7u
!2020.06.21.0900cdt JMS- Unpacks a Unit Cube vector into its 14 components.
!--Globals
 use ModelDef    ,only: &
   M7u,DoFL,H,DoFreset,Sens,nM7L,ID,iType,Locked,iec,ioe,DoFsInUse,M7Init
 implicit none
  !--EndDefs-----------------------------------------------
   DoFL      =M7u%DoF
   H         =M7u%H
   DoFreset  =M7u%DoFreset
   Sens      =M7u%Sens
   nM7L      =M7u%n
   ID        =M7u%ID
   iType     =M7u%iType
   Locked    =M7u%Locked
   iec       =M7u%ie
   ioe       =M7u%ioe
   DoFsInUse =M7u%DoFsInUse
   M7Init    =M7u%M7Init                                                ;return
End Subroutine UnPackM7u
!---------------------------------------------------------------------------7-9

Subroutine CubeGrid(nCol)
!2024.04.28.2015cdt JMS- Using +X:Orange, +Y:Green, +Z:White for Rubik's cube
!2020.04.05.1315cdt JMS- Draws X,Y,Z axes/6dof Origin, and an enclosing cube.
!--Globals                                            S1ModDef.f95: 2024.03.18
 use OpenGLRec,only:  & !Ref: OpenGL GL/GLU/GLUT docs
      glVertex3f,glShadeModel,GL_SMOOTH,GL_FLAT,glBegin ,GL_LINES,glLineWidth &
     ,glFlush,glEnd
 use S2Callback, only:CheckGL
!--End Globals
 implicit none
 !--Arguments
  integer(4)::nCol
 !--Internals
  integer(4)::i,j,k
  real(4)   ::Frac,gi,gj,gk
 !real(8)   ::Xyzh(4),Rpyh(4)
  real(8)   ::XYZRPYwC(7) !(X,Y,Z,Roll,Pitch,Yaw,height of Character)
  character(len=80)::PText


  !--EndDefs-----------------------------------------------
  !--draw corners
   Frac=.9
   call glLineWidth(1.)
   call glShadeModel(GL_SMOOTH) !(GL_FLAT) !(GL_SMOOTH)
   call Colors3D(nCol)
   if(nCol>0) then
     call glBegin(GL_LINES)
       do     i=-1,1; gi=i
         do   j=-1,1; gj=j
           do k=-1,1; gk=k
             call glVertex3f(gi     ,gj     ,gk      )
             call glVertex3f(gi*Frac,gj     ,gk      )
             call glVertex3f(gi     ,gj     ,gk      )
             call glVertex3f(gi     ,gj*Frac,gk      )
             call glVertex3f(gi     ,gj     ,gk      )
             call glVertex3f(gi     ,gj     ,gk*Frac )
       enddo; enddo; enddo!i
     call glEnd;                                                   call glFlush
   endif!nCol>0
                                               call glFlush;call CheckGL(+70628)
 !--Draw colored axes:
   call glLineWidth(5.)
  !--                                                            mod 2020.04.23

   if(1>0)then !Use +X:Green, +Y:Yellow,+Z:Red-------------
   call Colors3D(11) !Green
   call glBegin(GL_LINES)
     call glVertex3f(0.,0.,0.); call glVertex3f(1.,0.,0.)
   call glEnd
  !--
   call Colors3D( 9) !Yellow 
   call glBegin(GL_LINES)
     call glVertex3f(0.,0.,0.); call glVertex3f(0.,1.,0.)
   call glEnd
  !--
   call Colors3D( 7) !Red
   call glBegin(GL_LINES)
     call glVertex3f(0.,0.,0.); call glVertex3f(0.,0.,1.)
   call glEnd
  !--
                                               call glFlush;call CheckGL(+70646)
   if(nCol>0) then
    !--Axis labels:
                  !XYZRPYwC=(/   X ,   Y  , Z   , Roll, Pitch, Yaw,width Ch/)
                   XYZRPYwC=(/ .8d0, .0d0,-.02d0, 0.d0,  0.d0,  0.d0 ,.1d0 /)
                      !VecFont7(XYZRPYwC,LineWidth,iCol,Label)
     PText="+X" ; call VecFont7(XYZRPYwC,  5.     , 11 ,PText) !Green
     
                   XYZRPYwC=(/ .0d0, .8d0,-.02d0, 0.d0,  0.d0, 90.d0 ,.1d0 /)             
     PText="+Y" ; call VecFont7(XYZRPYwC,  5.     ,  9 ,PText) !Yellow
     
                   XYZRPYwC=(/ .0d0, .02d0, .8d0, 0.d0,-90.d0,  0.d0 ,.1d0 /)          
     PText="+Z" ; call VecFont7(XYZRPYwC,  5.     ,  7 ,PText) !Red
    !              XYZRPYwC=(/   X ,   Y  , Z   , Roll, Pitch, Yaw,width Ch/)
                   XYZRPYwC=(/ .8d0, .02d0, .0d0, 0.d0,  0.d0,  0.d0 ,.1d0 /)
   endif 

   else !Use rubik's cube face colors: --------------------
  !call Colors3D(11) !Green
   call Colors3D(15) !Orange
   call glBegin(GL_LINES)
     call glVertex3f(0.,0.,0.); call glVertex3f(1.,0.,0.)
   call glEnd
  !--
   call Colors3D( 9) !Yellow
   call Colors3D(11) !Green  
   call glBegin(GL_LINES)
     call glVertex3f(0.,0.,0.); call glVertex3f(0.,1.,0.)
   call glEnd
  !--
  !call Colors3D( 7) !Red
   call Colors3D( 1) !White
   call glBegin(GL_LINES)
     call glVertex3f(0.,0.,0.); call glVertex3f(0.,0.,1.)
   call glEnd
  !--
   call Colors3D( 7) !Red
   call glBegin(GL_LINES)
     call glVertex3f(0.,0.,0.); call glVertex3f(-1.,0.,0.)
   call glEnd
  !--
   call Colors3D(14) !Blue  
   call glBegin(GL_LINES)
     call glVertex3f(0.,0.,0.); call glVertex3f(0.,-1.,0.)
   call glEnd
  !--
   call Colors3D( 9) !Yellow
   call glBegin(GL_LINES)
     call glVertex3f(0.,0.,0.); call glVertex3f(0.,0.,-1.)
   call glEnd
                                              call glFlush;call CheckGL(+70646)
   if(nCol>0) then
    !--Axis labels:
                  !XYZRPYwC=(/   X ,   Y  , Z   , Roll, Pitch, Yaw,width Ch/)
                   XYZRPYwC=(/ .8d0, .0d0,-.02d0, 0.d0,  0.d0,  0.d0 ,.1d0 /)
                      !VecFont7(XYZRPYwC,LineWidth,iCol,Label)
    !PText="+X" ; call VecFont7(XYZRPYwC,  5.     , 11 ,PText) !Green
     PText="+X" ; call VecFont7(XYZRPYwC,  2.     , 15 ,PText) !Orange
     
                   XYZRPYwC=(/ .0d0, .8d0,-.02d0, 0.d0,  0.d0, 90.d0 ,.1d0 /)             
    !PText="+Y" ; call VecFont7(XYZRPYwC,  5.     ,  9 ,PText) !Yellow
     PText="+Y" ; call VecFont7(XYZRPYwC,  2.     , 11 ,PText) !Green
     
                   XYZRPYwC=(/ .0d0, .02d0, .8d0, 0.d0,-90.d0,  0.d0 ,.1d0 /)          
    !PText="+Z" ; call VecFont7(XYZRPYwC,  5.     ,  7 ,PText) !Red
     PText="+Z" ; call VecFont7(XYZRPYwC,  2.     ,  1 ,PText) !White
    !              XYZRPYwC=(/   X ,   Y  , Z   , Roll, Pitch, Yaw,width Ch/)
                   XYZRPYwC=(/ .8d0, .02d0, .0d0, 0.d0,  0.d0,  0.d0 ,.1d0 /)
    !call VecFont7(XYZRPYwC,LineWidth,iCol,Label)
 
                   XYZRPYwC=(/-1.d0, .0d0,-.02d0, 0.d0,  0.d0,  0.d0 ,.1d0 /)
     PText="-X" ; call VecFont7(XYZRPYwC,  2.     ,  7 ,PText) !Red
     
                   XYZRPYwC=(/.0d0,-1.d0,-.02d0, 0.d0,  0.d0, 90.d0 ,.1d0 /)    
     PText="-Y" ; call VecFont7(XYZRPYwC,  2.     , 14 ,PText) !Blue
     
                   XYZRPYwC=(/ .0d0, .02d0,-1.d0, 0.d0,-90.d0,  0.d0 ,.1d0 /)             
     PText="-Z" ; call VecFont7(XYZRPYwC,  2.     ,  9 ,PText) !Yellow    
   endif 
   endif!(1>0)then !---------------------------------------
                                              call glFlush;call CheckGL(+70658)
                                                                        ;return
End Subroutine CubeGrid
!---------------------------------------------------------------------------7-9

Subroutine WirePlane(Sizeu,nCol)
!2023.09.07.1200cdt JMS-  Draws wire plane model.
!--Globals                                             S1ModDef.f95: 2024.03.18
 use OpenGLRec,only:  & !Ref: OpenGL GL/GLU/GLUT docs
      glBegin      ,glEnd      ,glFlush                          &
     ,glShadeModel ,GL_SMOOTH  ,GL_FLAT                          &
     ,GL_POINTS    ,glPointSize                                  &
     ,GL_LINES     ,glLineWidth                                  &
     ,GL_LINE_STRIP                                              &
     ,GL_Triangles                                               &
     ,GL_QUADS                                                   &
     ,glPolygonMode,GL_LINE       ,GL_FILL                       &
     ,glCullFace   ,GL_CULL_FACE  ,GL_Front  ,GL_BACK            &
!   !,GL_FRONT_AND_GL_BACK                                       &
     ,glVertex3f   ,glVertex3fv                                  &
     ,GL_Lighting  ,glEnable   ,glDisable

 use S2Callback, only:CheckGL
!--End Globals
 implicit none
 !--Arguments
  real(4)   ::sizeu
  integer(4)::nCol
 !--Internals
  real(8)   ::XYZRPYwC(7) !(X,Y,Z,Roll,Pitch,Yaw,height of Character)
  character(len=80)::PText
  real(4)   ::Po(3) = (/  0.   , 0.   , 0.     /)!Origin
  real(4)   ::Px(3) = (/  1.   , 0.   , 0.     /)!Axes
  real(4)   ::Py(3) = (/  0.   ,  .5  , 0.     /)
  real(4)   ::PZ(3) = (/  0.   , 0.   ,  .5    /)
  real(4)   ::P1(3) = (/ +1.   , 0.   , 0.     /)!Fuselage
  real(4)   ::P2(3) = (/ -1.   , -.667, 0.     /)
  real(4)   ::P3(3) = (/ -1.   , +.667, 0.     /)
  real(4)   ::P4(3) = (/  0.   , 0.   , 0.     /)!Tail
  real(4)   ::P5(3) = (/ -1.   , 0.   , 0.     /)
  real(4)   ::P6(3) = (/ -1.   , 0.   , -.667  /)
  !--EndDefs-----------------------------------------------
   call glLineWidth(1.)
   call glShadeModel(GL_SMOOTH) !(GL_FLAT)(GL_SMOOTH)
   call Colors3D(nCol)                        ;call glFlush;call CheckGL(+70628)
  !--
!  call Colors3D( 1) !White
!  call Colors3D( 7) !Red
!  call Colors3D( 9) !Yellow
!  call Colors3D(11) !Green

   call glPolygonMode(GL_FRONT            ,GL_FILL)
   call glPolygonMode(GL_BACK             ,GL_FILL)

   call glEnable(GL_CULL_FACE)   !-----
   call glCullFace(GL_BACK)
   call Colors3D(2)
   call glBegin(GL_Triangles)
    !Fuselage:
     call glVertex3fv( P1*SizeU)
     call glVertex3fv( P2*SizeU)
     call glVertex3fv( P3*SizeU)
   call glEnd
   call glDisable(GL_CULL_FACE)                                   ;call glFlush
                                 !-----
  !call Colors3D(nCol)
  !call Colors3D( 12 )
   call Colors3D(  4 )
   call glBegin(GL_Triangles)
    !Tail:
     call glVertex3fv( P4*SizeU)
     call glVertex3fv( P5*SizeU)
     call glVertex3fv( P6*SizeU)
   call glEnd

   call Colors3D(3)
  !call Colors3D(5)  !...didn't help RC clarity.
   call glLineWidth(4.)
   call glBegin(GL_LINE_STRIP)
    !Fuselage:
     call glVertex3fv( P1*SizeU)
     call glVertex3fv( P2*SizeU)
     call glVertex3fv( P3*SizeU)
     call glVertex3fv( P1*SizeU)
   call glEnd
   call glBegin(GL_LINE_STRIP)
    !Tail:
     call glVertex3fv( P4*SizeU)
     call glVertex3fv( P5*SizeU)
     call glVertex3fv( P6*SizeU)
     call glVertex3fv( P4*SizeU)
   call glEnd

 !--Draw colored axes:
   call glLineWidth(3.)
  !--
   call Colors3D(11) !Green
   call glBegin(GL_LINES)
     call glVertex3fv( Po*SizeU); call glVertex3fv( Px*SizeU)
   call glEnd
  !--
   call Colors3D( 9) !Yellow
   call glBegin(GL_LINES)
     call glVertex3fv( Po*SizeU); call glVertex3fv( Py*SizeU)
   call glEnd
  !--
   call Colors3D( 7) !Red
   call glBegin(GL_LINES)
     call glVertex3fv( Po*SizeU); call glVertex3fv( Pz*SizeU)
   call glEnd

   if(nCol>0) then
    !--Axis labels:
    !              XYZRPYwC=(/   X ,   Y  , Z   , Roll, Pitch, Yaw,width Ch/)
             !XYZRPYwC     =(/ .8d0, .02d0, .0d0,   0.d0,  0.d0,  0.d0 ,.1d0 /)
              XYZRPYwC     =(/1.2d0, .02d0, .0d0,   0.d0,  0.d0,180.d0 ,.1d0 /)
              XYZRPYwC(1:3)=XYZRPYwC(1:3)*SizeU
    !call VecFont7(XYZRPYwC,LineWidth,iCol,Label)
     PText="+X" ; call VecFont7(XYZRPYwC,  2.     , 11 ,PText) !Green
              XYZRPYwC     =(/-.02d0, .4d0, .0d0,   0.d0,  0.d0,+90.d0 ,.1d0 /)
              XYZRPYwC(1:3)=XYZRPYwC(1:3)*SizeU
     PText="+Y" ; call VecFont7(XYZRPYwC,  2.     ,  9 ,PText) !Yellow
             !XYZRPYwC     =(/ .00d0, .02d0, .4d0,   0.d0,-90.d0,  0.d0 ,.1d0 /)
             !XYZRPYwC     =(/ .08d0, .00d0,1.2d0,+180.d0,+90.d0,  0.d0 ,.1d0 /)
              XYZRPYwC     =(/-.20d0,-.50d0, .6d0, -90.d0,  0.d0,+90.d0 ,.1d0 /)
              XYZRPYwC(1:3)=XYZRPYwC(1:3)*SizeU
     PText="+Z" ; call VecFont7(XYZRPYwC,  2.     ,  7 ,PText) !Red
                                              call glFlush;call CheckGL(+70658)
   endif                                                                ;return
End Subroutine WirePlane
!---------------------------------------------------------------------------7-9

Subroutine VecFont7(XYZRPYwC,LineWidth,iCol,Label)
!2020.06.12.0630cdt JMS- Vector-based character string writing in ~7DoF.
!                          using an OpenGL vector font.
!A standard 6DoF + scale operation with an unusual scale.
!XYZRPYwC(7)=(X, Y, Z, Roll, Pitch, Yaw, width of Character)
!  LineWidth: r*4 pixels, color index:i*4 iCol, & the Label*80.

 use OpenGLRec,only:  & !Ref: OpenGL GL/GLU/GLUT docs
      glGetIntegerv,GL_MATRIX_MODE,glMatrixMode,GL_MODELVIEW,glPushMatrix   &
     ,glMultMatrixd,glLineWidth,glShadeModel,GL_FLAT,glBegin,glEnd,GL_LINES &
     ,glutStrokeCharacter,GLUT_STROKE_MONO_ROMAN,glPopMatrix,glFlush        &
     ,glLoadIdentity,GL_PROJECTION,glOrtho,glGetDoublev,GL_MODELVIEW_MATRIX
 use ioDef     ,only: Up
 use ScreenDef ,only: vScrnHVD !screen & colors
!-----
 implicit none                                                       !arguments
  real(8)   ::XYZRPYwC(7) !(X,Y,Z,Roll,Pitch,Yaw,height of Character)
  real(4)   ::LineWidth  !Width of character strokes, in pixels
  integer(4)::iCol       !Text color
  character::Label*80   !Text to be written
  !--                                                                !internals
  integer(4)::i,iCharL,MtxMode(1),n,nChar
  real(8)   ::XyzRpySL(7)
  real(8)   ::Model7h(4,4)
  character::A*80,A80(80)*1;   equivalence(A,A80)
! real(8)   ::XyzRpySDecode(7) !(X,Y,Z,Roll,Pitch,Yaw,height of Character)
  !-----                                                              !end defs

  !--Adapt ModelView:
  !if(Up>5) write(Up,"(/'VecFont7: ',80a1)") (Label(i:i),i=1,len_trim(Label))
   call glGetIntegerv(GL_MATRIX_MODE,MtxMode)
   call glMatrixMode(GL_MODELVIEW ); call glPushMatrix !; call glLoadIdentity
   XyzRpySL   = XYZRPYwC
  !XyzRpySL(4)=0.d0              !no roll - 2021.10.03
   XyzRpySL(4)=XyzRpySL(4)-90.d0 !From: +X:along text +Y:up 
                                 ! To : +X:along text -Z:up          2022.05.10
   XyzRpySL(7)= XYZRPYwC(7)/110.d0 !scales size to user units in modelspace.
   call hFS7Gen(Model7h,XyzRpySL,0) !,Up)

!  if(Up>5) call Decode7DoF(     Model7h,0,XyzRpySDecode, 0)!,Up)
                                                    call glMultMatrixd(Model7h)
  !--Render the text
   call Colors3D(iCol)
   if(LineWidth>0.) call glLineWidth(LineWidth)                     !2021.10.03
   call glShadeModel(GL_FLAT)
   if(iCol>0) then;call glBegin(GL_LINES);call glEnd;call glFlush !Quirk fix?
   endif !iCol>0)
   A=char(0); A=Label; nChar=len_trim(A)
   do n=1,nChar;         i=ichar(A80(n))             ;if((n<1).or.(n> 80)) exit
     call PrintableIchar(i,iCharL)                          ;if(iCharL==0) exit
     call glutStrokeCharacter(GLUT_STROKE_MONO_ROMAN,iCharL)
   enddo                                                          ;call glFlush
  !Restore MODELVIEW, & MatrixMode:
   call glMatrixMode(GL_MODELVIEW ); call glPopMatrix             ;call glFlush
   call glMatrixMode(  MtxMode(1))                                ;call glFlush
   return
End Subroutine VecFont7
!---------------------------------------------------------------------------7 9

Subroutine QFromRpy(Q,Rpyh)
!2020.05.29.1010cdt JMS- Computes the quaternion of an SFS rotation. (McAir)
 implicit none
 !--Arguments
  real(8)  ::Q(4)    !Quaternion rotation
  real(8)  ::Rpyh(4) !homogeneous (Roll,Pitch,Yaw,W) SFS coordinates        deg
 !--Internals
  real(8)  ::RollD2,PitchD2,YawD2 !Normalized angles, /2.
  real(8)  ::Sy,cY,sP,cP,sR,cR    !Sines & cosines of the normalized angles
  !--EndDefs-----------------------------------------------
   RollD2 = Rpyh(1)/(2.d0*Rpyh(4)); sR=dsind(RollD2 ); cR=dcosd(RollD2 )
   PitchD2= Rpyh(2)/(2.d0*Rpyh(4)); sP=dsind(PitchD2); cP=dcosd(PitchD2)
   YawD2  = Rpyh(3)/(2.d0*Rpyh(4)); sY=dsind(YawD2  ); cY=dcosd(YawD2  )
  !--
   Q(1)=cY*cP*cR+sY*sP*sR
   Q(2)=sY*cP*cR-cY*sP*sR
   Q(3)=cY*sP*cR+sY*cP*sR
   Q(4)=cY*cP*sR-sY*sP*cR

!
!  Q(1)=cY*cP*cR+sY*sP*sR  !same
!  Q(2)=cY*cP*SR-sY*sP*cR  !prev Q(4)
!  Q(3)=cY*sP*cR+sY*cP*sR  !same
!  Q(4)=SY*cP*sR-cY*sP*sR  !prev Q(2)
                                                                         return
End Subroutine QFromRpy
!---------------------------------------------------------------------------7-9

Subroutine QoutFromQoQi(Qout,Qo,Qi)
!2020.05.29.1010cdt JMS- Sequential gimbal multiplication.
!                      - Qout <-hodpodge(Qouter <- Qinner)
 implicit none
 !--Arguments
  real(8)  ::Qout(4) !Quaternion- composite (~concatenated) rotation
  real(8)  ::Qo(4)   !          - gimbal- outer
  real(8)  ::Qi(4)   !          -       - inner
 !--Internals
  real(8)  ::Q(4)     !Isolates the output from the inputs.
  !--EndDefs-----------------------------------------------
   Q(1) = -Qo(4)*Qi(4) -Qo(3)*Qi(3) -Qo(2)*Qi(2) +Qo(1)*Qi(1)
   Q(2) = -Qo(3)*Qi(4) +Qo(4)*Qi(3) +Qo(1)*Qi(2) +Qo(2)*Qi(1)
   Q(3) = +Qo(2)*Qi(4) +Qo(1)*Qi(3) -Qo(4)*Qi(2) +Qo(3)*Qi(1)
   Q(4) = +Qo(1)*Qi(4) -Qo(2)*Qi(3) +Qo(3)*Qi(2) +Qo(4)*Qi(1)
  !--
   Qout(1) = Q(1)
   Qout(2) = Q(2)
   Qout(3) = Q(3)
   Qout(4) = Q(4)                                                       ;return
End Subroutine QoutFromQoQi
!---------------------------------------------------------------------------7-9

Subroutine QtoRpy(Q,Rpyh)
!2023.05.03.1405cdt JMS- Sets roll=0. deg. within .1 deg. of "up" or "down"
!2020.05.29.1010cdt JMS- Updates Manual Modelspace PoI 7DoF.
 implicit none
 !--Arguments
  real(8)  ::Q(4)    !Quaternion rotation input
  real(8)  ::Rpyh(4) !homogeneous (Roll,Pitch,Yaw,W), SFS coordinates       deg
 !--Internals
  real(8)  ::A11,A21,A31,A12,A22,A32,A13,A23,A33
  real(8)  ::sP

  !--EndDefs-----------------------------------------------
 !These are (agree with) the SFS Euler direction cosines:           2023.05.05
   A11=       Q(1)*Q(1)-Q(2)*Q(2)-Q(3)*Q(3)+Q(4)*Q(4)
   A21=2.D0*( Q(1)*Q(2)          +Q(3)*Q(4)           )
   A31=2.D0*(-Q(1)*Q(3)+Q(2)*Q(4)                     )
  !--
   A12=2.D0*(-Q(1)*Q(2)          +Q(3)*Q(4)           )
   A22=       Q(1)*Q(1)-Q(2)*Q(2)+Q(3)*Q(3)-Q(4)*Q(4)
   A32=2.D0*(           Q(2)*Q(3)          +Q(4)*Q(1) )
  !--
   A13=2.D0*( Q(1)*Q(3)+Q(2)*Q(4)                     )
   A23=2.D0*(           Q(2)*Q(3)          -Q(4)*Q(1) )
   A33=       Q(1)*Q(1)+Q(2)*Q(2)-Q(3)*Q(3)-Q(4)*Q(4)
  !--
  !Quaternion direction cosine printout:
  !write(iP,"(3f20.9)") A11,A12,A13
  !write(iP,"(3f20.9)") A21,A22,A23
  !write(iP,"(3f20.9)") A31,A32,A33

  !Previous solution:
!  Rpyh(1)=datan2d(A32,A33)
!  Rpyh(2)=dasind(-A31)
!  Rpyh(3)=datan2d(A21,A11)
!  Rpyh(4)=1.d0

  !Revised solution: Within .1 degree of "Up" or "Down"... Roll=0. degrees
                             sP = -A31
           RPYh(2) =  dasind(sP)        !Pitch

   if(dabs(sP)<.999998476d0)  &              !sin(89.9deg)=.999998476...
     then; RPYh(1) =  datan2d(A32,A33)  !Roll
           RPYh(3) =  datan2d(A21,A11)  !Yaw
     else; RPYh(1) =  0.d0              !Roll
           RPYh(3) =  datan2d(-A12,A22) !Yaw
   endif!|sP|<1.
           Rpyh(4)=1.d0                                                   ;return
End Subroutine QtoRpy
!---------------------------------------------------------------------------7-9

Subroutine QToQinverse(Q,Qinverse)
!2020.05.29.1010cdt JMS- UnRotates Q.
 implicit none
 !--Arguments
  real(8)  ::Q(4)         !Quaternion- input
  real(8)  ::Qinverse(4)  !          - inverse
 !--Internals
  !--EndDefs-----------------------------------------------
   Qinverse(1) = +Q(1)
   Qinverse(2) = -Q(2)
   Qinverse(3) = -Q(3)
   Qinverse(4) = -Q(4)
End Subroutine QToQinverse
!---------------------------------------------------------------------------7-9

Subroutine QtoVhatAngle(Q,Vhat,Angle,iP)
!2023.08.22.1545cdt JMS- Conversion: quaternion -> "Jeffternion"
!2020.05.29.1010cdt JMS- Computes a quaternion's unit vector & rotation angle.
!                      - Ref,: D.H.Eberle 3D Game Engine Design P.16-17
 implicit none
 !--Arguments
  real(8)   ::Q(4)    !Quaternion rotation
  real(8)   ::Vhat(3) !Rotation axis unit vector
  real(8)   ::Angle   !Rotation angle around Vhat
  integer(4)::iP      ! >5: Printing enable, use: write(iP,...)
 !--Internals
  real(8)  ::sA
  !--EndDefs-----------------------------------------------
   if(     Q(1)==+1.d0) then !No rotation
     Angle   = 0.d0
     Vhat    = 0.d0
    else                     !Compute the rotation axis unit vector & angle.
     Angle   = 2.d0*dacosd(Q(1));  sA=dsind(Angle/2.d0)!Q(1)=    cos(a/2)
    !if(dabs(Angle-180.d0)<1.d-20) sA=1.d0
     Vhat(3) =             Q(2) / sA               !Q(2)=Vhat(1)*sin(a/2)
     Vhat(2) =             Q(3) / sA               !Q(3)=Vhat(2)*sin(a/2)
     Vhat(1) =             Q(4) / sA               !Q(4)=Vhat(3)*sin(a/2)
   endif! Q(1)==+1.d0 
   if(iP<6)                                                              return
   write(iP,"(4f15.9,' Q(1:4) Quaternion')") Q
   write(iP,"(4f15.9,' cw:X,Y,Z,Angle>=0.',/,66x,'= Jeffternion')") Vhat,Angle
                                                                         return
End Subroutine QtoVhatAngle
!---------------------------------------------------------------------------7-9

