!Sa-Mathh8Mod.f95   Auxiliary         Stereo-3D Simulation Environment Vsn:1.00
!2025.05.24.1840dt JMS- Basic real(8) homog. vector math

!            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...
!Module hVecMath8Mod
! contains
!  Subroutine hAdd(Vouth,V1h,V2h)
!  Subroutine hSubtract(Vouth,V1h,V2h)
!  Subroutine hCross(Vouth,V1h,V2h)
!  Subroutine hDot(V1h,V2h,DotP)
!  Subroutine hVnorm(Vinh,Vouth,Vmag)
!  Subroutine hPointPolar(Ph,aRpyh,PDmag)
!End Module hVecMath8Mod

!---------------------------------------------------------------------------7-9
Module hVecMath8Mod
!use hVecMath8Mod                                                   !2013.10.23
!use EnvJmsMod, only:FaultReport
!---
Implicit none
!---
public ::        &
! real(8) homogeneous math:
   hAdd          & !                                                 2013.09.30
  ,hSubtract     & !                                                 2013.09.30
  ,hCross        & !                                                 2013.10.03
  ,hDot          & !                                                 2013.10.03
  ,hVnorm        & !                                                 2013.10.03
  ,hPointPolar     !
!---
 contains
!---------------------------------------------------------------------------7 9
!                    ///// real(8) homogeneous math: /////
!---------------------------------------------------------------------------7 9
Subroutine hAdd(Vouth,V1h,V2h)
!2013.09.30.1650cdt JMS- Homogeneous vector addition:  Vouth=V1h+V2H
!                      - Traveler2/Athlon64/WinXPPro/APF9.0/Ogl1.2.1
 implicit none
  real(8)   ::Vouth(4)  !Output vector  (X,Y,Z,1.)                    !arguments
  real(8)   ::V1h(4)    !Input vector#1
  real(8)   ::V2h(4)    !Input vector#2
 !                                                                   !internals
  integer(4)::i
  real(8)   ::vTemph(4)    !Input vector#2
  real(8)   ::V1Scale
  real(8)   ::V2Scale
  !-----
   V1Scale=V1h(4) ;if(V1Scale.eq.0.d0) V1Scale=1.d0
   V2Scale=V2h(4) ;if(V2Scale.eq.0.d0) V2Scale=1.d0
   do i=1,3 ;vTemph(i)=V1h(i)/V1Scale+V2h(i)/V2Scale; enddo
             vTemph(4)=1.d0
   if(V1h(4)*V2h(4).eq.0.d0) then !~~~~~~~~~~~~~~~~
    !call jPause("hAdd: V1h(4)*V2h(4)=0.")                    !Diagnostics
     stop             "hAdd: V1h(4)*V2h(4)=0." !~~~
     vTemph(4)=0.d0
   endif !V1h(4)*V2h(4)=0.
   do i=1,4 ;Vouth(i)=vTemph(i) ;enddo!i
   return
End Subroutine hAdd
!---------------------------------------------------------------------------7 9
Subroutine hSubtract(Vouth,V1h,V2h)
!2013.09.30.1700cdt JMS- Homogeneous vector Subtraction:  Vouth=V1h-V2H
!                      - Traveler2/Athlon64/WinXPPro/APF9.0/Ogl1.2.1
 implicit none
  real(8)   ::Vouth(4)  !Output vector  (X,Y,Z,1.)                    !arguments
  real(8)   ::V1h(4)    !Input vector#1
  real(8)   ::V2h(4)    !Input vector#2
 !                                                                   !internals
  integer(4)::i
  real(8)   ::vTemph(4)    !Input vector#2
  real(8)   ::V1Scale
  real(8)   ::V2Scale
  !-----
   V1Scale=V1h(4) ;if(V1Scale.eq.0.d0) V1Scale=1.d0
   V2Scale=V2h(4) ;if(V2Scale.eq.0.d0) V2Scale=1.d0
   do i=1,3 ;vTemph(i)=V1h(i)/V1Scale-V2h(i)/V2Scale; enddo
                             vTemph(4)=1.d0
   if(V1h(4)*V2h(4).eq.0.d0) then !~~~~~~~~~~~~~~~~~~~
    !call jPause("hSubtract: V1h(4)*V2h(4)=0.")               !Diagnostics
     stop             "hSubtract: V1h(4)*V2h(4)=0."!~~
     vTemph(4)=0.d0
   endif !V1h(4)*V2h(4)=0.
   do i=1,4 ;Vouth(i)=vTemph(i) ;enddo!i
   return
End Subroutine hSubtract
!---------------------------------------------------------------------------7 9
Subroutine hCross(Vouth,V1h,V2h)
!2013.10.03.2155cdt JMS- Cross product of homogeneous vectors: Vouth=V1h X V2h
!                      - Note: Vouth is isolated from V1h & V2h.
!                      - Traveler2/Athlon64/WinXPPro/APF9.0/Ogl1.2.1
implicit none                                                        !arguments
  real(8)   ::V1h(4)     !Vector- input- #1 
  real(8)   ::V2h(4)     !             - #2
  real(8)   ::Vouth(4)   !      - output cross product of = V1h X V2h
 !---                                                                !internals
  real(8)   ::Temph(4)   !Isolates Vouth from V1h & V2h.
  !----- 
   Temph(1)=V1h(2)*V2h(3)-V1h(3)*V2h(2)
   Temph(2)=V1h(3)*V2h(1)-V1h(1)*V2h(3)
   Temph(3)=V1h(1)*V2h(2)-V1h(2)*V2h(1)
   Temph(4)=V1h(4)*V2h(4)
   if(Temph(4).ne.0.d0) then
     Temph(1:3)=Temph(1:3)/Temph(4) ;Temph(4)=1.d0
    else !~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    !call jPause("hCross: Temph(4)<>0.")                     !Diagnostics
     stop             "hCross: Temph(4)=0."
   endif !Temph(4)<>0.d0 ~~~~~~~~~~~~~~~~~~~~
   Vouth=Temph
   return
End Subroutine hCross
!---------------------------------------------------------------------------7 9
Subroutine hDot(V1h,V2h,DotP)                                 !2013.11.22.0850
!2013.11.22.0850cdt JMS- zero (4)th components are permitted.
!2013.10.03.2155cdt JMS- Cross product of homogeneous vectors: Vouth=V1h X V2h
!                      - Note: DotP is isolated from V1h & V2h.
!                      - Traveler2/Athlon64/WinXPPro/APF9.0/Ogl1.2.1
 implicit none                                                       !arguments
  real(8)   ::V1h(4)     !Vector- input- #1 
  real(8)   ::V2h(4)     !             - #2
  real(8)   ::DotP       !      - Dot product = V1h .dot. V2h
  !---                                                               !internals
  real(8)   ::Temp       !Isolates DotP from V1h() & V2h().
  !----- 
  Temp=V1h(1)*V2h(1)+V1h(2)*V2h(2)+V1h(3)*V2h(3)
  if(V1h(4).ne.0.d0) Temp=Temp/v1h(4)
  if(V2h(4).ne.0.d0) Temp=Temp/v2h(4)
 !if(V1h(4)*V2h(4).eq.0.d0)then !~~~~~~~~~~~~~
 !  call jPause("hDot: V1h(4)*V2h(4)=0.")                     !Diagnostics
 ! !stop             "hDot: V1h(4)*V2h(4)=0."
 !endif !V1h(4)*V2h(4)=0. ~~~~~~~~~~~~~~~~~~~~
  DotP=Temp
  return
End Subroutine hDot
!---------------------------------------------------------------------------7 9
Subroutine hVnorm(Vinh,Vouth,Vmag)                            !2013.11.22.0715
!2013.11.22.0715cst JMS- Improved the fault logic to pass Vinh(4)=0. inputs.
!2013.11.18.0840cst JMS- Comment corrections
!2013.10.03.2315cdt JMS- Normalizes a homogeneous vector: Vouth=norm(Vinh)
!                      - Traveler2/Athlon64/WinXPPro/APF9.0/Ogl1.2.1
 implicit none                                                       !arguments
  real(8)   ::Vinh(4)    !Vector- input
  real(8)   ::Vouth(4)   !      - output- normalized
  real(8)   ::Vmag       !Length of Vinh (homogeneous)
  !---                                                               !internals
  real(8)   ::Temph(4)   !Isolates Vouth from Vinh.
  real(8)   ::TempMag
  !-----
                            Temph=Vinh
  if(Vinh(4).ne.0.d0) Temph=Temph/Vinh(4)
  TempMag=Temph(1)*Temph(1)+Temph(2)*Temph(2)+Temph(3)*Temph(3)
  if(TempMag.gt.0.d0) then
    TempMag=dsqrt(TempMag)
    Temph(1:3)=Temph(1:3)/TempMag
   else
    Temph=(/0.d0,0.d0,0.d0,1.d0/); TempMag=0.d0
  endif !TempMag>0.
  if((TempMag.eq.0.d0).and.(Temph(4).eq.0)) then !~~~~~
   !call jPause("hVnorm: Vinh is a null vector.")            !Diagnostics
    stop             "hVnorm: Vinh is a null vector."
  endif !TempMag>0 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  Vouth=TempH ;Vmag=TempMag
  return
End Subroutine hVnorm
!---------------------------------------------------------------------------7 9
Subroutine hPointPolar(Ph,aRpyh,PDmag)
!2013.10.01.2155cdt JMS- Expresses a 3D point in FS ~polar coordinates.
!                      - Traveler2/Athlon64/WinXPPro/APF9.0/Ogl1.2.1
 !-----
 Implicit none                                                       !arguments
  real(8)   ::Ph(4)    !Input - Point#1
  real(8)   ::aRpyh(4) !Output- att.(Roll,Pitch,Yaw)-                  [degrees]
  real(8)   ::PDmag    !      - Distance to point
  !---                                                               !internals
  real(8)   ::Ptemph(4)
  !-----
   Ptemph=Ph
   call hVnorm(Ph,Ptemph,PDmag)
   aRpyh=(/0.d0,0.d0,0.d0,1.d0/)
   aRpyh(2)=-dasind(Ptemph(3))
   if((Ptemph(1).ne.0.d0).or.(Ptemph(2).ne.0.d0))  &
   aRpyh(3)=datan2d(Ptemph(2),Ptemph(1))
   return
End Subroutine hPointPolar
!---------------------------------------------------------------------------7 9
End Module hVecMath8Mod
!---------------------------------------------------------------------------7 9

