Attribute VB_Name = "Module3"
'******************************************************************************
'*    関数ライブラリ　ＳＳＳＬＩＢ : Space Scientific Subprogram Library      *
'******************************************************************************
'
'コンフィギュレーション識別番号：Lib-Ver24.0
'  配布用として不要な関数削除
'　2018/11/27　　原作者
'
' Actand      逆正接　tan−１（s／c）　[0〜360deg]
' AngMat      多段階の回転を表す方向余弦マトリクスの計算
' AngQtn      多段階の回転を表す四元数の計算
' Arcosd      逆余弦　cos−１（c）　   [0〜180deg]
' Arsind      逆正弦　sin−１（s）　   [-90〜90deg]
' Artand      逆正接　tan−１（s／c）　[-180〜180deg]
' AryCpy      １次元配列のコピー
' Azimth      方位角
' BinDgt      ２進数を各ビット列に分解する
' CarGeo      カルテシアン座標系から測地座標系へ変換
' Cd7Wgs      Ｃ−７系からＷＧＳ８４系へ変換
' Cosd        角度（度）の余弦
' DcmQtn      方向余弦マトリクスから四元数へ変換
' DcmRav      方向余弦マトリクスから回転角ベクトルへ変換
' Deg         ラジアンから度へ単位変換
' Derivs      微係数評価
' xxx_Derivs  微係数評価（xxx用）-----Module5に置く
'   ID= 1: Demo_AttitudeIntegrations_Derivs         姿勢パラメータ積分の演習（四元数、方向余弦マトリクス、オイラー角）
'   ID= 6: ableAndIntegTest_Derivs                  汎用関数テーブルの積分テスト
' Diff        差分
' DIP         大気中落下点（DIP）の計算
'   DIPr        大気中落下点（DIP）の地心半径ベクトル
'   DIPv        大気中落下点（DIP）の慣性速度ベクトル
'   DIPx        大気中落下点（DIP）の計算（DIP基本関数）
' DrvAng      回転角（オイラー角）の微係数
' DrvDcm      方向余弦マトリクスの微係数
' DrvQtn      四元数の微係数
' DrvRav      回転角ベクトルの微係数
' er          地球離心率
' GeoCar      測地座標系からカルテシアン座標系へ変換
' GeoDes      測地線に沿った距離の計算
' GeoGra      データ型「GeoGraphicQuantities」の生成
' GeoPos      地理関連諸量の計算
' GeoXyz      地理データから慣性座標系の飛行状態量の計算
' Gmst        グリニッジ平均恒星時（平均春分点からグリニッジまでの赤経）
' GrmSmt      グラム･シュミットの正規直交化
' Gmu         地球重力定数(m^3/s^2)
' GrnLng      グリニッジ平均恒星時（平均春分点からグリニッジまでの赤経）
' GrvAcc      地球重力加速度
' IIP         真空中瞬時落下点（IIP）の計算
'   IIPr        真空中瞬時落下点（IIP）の地心半径ベクトル
'   IIPv        真空中瞬時落下点（IIP）の速度ベクトル
'   IIPx        真空中瞬時落下点（IIP）の計算（IIP基本関数）
' IntIni      積分制御の初期化
' LatCtG      地心緯度から測地緯度へ変換
' LatGtC      測地緯度から地心緯度へ変換
' LchPad      射点位置データ
' LclPos      地表面位置ベクトルの計算
' LeepSec     閏秒の検索
' Lneqsl      線形方程式を解く
' LokAng      ルックアングル計算
' LsqBep      最小自乗法による最適な次数での多項式近似係数を求める
' LsqPol      最小自乗法による多項式近似係数を求める
' MatAng      ある視点の座標系から見た２つの方向余弦マトリクス間の回転角
' MatAry      ３ｘ３マトリクスを１次元配列に変換
' MatRtn      マトリクスの回転
' Mjd         修正ユリウス日の計算
' Mx31Mp      マトリクスとベクトルの乗算
' Mx33        ９要素から３ｘ３マトリクスの生成
' Mx33Dt      マトリクスのデターミナント
' Mx33Iv      逆マトリクス
' Mx33Mp      マトリクスの乗算
' Mx33Tp      マトリクスの転置
' Mx33V3      ３ベクトルから３ｘ３マトリクスの生成
' NrmVel      軌道面外速度
' OdrAng      回転角（回転順）　[OdrAng.x]i･[OdrAng.y]j･[OdrAng.z]k=A
' OrbElm      軌道要素
' OrbXyz      軌道要素から慣性座標系位置及び速度の計算
' PolyEx      多項式展開
' Qtn         四元数の生成
' QtnAng      ある視点の座標系から見た２つの四元数間の回転角
' QtnAry      四元数を１次元配列に変換
' QtnCnj      共役四元数
' QtnDcm      四元数から方向余弦マトリクスへ変換
' QtnRav      四元数から回転角ベクトルへ変換
' QtnRtn      四元数の回転
' QtqqMp      四元数の乗算
' QtqvMp      四元数とベクトルの乗算
' quickSort   クイック･ソート
' Rad         度からラジアンへ単位変換
' RavDcm      回転角ベクトルから方向余弦マトリクスへ変換
' RavQtn      回転角ベクトルから四元数へ変換
' Rer         地球赤道半径
' RkgInt      ルンゲクッタ･ギル法による数値積分
' RtnAng      回転角（オイラー角）　[RtnAng.i]i･[RtnAng.j]j･[RtnAng.k]k=A
' RtnMat      回転マトリクス　RtnMat=[A]ix
' RtnQtn      回転を表す四元数
' SetPad      射点位置の設定
' SetRef      慣性座標系基準の設定
' SetTime     発射日時の設定
' Sind        角度（度）の正弦
' Site        データ型「SiteLocation」の生成
' Sqrt        平方根の計算
' StaVar      データ型「StateVariables」の生成
' StdRnd      正規乱数の発生
' TblInt      汎用テーブルの補間
' Vct3D       ３次元ベクトルの生成
' VctAdd      ベクトルの加算
' VctAng      ２つのベクトルの間の角度
' VctAry      ３次元ベクトルを１次元配列に変換
' VctCrp      ベクトルの外積
' VctInp      ベクトルの内積
' VctMag      ベクトルの絶対値
' VctNrm      ベクトルの正規化
' VctRtn      ベクトルの回転　VctRtn=[A]ix・V
' VctSmp      ベクトルとスカラーの積
' VctSub      ベクトルの減算
' Wer         地球自転レート(deg/s)
' WgsCd7      ＷＧＳ８４系からＣ−７系へ変換

Option Explicit

'*********************************　型の定義　**********************************
    
'３次元ベクトル
    Public Type Vector3D
        x As Double
        y As Double
        z As Double
    End Type

'ｎ次元ベクトル
    Public Type VectorND
        a() As Double
    End Type

'３X３マトリクス
    Public Type Matrix3x3
        x As Vector3D
        y As Vector3D
        z As Vector3D
    End Type

'四元数
    Public Type Quaternion
        c As Double
        s As Vector3D
    End Type

'日付
    Public Type yymmdd
        Yr As Double
        Mn As Double
        Dy As Double
    End Type

'時刻
    Public Type hhmmss
        hr As Double
        min As Double
        sec As Double
    End Type

'汎用テーブル
    Public Type Table
        Fnam As String      '関数名
        Anam As String      '引数名
        Nf As Integer       '関数の数
        Intp As String      '補間方法、"ステップ" or "１次補間"
        fEnd As String      'テーブル前端点の処理、"ゼロ" or "一定" or "外挿"
        bEnd As String      'テーブル後端点の処理、"ゼロ" or "一定" or "外挿"
        cInt As String      '節点処理、"積分" or "補間"
        Mlt() As Double     '引数及び関数の乗数
        Add() As Double     '引数及び関数の加数
        nD As Integer       'データ数（引数データの個数）
        ix As Integer       'ポインタ
        ixp As Integer      '前回のポインタ
        jDis As Integer     '不連続点処理　=0：不連続点から遠い、　≠0：処理中
        arg As Double       '前回の引数値
        slp As Double       '引数勾配の逆数
        slpP As Double      '前回の引数勾配の逆数
        slpM As Double      '引数勾配逆数の乗数
        slpN As Integer     '引数勾配逆数乗数の変更回数
        slpT As Double      '引数勾配逆数を前回求めた時刻
        slpArg As Double    '引数勾配逆数を前回求めた引数
        aDis As Double      '次の引数節点（不連続点）
        tDis As Double      '次の引数節点の時刻（次の不連続点到達時刻）
        Val() As Double     '前回の関数値、配列（1〜Nf）
        Data() As Double    'データ、配列（1〜Nd、0〜Nf）
    End Type

'積分制御変数
    Public Type IntegrationControlVariables
        ID As Integer       '微係数計算関数の識別
        nInt As Integer     '積分変数の個数
        jIms As Integer     '積分計算の内部ステップ番号(=0：不連続点処理）
        t As Double         '時刻
        dti As Double       '通常の積分刻み
        dt As Double        '実際の積分刻み
        tDis As Double      '次の不連続点までの時間（積分ループから抜けるまでの時間）
    End Type

'地上局位置データ
    Public Type SiteLocation
        alt As Double       '基準楕円面からの高度（m）
        Ltg As Double       '測地緯度（deg）
        lng As Double       '経度（deg）
        Azl As Double       '局設置方位角（deg）
        lat As Double       '天文緯度（deg）
        Gsp As Double       '基準楕円面からのジオイドのずれ（m）
        Def As Double       '東西方向の鉛直からの傾き（deg）
    End Type

'地上局からの視野情報
    Public Type ViewQuantities
        r As Vector3D       '地心半径ベクトル（m）
        Rg As Double        '直距離（m）
        Az As Double        '方位角（北から時計回りを正方向）（deg）
        El As Double        '上下角（deg）
        Tla As Double       'トータルルックアングル（deg）
        Rla As Double       'ロールルックアングル（deg）
    End Type

'慣性座標系規定データ
    Public Type InertialFrame
        Y0Day As yymmdd     '発射年月日
        Xtime As hhmmss     '発射時刻
        ID As Integer       '慣性座標系基準点（０：射点、１：グリニッジ、２：春分点）
        t As Double         '慣性基準設定から計算開始基準までの時間（sec）
        Ilng As Double      '慣性基準設定時のグリニッジから慣性座標系基準点までの経度（deg）
        Glng As Double      '慣性基準設定時の春分点からグリニッジまでの経度（deg）
    End Type

'飛行状態量
    Public Type StateVariables
        r As Vector3D       '地心半径ベクトル（m）
        v As Vector3D       '慣性速度ベクトル（m/sec）
        qIB As Quaternion   '慣性座標系で表した機体座標系の四元数
        Dibm As Matrix3x3   '慣性座標系で表した機体座標系の方向余弦マトリクス
    End Type

'地理関連諸量
    Public Type GeoGraphicQuantities
        ri As Double        '地心半径（m）
        Vltc As Double      '地心緯度（deg）
        Vlni As Double      '慣性基準からの経度（deg）
        alt As Double       '高度（m）
        Vlat As Double      '測地緯度（deg）
        Vlng As Double      '経度（deg）
        Vi As Double        '慣性速度（m/sec）
        Fpvi As Double      '慣性速度上下角（deg）
        Azvi As Double      '慣性速度方位角（deg）
        Atli As Double      '機軸上下角（deg）
        Amis As Double      '機軸方位角（deg）
        Rlat As Double      '機軸ロール角（バンク角）（deg）
    End Type

'軌道要素
    Public Type OrbitalElements
        a As Double         '軌道長半径（m）
        ecc As Double       '離心率
        ainc As Double      '軌道傾斜角（deg）
        asnd As Double      '昇降点経度（deg）
        argp As Double      '近地点引数（deg）
        argl As Double      '緯度引数（deg）
        apf As Double       '遠地点高度（m）
        prf As Double       '近地点高度（m）
        Raan As Double      '昇降点赤経（deg）
        ant As Double       '真近点離角（deg）
        ane As Double       '離心近点離角（deg）
        anm As Double       '平均近点離角（deg）
        apg As Double       '遠地点半径（m）
        prg As Double       '近地点半径（m）
        vapf As Double      '遠地点速度（m/s）
        vprf As Double      '近地点速度（m/s）
        envv As Double      '軌道エネルギー（m^2/s^2）
        perd As Double      '周期（s）
        p As Double         '半通径（m）
        am As Vector3D      '軌道角運動量ベクトル（m^2/s）
        h As Double         '軌道角運動量（m^2/s）
        asndx As Double     'X軸から昇交点までの経度 (deg)
    End Type

'エンジン･モデル
    Public Type Engine
        Name As Variant                     'エンジン名
        Type As Integer                     '推進系タイプ
        gt1 As Table                        '推進系特性１（推力テーブルなど。推進系タイプにより異なる）
        gt2 As Table                        '推進系特性２（比推力テーブルなど。推進系タイプにより異なる）
    End Type
        
'ＩＭＵ誤差モデル
    Public Type ImuErrorModel
        Name As Variant                     '誤差名
        ix As Integer                       '誤差入力軸識別番号
        InitialAlignmentError As Boolean    '初期アライメント
        InitialAlignment(3) As Double
        AccBiasError As Boolean             '加速度計バイアス
        AccBias(3) As Double
        AccScaleFactorError As Boolean      '加速度計スケールファクタ
        AccScaleFactor(3) As Double
        AccNonLinearityError As Boolean     '加速度計スケールファクタ非直線性
        AccNonLinearity(3) As Double
        AccAlignmentError As Boolean        '加速度計ミスアライメント
        AccAlignment(6) As Double
        GroBiasError As Boolean             'ジャイロバイアス
        GroBias(3) As Double
        GroScaleFactorError As Boolean      'ジャイロスケールファクタ
        GroScaleFactor(3) As Double
        GroNonLinearityError As Boolean     'ジャイロスケールファクタ非直線性
        GroNonLinearity(3) As Double
        GroRandomWalkError As Boolean       'ジャイロランダムウォーク
        GroRandomWalk(3) As Double
        GroAlignmentError As Boolean        'ジャイロミスアライメント
        GroAlignment(6) As Double
    End Type

'機体誤差モデル
    Public Type BodyErrorModel
        Name As Variant                     '誤差名
        ix As Integer                       '誤差入力軸識別番号
        Nominal As Boolean                  'ノミナル
        ThrustVariationError As Boolean     '推力変動
        ThrustVariation As Double
        IspVariationError As Boolean        '比推力変動
        IspVariation As Double
        ThrustMisAlignmentError As Boolean  '推力ミスアライメント
        ThrustMisAlignment(3) As Double
        MassError As Boolean                '質量誤差
        Mass As Double
        PropellantError As Boolean          '推進薬搭載量誤差
        Propellant As Double
        TotalImpulseError As Boolean        'トータル･インパルス誤差
        TotalImpulse(3) As Double
        VelocityImpulseError As Boolean     '速度インパルス誤差
        VelocityImpulse(3) As Double
        AttitudeError As Boolean            '姿勢誤差
        Attitude(3) As Double
    End Type

'*******************************　定数の定義　*********************************

    Public Const vbTacr As Double = 0.000000001         '時間の精度（sec）
    Public Const vbTmax As Double = 1E+50               '時間の上限（sec）
    Public Const vbInf As Double = 1E+50                '無限大
    Public Const vbSmall As Double = 0.0000000001       '無限小
    Public Const vbMsS As Double = 0.999999999          '微小に小さくする倍率
    Public Const vbMsL As Double = 1.000000001          '微小に大きくする倍率

'角度の単位変換定数
    Public Const CDGRD As Double = 57.2957795130823     'ラジアンから度へ
    Public Const CRDDG As Double = 1.74532925199432E-02 '度からラジアンへ
    Public Const PID2  As Double = 1.57079632679489     'π/2（ 90度）
    Public Const Pi As Double = 3.14159265358979        'π   （180度）
    Public Const TOPI  As Double = 6.28318530717958     '2π  （360度）

'長さの単位変換定数
    Public Const CMTFT As Double = 0.3048               'フィートからメートルへ
    Public Const CMTNM As Double = 1851.99628584        '海里からメートルへ
    Public Const CFTMT As Double = 3.28083989501312     'メートルからフィートへ
    Public Const CFTNM As Double = 6076.1033            '海里からフィートへ
    Public Const CNMMT As Double = 5.39957886333684E-04 'メートルから海里へ
    Public Const CNMFT As Double = 1.64579163754507E-04 'フィートから海里へ

'質量・力の単位変換定数
    Public Const CKGLB As Double = 0.45359243           'ポンドからキログラムへ
    Public Const CKGNW As Double = 0.101971621297793    'ニュートンからキログラムへ
    Public Const CNWKG As Double = 9.80665              'キログラムからニュートンへ
    Public Const CNWLB As Double = 4.4482222036595      'ポンドからニュートンへ
    Public Const CLBKG As Double = 2.20462233022716     'キログラムからポンドへ
    Public Const CLBNW As Double = 0.224808913362582    'ニュートンからポンドへ

'地球モデル定数
    Public Const GMER As Double = 398600900000000#      '重力定数(m^3/s^2)
    Public Const AXER As Double = 6378142#              '赤道半径(m)
    Public Const ARER As Double = 6378142#              '平均半径(m)
    Public Const WEER As Double = 7.29211585489679E-05  '自転レート(rad/s)
    Public Const EER  As Double = 8.18194953208103E-02  '離心率
    Public Const GER2 As Double = 66063089804.0627      '重力加速度係数（２次）
    Public Const GER3 As Double = -329263942461111#     '重力加速度係数（３次）
    Public Const GER4 As Double = -1.64767870544296E+21 '重力加速度係数（４次）
    Public Const GER5 As Double = -9.103953345E+26      '重力加速度係数（５次）
    Public Const WEERD As Double = 4.17807462206018E-03 '自転レート(deg/s)
    Public Const EER2  As Double = 6.69442981455211E-03 '離心率の２乗
    Public Const ER12 As Double = 0.993305570185447     '１−離心率２乗
    Public Const JDW0 As Double = 6 / 24 * 360 + 41 / 24 / 60 * 360 _
                                + 50.54841 * 360 / 24 / 3600        '太陽恒星時計算用係数
    Public Const JDW1 As Double = 8640184.812866 * 360 / 24 / 3600  '太陽恒星時計算用係数
    Public Const JDW2 As Double = 0.093104 * 360 / 24 / 3600        '太陽恒星時計算用係数
    Public Const JDW3 As Double = 0.0000062 * 360 / 24 / 3600       '太陽恒星時計算用係数
    
'*************************　パブリック変数の定義　*****************************

'積分制御
    Public INTG As IntegrationControlVariables
    Public INDX() As Integer    'ORDN/DRVS内の積分する要素のインデックス
    Public DRVS() As Double     '微係数
    Public ORDN() As Double     '積分値
    Public TPI1() As Double     '中間データ
    Public Varb0() As Double     '関数Derivsと授受する実数型変数データ領域No.0
    Public Varb1() As Double     '関数Derivsと授受する実数型変数データ領域No.1
    Public Varb2() As Double     '関数Derivsと授受する実数型変数データ領域No.2
    Public Varb3() As Double     '関数Derivsと授受する実数型変数データ領域No.3
    Public Varb4() As Double     '関数Derivsと授受する実数型変数データ領域No.4
    Public iVrb0() As Integer    '関数Derivsと授受する整数型変数データ領域No.0
    Public iVrb1() As Integer    '関数Derivsと授受する整数型変数データ領域No.1
    Public iVrb2() As Integer    '関数Derivsと授受する整数型変数データ領域No.2
    Public iVrb3() As Integer    '関数Derivsと授受する整数型変数データ領域No.3
    Public iVrb4() As Integer    '関数Derivsと授受する整数型変数データ領域No.4
    Public Cons0() As Double     '関数Derivsと授受する実数型定数データ領域No.0
    Public Cons1() As Double     '関数Derivsと授受する実数型定数データ領域No.1
    Public Cons2() As Double     '関数Derivsと授受する実数型定数データ領域No.2
    Public Cons3() As Double     '関数Derivsと授受する実数型定数データ領域No.3
    Public Cons4() As Double     '関数Derivsと授受する実数型定数データ領域No.4
    Public iCns0() As Double     '関数Derivsと授受する整数型定数データ領域No.0
    Public iCns1() As Double     '関数Derivsと授受する整数型定数データ領域No.1
    Public iCns2() As Double     '関数Derivsと授受する整数型定数データ領域No.2
    Public iCns3() As Double     '関数Derivsと授受する整数型定数データ領域No.3
    Public iCns4() As Double     '関数Derivsと授受する整数型定数データ領域No.4
    Public Temp0() As Double     '実数型データ一次記憶領域No.0

'ルンゲクッタ･ギル法の数値積分定数
    Public RKG1(4) As Double
    Public RKG2(4) As Double
    Public RKG3(4) As Double
    Public RKG4(4) As Double

'慣性座標系規定データ
    Public INCO As InertialFrame

'飛行状態量
    Public Trj As StateVariables

'射点位置
    Public Cpad As SiteLocation

'テーブルデータ
    Public Table0 As Table      '汎用テーブル０
    Public Table1 As Table      '汎用テーブル１
    Public Table2 As Table      '汎用テーブル２
    Public Table3 As Table      '汎用テーブル３
    Public Table4 As Table      '汎用テーブル４
    Public Table5 As Table      '汎用テーブル５
    Public Table6 As Table      '汎用テーブル６
    Public Table7 As Table      '汎用テーブル７
    Public Table8 As Table      '汎用テーブル８
    Public Table9 As Table      '汎用テーブル９

'エンジン
    Public ENG As Engine
    
'ＩＭＵ誤差モデル
    Public IMU As ImuErrorModel

'機体誤差モデル
    Public BODY As BodyErrorModel

'大気モデル
    Public AM1976 As Table '大気モデル汎用テーブル
'
'******************************************************************************
    
    
    
    
Function Actand(s As Double, c As Double) As Double

'逆正接　tan−１（S／C）　[0〜360deg]

' s      ：入力、正弦（sin）
' c      ：入力、余弦（cos）
' Actand ：出力、逆正接[0〜360deg]

    If (c > 0#) Then
        If (s >= 0#) Then
            Actand = Atn(s / c) * CDGRD         '[0〜90)度
        Else
            Actand = Atn(s / c) * CDGRD + 360#  '(270〜360]度
        End If
    ElseIf (c < 0#) Then
            Actand = Atn(s / c) * CDGRD + 180#  '(90〜270)度
    Else
        If (s >= 0#) Then
            Actand = 90#                        '90度
        Else
            Actand = 270#                       '270度
        End If
    End If

End Function
Function AngMat(a() As Double, ix() As Integer) As Matrix3x3

'多段階の回転を表す方向余弦マトリクスの計算

' a      ：入力、回転角（deg）
' ix     ：入力、回転軸　１：第1軸（Ｘ軸）、　２：第2軸（Ｙ軸）、　３：第3軸（Ｚ軸）
' AngMat ：出力、３ｘ３マトリクス

    Dim m As Matrix3x3, n As Integer, i As Integer

    n = UBound(a)               '回転角の個数
    m = RtnMat(a(1), ix(1))     '一回の回転による方向余弦マトリクス
    If (n > 1) Then
        For i = 2 To n
            m = MatRtn(m, CDbl(a(i)), CInt(ix(i)))
        Next i
    End If

    AngMat = m

End Function
Function AngQtn(a() As Double, ix() As Integer) As Quaternion

'多段階の回転を表す四元数の計算

' a      ：入力、回転角（deg）
' ix     ：入力、回転軸　１：第1軸（Ｘ軸）、　２：第2軸（Ｙ軸）、　３：第3軸（Ｚ軸）
' AngQtn ：出力、四元数

    Dim q As Quaternion, n As Integer, i As Integer

    n = UBound(a)                           '回転角の個数
    q = RtnQtn(CDbl(a(1)), CInt(ix(1)))     '一回の回転による四元数
    If (n > 1) Then
        For i = 2 To n
            q = QtnRtn(q, CDbl(a(i)), CInt(ix(i)))
        Next i
    End If

    AngQtn = q

End Function
Function Arcosd(c As Double) As Double
      
'逆余弦　cos−１（C）　[0〜180deg]

' c      ：入力、余弦（cos）
' Arcosd ：出力、逆余弦[0〜180deg]
    
    If (c > 0#) Then
        If (c >= 1#) Then
            Arcosd = 0#
        Else
            Arcosd = Atn(Sqr(1 - c * c) / c) * CDGRD
        End If
    ElseIf (c = 0#) Then
            Arcosd = 90#
    Else
        If (c <= -1#) Then
            Arcosd = 180#
        Else
            Arcosd = Atn(Sqr(1 - c * c) / c) * CDGRD + 180#
        End If
    End If

End Function
Function Arsind(s As Double) As Double
      
'逆正弦　sin−１（S）　[-90〜90deg]

' S      ：入力、正弦（sin）
' Arsind ：出力、逆正弦（deg）
    
    If (s >= 1#) Then
            Arsind = 90#
    ElseIf (s <= -1#) Then
            Arsind = -90#
    Else
        Arsind = Atn(s / Sqr(1 - s * s)) * CDGRD
    End If

End Function
Function Artand(s As Double, c As Double) As Double

'逆正接　tan−１（S／C）　[-180〜180deg]

' s      ：入力、正弦（sin）
' c      ：入力、余弦（cos）
' Artand ：出力、逆正接[-180〜180deg]

    If (c > 0#) Then
            Artand = Atn(s / c) * CDGRD         '(-90〜0〜90)度
    ElseIf (c < 0#) Then
        If (s >= 0#) Then
            Artand = Atn(s / c) * CDGRD + 180#  '(90〜180]度
        Else
            Artand = Atn(s / c) * CDGRD - 180#  '[-180〜-90)度
        End If
    Else
        If (s >= 0#) Then
            Artand = 90#                        '90度
        Else
            Artand = -90#                       '-90度
        End If
    End If

End Function
Function AryCpy(a() As Double, ia As Integer, b() As Double, ib As Integer, n As Integer) As Boolean

'１次元配列のコピー

' a      ：入力、コピー元配列
' ia     ：入力、コピー元配列の先頭インデックス
' b      ：入力、コピー先配列
' ib     ：入力、コピー先配列の先頭インデックス
' n      ：入力、コピー数
' AryCpy ：出力、完了コード（常にTrue）
    
    Dim i As Integer
    
    For i = 0 To n - 1
        b(ib + i) = a(ia + i)
    Next i
    
    AryCpy = True
    
End Function
Function Azimth(r As Vector3D, v As Vector3D) As Double

'方位角

' r      ：入力、地心半径ベクトル
' V      ：入力、方位角を求めるベクトル
' Azimth ：出力、方位角（-180〜180deg）

    Dim h As Vector3D
    
    h = VctCrp(r, v)
    Azimth = Artand(VctMag(r) * h.z, r.y * h.x - r.x * h.y)
 
End Function
Function BinDgt(Bn As Variant, n As Integer) As Integer()

'２進数を各ビット列に分解する
'分解したビットは、MSB〜LSBの方向に先頭から入れる

' Bn     ：入力、２進数
' n      ：入力、２進数のビット長

    Dim Bp As Long, Ba As Long, Mask As Long, b() As Integer, i As Integer
    ReDim b(n)

    Bp = CLng(Bn)
    Mask = 2 ^ (n - 1) - 1
    For i = 1 To n - 1
        Ba = Bp And Mask
        If (Ba = Bp) Then b(i) = 0 Else b(i) = 1
        Bp = Ba
        Mask = CLng(Mask / 2) - 1
    Next i
    b(n) = Bp

    BinDgt = b

End Function
Function CarGeo(r As Vector3D) As Vector3D

'カルテシアン座標系から測地座標系へ変換

' r     ：入力、地心半径ベクトル
' CarGeo：出力、測地緯度、経度、高度

    Dim lat As Double, lng As Double, alt As Double
    Dim Glat As Double, sGlat As Double, cGlat As Double
    Dim RslR As Double, RslE As Double, RslZ As Double
    Dim rE As Double, AltE As Double, AltZ As Double
    
    rE = Sqr(r.x ^ 2 + r.y ^ 2)
    lat = Artand(r.z, ER12 * rE)
    lng = Actand(r.y, r.x)
    lng = lng - CDbl(Int(lng / 360#)) * 360#
    Glat = lat * CRDDG
    sGlat = Sin(Glat)
    cGlat = Cos(Glat)
    RslR = AXER / Sqr(1# - EER2 * sGlat ^ 2)
    RslE = RslR * cGlat
    RslZ = RslR * sGlat * ER12
    AltE = rE - RslE
    AltZ = r.z - RslZ
    alt = Sqr(AltE ^ 2 + AltZ ^ 2)
    CarGeo.x = lat '測地緯度
    CarGeo.y = lng '経度
    CarGeo.z = alt '高度
    
End Function
Function Cd7Wgs(rc As Vector3D, vc As Vector3D, ByRef Vw As Vector3D) As Vector3D

'Ｃ-７系からＷＧＳ８４系へ変換

' Rc     ：入力、Ｃ-７系位置ベクトル
' Vc     ：入力、Ｃ-７系速度ベクトル
' Cd7Wgs ：出力、ＷＧＳ８４系位置ベクトル
' Vw     ：出力、ＷＧＳ８４系速度ベクトル

    Dim a As Vector3D, b As Vector3D, DwcM As Matrix3x3

    a.x = rc.x + 140#
    a.y = rc.y - 510#
    a.z = rc.z - 689#
    DwcM = RtnMat(0.554 / 3600#, 3)
    b = Mx31Mp(DwcM, a)
    Cd7Wgs.x = 1.0000002263 * b.x - 140#
    Cd7Wgs.y = 1.0000002263 * b.y + 516#
    Cd7Wgs.z = 1.0000002263 * b.z + 677.5
    b = Mx31Mp(DwcM, vc)
    Vw.x = 1.0000002263 * b.x
    Vw.y = 1.0000002263 * b.y
    Vw.z = 1.0000002263 * b.z

End Function
Function Cosd(a As Double) As Double

'角度（度）の余弦

' a      ：入力、角度（度）
' Cosd   ：出力、余弦

    Cosd = Cos(CRDDG * a)

End Function
Function DcmRav(a As Matrix3x3) As Vector3D

'方向余弦マトリクスから回転角ベクトルへ変換

' a      ：入力、方向余弦マトリクス
' DcmRav ：出力、回転角ベクトル

    Dim b As Vector3D, bm As Double
    Dim sa As Double, Ca As Double, s As Double

    b.x = a.y.z - a.z.y
    b.y = a.z.x - a.x.z
    b.z = a.x.y - a.y.x
    bm = VctMag(b)
    sa = bm / 2#
    Ca = (a.x.x + a.y.y + a.z.z - 1#) / 2#
    If (sa < vbSmall) Then
        If (Ca > 0#) Then
            s = CDGRD / (2# - sa / 3#)  '０度近傍
        Else
            s = vbInf                   '１８０度近傍
      End If
    Else
        s = Artand(sa, Ca) / bm
    End If
    DcmRav = VctSmp(b, s)

End Function
Function DcmQtn(a As Matrix3x3) As Quaternion

'方向余弦マトリクスから四元数へ変換

' a      ：入力、方向余弦マトリクス
' DcmQtn ：出力、四元数

    Dim p As Double
    
    p = 2# * Sqr(1# + a.x.x + a.y.y + a.z.z)
    If (p = 0#) Then
        DcmQtn.c = vbNull
        DcmQtn.s.x = vbNull
        DcmQtn.s.y = vbNull
        DcmQtn.s.z = vbNull
    Else
        DcmQtn.c = p / 4#
        DcmQtn.s.x = (a.y.z - a.z.y) / p
        DcmQtn.s.y = (a.z.x - a.x.z) / p
        DcmQtn.s.z = (a.x.y - a.y.x) / p
    End If

End Function
Function Deg(a As Double) As Double

'ラジアンから度へ単位変換

' a      ：入力、角度（rad）
' Deg    ：出力、角度（deg）

    Deg = CDGRD * a

End Function
Function Derivs() As Boolean

'微係数評価

' Derivs ：出力、True：正常終了、False：微係数評価関数が未定義または異常終了

    Select Case INTG.ID

    Case 1 '姿勢パラメータ積分の演習（四元数、方向余弦マトリクス、オイラー角）
        Derivs = Demo_AttitudeIntegrations_Derivs()

    Case 6 '汎用関数テーブルの積分テスト
        Derivs = TableAndIntegTest_Derivs()

    Case Else
        Derivs = False
    End Select

End Function
Function Diff(a As Variant, b As Variant) As Variant

'差分

' a      ：入力、数値として扱えるデータ（整数、実数、日付型、通貨型）
' b      ：入力、数値として扱えるデータ（整数、実数、日付型、通貨型）
' Diff   ：出力、差分

    If (a = vbEmpty Or b = vbEmpty) Then
        Diff = ""
    Else
        Diff = a - b
    End If
    
End Function
Function DIP(Rv As Vector3D, Vv As Vector3D, beta As Double) As Vector3D

'大気中落下点（DIP）の計算

'Rv     ：入力、地心半径ベクトル [m]
'Vv     ：入力、慣性速度ベクトル [m/s]
'beta   ：入力、弾道係数 [kg/m^2]
'DIP    ：出力、DIP測地緯度[度]、経度[度]、落下時間[s]

    Dim RpV As Vector3D, VpV As Vector3D, Arglp As Double
    
    DIP = DIPx(Rv, Vv, beta, RpV, VpV)

End Function
Function DIPr(Rv As Vector3D, Vv As Vector3D, beta As Double) As Vector3D

'大気中落下点（DIP）の地心半径ベクトル

'Rv     ：入力、地心半径ベクトル [m]
'Vv     ：入力、慣性速度ベクトル [m/s]
'beta   ：入力、弾道係数 [kg/m^2]
'DIPr   ：出力、DIP地心半径ベクトル [m]

    Dim RpV As Vector3D, VpV As Vector3D
    
    If (Rv.x <> Temp0(1) Or Rv.y <> Temp0(2) Or Rv.z <> Temp0(3) Or _
        Vv.x <> Temp0(4) Or Vv.y <> Temp0(5) Or Vv.z <> Temp0(6) Or beta <> Temp0(7)) Then
        DIPr = DIPx(Rv, Vv, beta, RpV, VpV)
    End If
    DIPr.x = Temp0(8)
    DIPr.y = Temp0(9)
    DIPr.z = Temp0(10)

End Function
Function DIPv(Rv As Vector3D, Vv As Vector3D, beta As Double) As Vector3D

'大気中落下点（DIP）の慣性速度ベクトル

'Rv     ：入力、地心半径ベクトル [m]
'Vv     ：入力、慣性速度ベクトル [m/s]
'beta   ：入力、弾道係数 [kg/m^2]
'DIPv   ：出力、DIP慣性速度ベクトル [m/s]

    Dim RpV As Vector3D, VpV As Vector3D
    
    If (Rv.x <> Temp0(1) Or Rv.y <> Temp0(2) Or Rv.z <> Temp0(3) Or _
        Vv.x <> Temp0(4) Or Vv.y <> Temp0(5) Or Vv.z <> Temp0(6) Or beta <> Temp0(7)) Then
        DIPv = DIPx(Rv, Vv, beta, RpV, VpV)
    End If
    DIPv.x = Temp0(11)
    DIPv.y = Temp0(12)
    DIPv.z = Temp0(13)

End Function
Function DIPx(Rv As Vector3D, Vv As Vector3D, beta As Double, ByRef RetR As Vector3D, ByRef RetV As Vector3D) As Vector3D

'大気中落下点（DIP）の計算（DIP基本関数）

'Rv     ：入力、地心半径ベクトル [m]
'Vv     ：入力、慣性速度ベクトル [m/s]
'beta   ：入力、弾道係数 [kg/m^2]
'RetR   ：出力、落下点地心半ベクトル [m]
'RetV   ：出力、落下点速度ベクトル [m/s]
'DIPx   ：出力、DIP測地緯度[度]、経度[度]、落下時間[s]

    Dim dtn As Double, dtd As Double
    Dim dtS As Double, dTdr As Double, dCor As Double
    Dim Rk1 As Double, Rk2 As Double, Rk3 As Double
    Dim i As Integer, k As Integer
    
    Dim theAir As Boolean, LowAlt As Boolean
    Dim dtMin As Double, dtMid As Double, dtMax As Double, accMin As Double, accMid As Double, altL As Double

    Dim aDen As Double, aAcc As Double
    
    Dim RpV As Vector3D, VpV As Vector3D, ApV As Vector3D, WeV As Vector3D, VaV As Vector3D
    Dim Re2 As Double, rE As Double, Va2 As Double, r As Double, rDot As Double
    Dim RqV As Vector3D, TqV As Vector3D, rAng As Double
    
    Dim Vlat As Double, sVlat As Double, cVlat As Double
    Dim RslR As Double, RslE As Double, RslZ As Double, AltE As Double, AltZ As Double, alt As Double
    
    '大気モデル
    AM1976.Fnam = "ＪＲＡ−２５＋１９７６年米国標準大気"
    AM1976.Anam = "高度[m]"
    AM1976.Nf = 1
    AM1976.Intp = "１次補間"
    AM1976.fEnd = "一定"
    AM1976.bEnd = "外挿"
    AM1976.cInt = "補間"
    ReDim AM1976.Mlt(0 To AM1976.Nf)
    AM1976.Mlt(0) = 1#
    AM1976.Mlt(1) = 1#
    ReDim AM1976.Add(0 To AM1976.Nf)
    AM1976.Add(0) = 0#
    AM1976.Add(1) = 0#
    AM1976.nD = 42
    AM1976.ix = 0
    AM1976.tDis = vbTmax
    AM1976.arg = 0#
    ReDim AM1976.Val(1 To AM1976.Nf)
    AM1976.Val(1) = 0#
    ReDim AM1976.Data(1 To AM1976.nD, 0 To AM1976.Nf)
    AM1976.Data(1, 0) = -3000#
    AM1976.Data(1, 1) = 0#
    AM1976.Data(2, 0) = 0#
    AM1976.Data(2, 1) = 1.204
    AM1976.Data(3, 0) = 3000
    AM1976.Data(3, 1) = 0.8873
    AM1976.Data(4, 0) = 6000
    AM1976.Data(4, 1) = 0.6461
    AM1976.Data(5, 0) = 9000
    AM1976.Data(5, 1) = 0.4639
    AM1976.Data(6, 0) = 12000
    AM1976.Data(6, 1) = 0.326
    AM1976.Data(7, 0) = 15000
    AM1976.Data(7, 1) = 0.218
    AM1976.Data(8, 0) = 18000
    AM1976.Data(8, 1) = 0.1339
    AM1976.Data(9, 0) = 21000
    AM1976.Data(9, 1) = 0.07853
    AM1976.Data(10, 0) = 24000
    AM1976.Data(10, 1) = 0.04718
    AM1976.Data(11, 0) = 26000
    AM1976.Data(11, 1) = 0.0339
    AM1976.Data(12, 0) = 28000
    AM1976.Data(12, 1) = 0.02458
    AM1976.Data(13, 0) = 30000
    AM1976.Data(13, 1) = 0.0179
    AM1976.Data(14, 0) = 32000
    AM1976.Data(14, 1) = 0.01314
    AM1976.Data(15, 0) = 34000
    AM1976.Data(15, 1) = 0.009634
    AM1976.Data(16, 0) = 36000
    AM1976.Data(16, 1) = 0.007102
    AM1976.Data(17, 0) = 38000
    AM1976.Data(17, 1) = 0.005219
    AM1976.Data(18, 0) = 40000
    AM1976.Data(18, 1) = 0.00387
    AM1976.Data(19, 0) = 42000
    AM1976.Data(19, 1) = 0.002883
    AM1976.Data(20, 0) = 44000
    AM1976.Data(20, 1) = 0.002196
    AM1976.Data(21, 0) = 46000
    AM1976.Data(21, 1) = 0.001686
    AM1976.Data(22, 0) = 48000
    AM1976.Data(22, 1) = 0.0013
    AM1976.Data(23, 0) = 50000
    AM1976.Data(23, 1) = 0.001015
    AM1976.Data(24, 0) = 52000
    AM1976.Data(24, 1) = 0.0007951
    AM1976.Data(25, 0) = 54000
    AM1976.Data(25, 1) = 0.0006257
    AM1976.Data(26, 0) = 56000
    AM1976.Data(26, 1) = 0.0004947
    AM1976.Data(27, 0) = 60000
    AM1976.Data(27, 1) = 0.00030968
    AM1976.Data(28, 0) = 65000
    AM1976.Data(28, 1) = 0.00016321
    AM1976.Data(29, 0) = 70000
    AM1976.Data(29, 1) = 0.000082829
    AM1976.Data(30, 0) = 72000
    AM1976.Data(30, 1) = 0.000062374
    AM1976.Data(31, 0) = 75000
    AM1976.Data(31, 1) = 0.000039921
    AM1976.Data(32, 0) = 80000
    AM1976.Data(32, 1) = 0.000018458
    AM1976.Data(33, 0) = 86000
    AM1976.Data(33, 1) = 0.000006958
    AM1976.Data(34, 0) = 91000
    AM1976.Data(34, 1) = 0.00000286
    AM1976.Data(35, 0) = 100000
    AM1976.Data(35, 1) = 0.0000005604
    AM1976.Data(36, 0) = 120000
    AM1976.Data(36, 1) = 0.00000002222
    AM1976.Data(37, 0) = 140000
    AM1976.Data(37, 1) = 0.000000003831
    AM1976.Data(38, 0) = 200000
    AM1976.Data(38, 1) = 0.0000000002541
    AM1976.Data(39, 0) = 400000
    AM1976.Data(39, 1) = 0.000000000002803
    AM1976.Data(40, 0) = 700000
    AM1976.Data(40, 1) = 3.07E-14
    AM1976.Data(41, 0) = 1000000
    AM1976.Data(41, 1) = 3.561E-15
    AM1976.Data(42, 0) = 1E+99
    AM1976.Data(42, 1) = 0#

    '定数初期化
    WeV.x = 0#                                              '地球自転角速度[rad/s]
    WeV.y = 0#
    WeV.z = WEER
    dtMin = 0.125                                           '最小積分間隔[s]
    dtMid = 1#                                              '中間積分間隔[s]
    dtMax = 10#                                             '最大積分間隔[s]
    accMin = 0.01                                           '最小空力加速度[m/s^2]
    accMid = 1#                                             '中間空力加速度[m/s^2]
    altL = 30000#                                           '落下開始時の低高度判定値[m]
    RqV = VctNrm(Rv)                                        '初期RT座標系（高度方向）
    TqV = VctNrm(VctCrp(VctCrp(Rv, Vv), Rv))                '初期RT座標系（接線方向）
    
    '積分制御データ初期化
    theAir = True                                           '落下途中
    INTG.nInt = 6                                           '積分変数の個数[ND]
    INTG.t = 0#                                             '落下開始からの経過時間[s]
    IntIni (INTG.nInt)                                      '積分制御の初期化
    
    '状態量初期化
    RpV = Rv                                                '落下開始時の地心半径ベクトル [m]
    VpV = Vv                                                '落下開始時の慣性速度ベクトル [m/s]
    ORDN(1) = RpV.x
    ORDN(2) = RpV.y
    ORDN(3) = RpV.z
    ORDN(4) = VpV.x
    ORDN(5) = VpV.y
    ORDN(6) = VpV.z
    
    '微係数初期化
    VaV = VctSub(VpV, VctCrp(WeV, RpV))                     '対気速度[m/s]
    Va2 = VaV.x ^ 2 + VaV.y ^ 2 + VaV.z ^ 2
    Re2 = RpV.x ^ 2 + RpV.y ^ 2
    rE = Sqr(Re2)
    Vlat = Atn(RpV.z / (ER12 * rE))                         '測地緯度[rad]
    sVlat = Sin(Vlat)
    cVlat = Cos(Vlat)
    RslR = AXER / Sqr(1# - EER2 * sVlat ^ 2)
    RslE = RslR * cVlat
    RslZ = RslR * sVlat * ER12
    AltE = rE - RslE
    AltZ = RpV.z - RslZ
    If (AltE >= 0#) Then
        alt = Sqr(AltE ^ 2 + AltZ ^ 2)                      '高度
    Else
        alt = -Sqr(AltE ^ 2 + AltZ ^ 2)                     '高度（地面にめり込んでいる）
    End If
    aDen = TblInt(AM1976, alt)                              '大気密度[kg/m^3]
    aAcc = 0.5 * aDen * Va2 / beta                          '空力加速度[m/s^2]
    ApV = VctAdd(VctSmp(VctNrm(VaV), -aAcc), GrvAcc(RpV))   '機体加速度[m/s^2]
    DRVS(1) = VpV.x
    DRVS(2) = VpV.y
    DRVS(3) = VpV.z
    DRVS(4) = ApV.x
    DRVS(5) = ApV.y
    DRVS(6) = ApV.z
    If (alt <= altL) Then LowAlt = True Else LowAlt = False '低高度からの落下
    
    '落下するまで繰り返す
    Do While theAir
        
        '空力加速度の大きさに合わせた積分間隔の設定
        
        If (LowAlt) Then                                         '低高度からの落下は積分刻みを小さくする
            INTG.dti = dtMin
        Else
            If (aAcc < accMin) Then
                INTG.dti = dtMax
            ElseIf (aAcc < accMid) Then
                INTG.dti = dtMid
            Else
                INTG.dti = dtMin
            End If
        End If
        
        '地表面に落下するまでの時間
        r = Sqr(Re2 + RpV.z ^ 2)
        rDot = VctInp(RpV, VpV) / r                             '高度方向速度[m/s]
        If (rDot < 0#) Then
            dtd = alt / -rDot                                   '地表面に落下するまでの時間[s]
        Else
            dtd = vbTmax                                        '上昇中
        End If
        
        '積分間隔の選択
        dtn = CDbl(Int((INTG.t + vbTacr) / INTG.dti) + 1) * INTG.dti - INTG.t    '通常積分点までの時間[s]
        If (dtd < dtn) Then
            INTG.dt = dtd
            theAir = False
        Else
            INTG.dt = dtn
        End If
        
        'ルンゲ･クッタ･ギル法による積分
        For INTG.jIms = 1 To 4
            Rk1 = RKG1(INTG.jIms)
            Rk2 = RKG2(INTG.jIms)
            Rk3 = RKG3(INTG.jIms)
            dtS = RKG4(INTG.jIms) * INTG.dt
            INTG.t = INTG.t + dtS
            For i = 1 To INTG.nInt
                dTdr = INTG.dt * DRVS(i)
                dCor = Rk1 * (dTdr - Rk2 * TPI1(i))
                ORDN(i) = ORDN(i) + dCor
                TPI1(i) = TPI1(i) + 3# * dCor - Rk3 * dTdr
            Next i
            RpV.x = ORDN(1)
            RpV.y = ORDN(2)
            RpV.z = ORDN(3)
            VpV.x = ORDN(4)
            VpV.y = ORDN(5)
            VpV.z = ORDN(6)
            
            '微係数評価
            VaV = VctSub(VpV, VctCrp(WeV, RpV))                     '対気速度[m/s]
            Va2 = VaV.x ^ 2 + VaV.y ^ 2 + VaV.z ^ 2
            Re2 = RpV.x ^ 2 + RpV.y ^ 2
            rE = Sqr(Re2)
            Vlat = Atn(RpV.z / (ER12 * rE))                         '測地緯度[rad]
            sVlat = Sin(Vlat)
            cVlat = Cos(Vlat)
            RslR = AXER / Sqr(1# - EER2 * sVlat ^ 2)
            RslE = RslR * cVlat
            RslZ = RslR * sVlat * ER12
            AltE = rE - RslE
            AltZ = RpV.z - RslZ
            If (AltE >= 0#) Then
                alt = Sqr(AltE ^ 2 + AltZ ^ 2)                      '高度
            Else
                alt = -Sqr(AltE ^ 2 + AltZ ^ 2)                     '高度（地面にめり込んでいる）
            End If
            aDen = TblInt(AM1976, alt)                              '大気密度[kg/m^3]
            aAcc = 0.5 * aDen * Va2 / beta                          '空力加速度[m/s^2]
            ApV = VctAdd(VctSmp(VctNrm(VaV), -aAcc), GrvAcc(RpV))   '機体加速度[m/s^2]
            DRVS(1) = VpV.x
            DRVS(2) = VpV.y
            DRVS(3) = VpV.z
            DRVS(4) = ApV.x
            DRVS(5) = ApV.y
            DRVS(6) = ApV.z
        Next INTG.jIms
        rAng = Actand(VctInp(TqV, RpV), VctInp(RqV, RpV))           '落下点までの地心角
        If (INTG.t > 100000# Or alt < 0# Or rAng > 270#) Then       '暴走防止
            theAir = False
        End If
    Loop
    
    '出力データの設定
    If (rAng <= 270#) Then
        DIPx.x = CDGRD * Vlat                                        '落下点（DIP）測地緯度[度]
        DIPx.y = Actand(RpV.y, RpV.x)                                '落下点（DIP）経度[度]（X軸基準経度及び地球自を補正していない経度）
        DIPx.z = INTG.t                                              '落下時間[s]
        RetR = RpV                                                   '落下点地心半径ベクトル[m]
        RetV = VpV                                                   '落下点慣性速度ベクトル[m/s]
    Else
        DIPx = Vct3D(999.999, 999.999, 999999#)                      '落下点までの地心角が270度以上になるので、計算停止
    End If
    ReDim Temp0(13)
    Temp0(1) = Rv.x
    Temp0(2) = Rv.y
    Temp0(3) = Rv.z
    Temp0(4) = Vv.x
    Temp0(5) = Vv.y
    Temp0(6) = Vv.z
    Temp0(7) = beta
    Temp0(8) = RetR.x
    Temp0(9) = RetR.y
    Temp0(10) = RetR.z
    Temp0(11) = RetV.x
    Temp0(12) = RetV.y
    Temp0(13) = RetV.z
 
End Function
Function DrvAng(a As Vector3D, W As Vector3D, ix As Integer) As Vector3D

'回転角（オイラー角）の微係数

' a      ：入力、回転角（オイラー角）
' W      ：入力、機体回転角速度（deg/s）
' ix     ：入力、回転順序（RtnAng参照）
' DrvAng ：出力、回転角（オイラー角）の微係数

    Dim Wx As Double, Wy As Double, Wz As Double
    Dim Ax As Double, Ay As Double, Az As Double
    Dim sx As Double, sy As Double, sz As Double
    Dim cx As Double, cy As Double, cz As Double

    Wx = CRDDG * W.x
    Wy = CRDDG * W.y
    Wz = CRDDG * W.z

    Select Case ix
    Case 1, 123
        Ay = a.y * CRDDG
        sy = Sin(Ay)
        cy = Cos(Ay)
        Az = a.z * CRDDG
        sz = Sin(Az)
        cz = Cos(Az)
        If (cy <> 0#) Then
            DrvAng.x = (cz * W.x - sz * W.y) / cy
            DrvAng.y = sz * W.x + cz * W.y
            DrvAng.z = W.z - DrvAng.x * sy
        Else
            DrvAng.x = vbNull
            DrvAng.y = vbNull
            DrvAng.z = vbNull
        End If
    Case 2, 231
        Az = a.z * CRDDG
        sz = Sin(Az)
        cz = Cos(Az)
        Ax = a.x * CRDDG
        sx = Sin(Ax)
        cx = Cos(Ax)
        If (cz <> 0#) Then
            DrvAng.y = (cx * W.y - sx * W.z) / cz
            DrvAng.z = sx * W.y + cx * W.z
            DrvAng.x = W.x - DrvAng.y * sz
        Else
            DrvAng.x = vbNull
            DrvAng.y = vbNull
            DrvAng.z = vbNull
        End If
    Case 3, 312
        Ax = a.x * CRDDG
        sx = Sin(Ax)
        cx = Cos(Ax)
        Ay = a.y * CRDDG
        sy = Sin(Ay)
        cy = Cos(Ay)
        If (cx <> 0#) Then
            DrvAng.z = (cy * W.z - sy * W.x) / cx
            DrvAng.x = sy * W.z + cy * W.x
            DrvAng.y = W.y - DrvAng.z * sx
        Else
            DrvAng.x = vbNull
            DrvAng.y = vbNull
            DrvAng.z = vbNull
        End If
    Case 4, 321
        Ay = a.y * CRDDG
        sy = Sin(Ay)
        cy = Cos(Ay)
        Ax = a.x * CRDDG
        sx = Sin(Ax)
        cx = Cos(Ax)
        If (cy <> 0#) Then
            DrvAng.z = (cx * W.z + sx * W.y) / cy
            DrvAng.y = -sx * W.z + cx * W.y
            DrvAng.x = W.x + DrvAng.z * sy
        Else
            DrvAng.x = vbNull
            DrvAng.y = vbNull
            DrvAng.z = vbNull
        End If
    Case 5, 132
        Az = a.z * CRDDG
        sz = Sin(Az)
        cz = Cos(Az)
        Ay = a.y * CRDDG
        sy = Sin(Ay)
        cy = Cos(Ay)
        If (cz <> 0#) Then
            DrvAng.x = (cy * W.x + sy * W.z) / cz
            DrvAng.z = -sy * W.x + cy * W.z
            DrvAng.y = W.y + DrvAng.x * sz
        Else
            DrvAng.x = vbNull
            DrvAng.y = vbNull
            DrvAng.z = vbNull
        End If
    Case 6, 213
        Ax = a.x * CRDDG
        sx = Sin(Ax)
        cx = Cos(Ax)
        Az = a.z * CRDDG
        sz = Sin(Az)
        cz = Cos(Az)
        If (cx <> 0#) Then
            DrvAng.y = (cz * W.y + sz * W.x) / cx
            DrvAng.x = -sz * W.y + cz * W.x
            DrvAng.z = W.z + DrvAng.y * sx
        Else
            DrvAng.x = vbNull
            DrvAng.y = vbNull
            DrvAng.z = vbNull
        End If
    Case Else
        DrvAng.x = vbNull
        DrvAng.y = vbNull
        DrvAng.z = vbNull
    End Select

End Function
Function DrvDcm(d As Matrix3x3, W As Vector3D) As Matrix3x3

'方向余弦マトリクスの微係数

' d      ：入力、方向余弦マトリクス
' W      ：入力、機体回転角速度（deg/s）
' DrvDcm ：出力、方向余弦マトリクスの微係数

    Dim Wx As Double, Wy As Double, Wz As Double

    Wx = CRDDG * W.x
    Wy = CRDDG * W.y
    Wz = CRDDG * W.z
    DrvDcm.x.x = d.y.x * Wz - d.z.x * Wy
    DrvDcm.x.y = d.y.y * Wz - d.z.y * Wy
    DrvDcm.x.z = d.y.z * Wz - d.z.z * Wy
    DrvDcm.y.x = d.z.x * Wx - d.x.x * Wz
    DrvDcm.y.y = d.z.y * Wx - d.x.y * Wz
    DrvDcm.y.z = d.z.z * Wx - d.x.z * Wz
    DrvDcm.z.x = d.x.x * Wy - d.y.x * Wx
    DrvDcm.z.y = d.x.y * Wy - d.y.y * Wx
    DrvDcm.z.z = d.x.z * Wy - d.y.z * Wx

End Function
Function DrvQtn(q As Quaternion, W As Vector3D) As Quaternion

'四元数の微係数

' q      ：入力、四元数
' W      ：入力、機体回転角速度（deg/s）
' DrvQtn ：出力、四元数の微係数

      Dim s As Double

      s = CRDDG / 2#
      DrvQtn.c = s * (-W.x * q.s.x - W.y * q.s.y - W.z * q.s.z)
      DrvQtn.s.x = s * (W.x * q.c + q.s.y * W.z - q.s.z * W.y)
      DrvQtn.s.y = s * (W.y * q.c + q.s.z * W.x - q.s.x * W.z)
      DrvQtn.s.z = s * (W.z * q.c + q.s.x * W.y - q.s.y * W.x)

End Function
Function DrvRav(a As Vector3D, W As Vector3D) As Vector3D

'回転角ベクトルの微係数

' a      ：入力、回転角ベクトル（deg）
' W      ：入力、機体回転角速度（deg/s）
' DrvRav ：出力、回転角ベクトルの微係数（deg/s）

    Dim b As Vector3D, c As Vector3D
    Dim Asq As Double, Ang As Double, sa As Double, Ca As Double
    Dim Sfb As Double, Sfc As Double

    Asq = a.x ^ 2 + a.y ^ 2 + a.z ^ 2
    Ang = CRDDG * Sqr(Asq)
    sa = Sin(Ang)
    Ca = Cos(Ang)
    Sfb = CRDDG / 2#
    If (Ang < vbSmall) Then
        Sfc = 0#
    Else
        Sfc = (1# - Ang * sa / (2# * (1# - Ca))) / Asq
    End If
    b = VctCrp(a, W)
    c = VctCrp(a, b)
    DrvRav.x = W.x + Sfb * b.x + Sfc * c.x
    DrvRav.y = W.y + Sfb * b.y + Sfc * c.y
    DrvRav.z = W.z + Sfb * b.z + Sfc * c.z

End Function
Function er() As Double

'地球離心率

'er     ：出力、地球離心率

    er = EER
    
End Function
Function GeoCar(lat As Double, lng As Double, alt As Double) As Vector3D

'測地座標系からカルテシアン座標系へ変換

' lat    ：入力、測地緯度
' lng    ：入力、経度
' alt    ：入力、高度
' GeoCar ：出力、地心半径ベクトル

    Dim Glat As Double, sGlat As Double, cGlat As Double
    Dim Glng As Double, sGlng As Double, cGlng As Double
    Dim Rsl As Double, Xai As Double, Eta As Double
    
    Glat = lat * CRDDG
    sGlat = Sin(Glat)
    cGlat = Cos(Glat)
    Rsl = AXER / Sqr(1# - EER2 * sGlat ^ 2)
    Xai = (Rsl + alt) * cGlat
    Eta = (ER12 * Rsl + alt) * sGlat
    Glng = lng * CRDDG
    sGlng = Sin(Glng)
    cGlng = Cos(Glng)
    GeoCar.x = Xai * cGlng
    GeoCar.y = Xai * sGlng
    GeoCar.z = Eta

End Function
Function GeoDes(lat1 As Double, lon1 As Double, lat2 As Double, lon2 As Double) As Double

'測地線に沿った距離の計算

' lat1　：入力、第1地点の測地緯度（北緯）　[度]
' lon1　：入力、第1地点の経度（東経）　[度]
' lat2　：入力、第2地点の測地緯度（北緯）　[度]
' lon2　：入力、第2地点の経度（東経）　[度]
' GeoDes：出力、2地点間の距離　[m]

    Dim glat1 As Double, glon1 As Double
    Dim glat2 As Double, glon2 As Double
    Dim tu1 As Double, tu2 As Double
    Dim cu1 As Double, su1 As Double, cu2 As Double
    Dim s1 As Double, b1 As Double, f1 As Double
    Dim x As Double, sx As Double, cx As Double
    Dim y As Double, sy As Double, cy As Double
    Dim sa As Double, c2a As Double, cz As Double
    Dim e As Double, s As Double, c As Double, r As Double, d As Double
    Dim a As Double, F As Double
    Dim iter As Integer, flag As Boolean

    Const EPS As Double = 0.00000000005
    Const MAXITER As Integer = 100
    iter = 0
    flag = False
    
    a = AXER
    F = 1# - Sqrt(1# - EER2)

    If (lat1 + lat2 = 0# And Abs(lon1 - lon2) >= 179.4) Then
        lat2 = 0.5 * (lat1 + lat2)
        lon2 = 0.5 * (lon1 + lon2)
        flag = True
    End If
    
    glat1 = CRDDG * lat1
    glon1 = CRDDG * lon1
    glat2 = CRDDG * lat2
    glon2 = CRDDG * lon2

    r = 1# - F
    tu1 = r * Tan(glat1)
    tu2 = r * Tan(glat2)
    cu1 = 1# / Sqrt(1# + tu1 * tu1)
    su1 = cu1 * tu1
    cu2 = 1# / Sqrt(1# + tu2 * tu2)
    s1 = cu1 * cu2
    b1 = s1 * tu2
    f1 = b1 * tu1
    x = glon2 - glon1
    d = x + 1#

    Do While ((Abs(d - x) > EPS) And (iter <= MAXITER))
        iter = iter + 1
        sx = Sin(x)
        cx = Cos(x)
        tu1 = cu2 * sx
        tu2 = b1 - su1 * cu2 * cx
        sy = Sqrt(tu1 * tu1 + tu2 * tu2)
        cy = s1 * cx + f1
        y = CRDDG * Artand(sy, cy)
        sa = s1 * sx / sy
        c2a = 1# - sa * sa
        cz = f1 + f1
        If (c2a > 0#) Then
            cz = cy - cz / c2a
        End If
        e = cz * cz * 2# - 1#
        c = ((-3# * c2a + 4#) * F + 4#) * c2a * F / 16#
        d = x
        x = ((e * cy * c + cz) * sy * c + y) * sa
        x = (1# - c) * x * F + glon2 - glon1
    Loop

    x = Sqrt((1# / (r * r) - 1#) * c2a + 1#)
    x = (x - 1#) / (x + 1#)
    c = (x * x / 4# + 1#) / (1# - x)
    d = (0.375 * x * x - 1#) * x
    x = e * cy
    s = ((((sy * sy * 4# - 3#) * (1# - e - e) * cz * d / 6# - x) * d / 4# + cz) * sy * d + y) * c * a * r
    If flag Then
        s = 2# * s
    End If
    If (iter >= MAXITER) Then
        s = vbInf   '計算できなかった
    End If

    GeoDes = s

End Function
Function GeoGra(d As Variant) As GeoGraphicQuantities

'データ型「GeoGraphicQuantities」の生成

' d      ：入力、地理関連諸量の配列データ
' GeoGra ：出力、地理関連諸量
    
    GeoGra.ri = d(1)
    GeoGra.Vltc = d(2)
    GeoGra.Vlni = d(3)
    GeoGra.alt = d(4)
    GeoGra.Vlat = d(5)
    GeoGra.Vlng = d(6)
    GeoGra.Vi = d(7)
    GeoGra.Fpvi = d(8)
    GeoGra.Azvi = d(9)
    GeoGra.Atli = d(10)
    GeoGra.Amis = d(11)
    GeoGra.Rlat = d(12)
    
End Function
Function GeoPos(t As Double, F As StateVariables) As GeoGraphicQuantities

'地理関連諸量の計算

' t      ：入力、発射後秒時(sec)
' F      ：入力、飛行状態量
' GeoPos ：出力、地理関連諸量

    Dim rE As Double, Re2 As Double
    Dim Vlat As Double, sVlat As Double, cVlat As Double
    Dim RslR As Double, RslE As Double, RslZ As Double
    Dim AltE As Double, AltZ As Double
    Dim vR As Vector3D, vT As Vector3D, vN As Vector3D
    
    Re2 = F.r.x ^ 2 + F.r.y ^ 2
    GeoPos.ri = Sqr(Re2 + F.r.z ^ 2)                        '地心半径
    rE = Sqr(Re2)
    GeoPos.Vi = VctMag(F.v)                                 '慣性速度
    
    GeoPos.Vltc = Artand(F.r.z, rE)                         '地心緯度
    GeoPos.Vlat = Artand(F.r.z, ER12 * rE)                  '測地緯度
    GeoPos.Vlni = Actand(F.r.y, F.r.x)                      '慣性基準からの経度
    GeoPos.Vlng = INCO.Ilng + GeoPos.Vlni - WEERD * t       '経度
    GeoPos.Vlng = GeoPos.Vlng - CDbl(Int(GeoPos.Vlng / 360#)) * 360#
    Vlat = GeoPos.Vlat * CRDDG
    sVlat = Sin(Vlat)
    cVlat = Cos(Vlat)
    RslR = AXER / Sqr(1# - EER2 * sVlat ^ 2)
    RslE = RslR * cVlat
    RslZ = RslR * sVlat * ER12
    AltE = rE - RslE
    AltZ = F.r.z - RslZ
    GeoPos.alt = Sqr(AltE ^ 2 + AltZ ^ 2)                   '高度
    vR = VctNrm(F.r)
    vN = VctNrm(VctCrp(F.v, F.r))
    vT = VctCrp(vR, vN)
    GeoPos.Fpvi = Artand(VctInp(F.v, vR), VctInp(F.v, vT))  '慣性速度上下角
    GeoPos.Azvi = Azimth(F.r, F.v)                          '慣性速度方位角
    GeoPos.Atli = Arcosd(VctInp(vR, F.Dibm.x))              '機軸上下角
    GeoPos.Amis = Azimth(vR, F.Dibm.x)                      '機軸方位角
    GeoPos.Rlat = Artand(-VctInp(vR, F.Dibm.y), -VctInp(vR, F.Dibm.z)) 'バンク角

End Function
Function GeoXyz(p As GeoGraphicQuantities) As StateVariables

'地理データから慣性座標系の飛行状態量の計算

' p      ：入力、地理関連諸量
' Geoxyz ：出力、飛行状態量

    Dim Dilm As Matrix3x3, Divm As Matrix3x3
    
    Dilm = MatRtn(RtnMat(p.Vlni, 3), -p.Vltc, 2)        '慣性座標系から見た地心緯度局地座標系の方向余弦マトリクス
    GeoXyz.r.x = p.ri * Dilm.x.x
    GeoXyz.r.y = p.ri * Dilm.x.y                        '地心半径ベクトル
    GeoXyz.r.z = p.ri * Dilm.x.z
    Divm = MatRtn(MatRtn(Dilm, -p.Azvi, 1), p.Fpvi, 2)  '慣性座標系から見た慣性速度座標系の方向余弦マトリクス
    GeoXyz.v.x = p.Vi * Divm.z.x
    GeoXyz.v.y = p.Vi * Divm.z.y                        '慣性速度ベクトル
    GeoXyz.v.z = p.Vi * Divm.z.z
    GeoXyz.Dibm = MatRtn(MatRtn(MatRtn(Dilm, -p.Amis, 1), -p.Atli, 2), p.Rlat, 1)   '機体座標系の方向余弦マトリクス
    GeoXyz.qIB = DcmQtn(GeoXyz.Dibm)                                                '機体座標系の四元数

End Function
Function Gmst(t As Double) As Double

'グリニッジ平均恒星時（平均春分点からグリニッジまでの赤経）

' t    ：入力、発射後秒時(sec)
' gmst ：出力、平均春分点からグリニッジまでの赤経（deg）

    Dim mjdUT1 As Double, mjd0 As Double, fUT1 As Double
    Dim t0 As Double, t1 As Double, mst As Double

    mjdUT1 = mjd(t)  'UTCと等しいものとする
    mjd0 = Int(mjdUT1)
    fUT1 = (mjdUT1 - mjd0) * 86400#
    t0 = (mjd0 - 51544.5) / 36525#
    t1 = (mjdUT1 - 51544.5) / 36525#
    mst = (24110.54841 + 8640184.812866 * t0 + 1.0027379093508 * fUT1 + (0.093104 - 0.0000062 * t1) * t1 * t1) / 86400#
    Gmst = (mst - Int(mst)) * 360#

End Function
Function Gmu() As Double

'地球重力定数(m^3/s^2)

    Gmu = GMER

End Function
Function GrmSmt(a() As Double) As Double()

'グラム･シュミットの正規直交化

' a      ：入力、マトリクス（N行×N列）

    Dim b() As Double, d() As Double
    Dim i As Integer, j As Integer, k As Integer, Na As Integer, n As Integer
    
    Na = UBound(a)
    n = CInt(Sqrt(CDbl(Na)) + 0.1)
    ReDim b(n, n), d(n)
    
    k = 0
    For j = 1 To n
        For i = 1 To n
            k = k + 1
            b(i, j) = a(k)
        Next i
    Next j
    For k = 1 To n
        For j = k To n
            d(j) = 0#
            For i = 1 To n
                d(j) = d(j) + b(i, j) ^ 2
            Next i
            d(j) = Sqrt(d(j))
        Next j
        For j = k To n
            For i = 1 To n
                b(i, j) = b(i, j) / d(j)
            Next i
        Next j
        If (k = n) Then Exit For
        For j = k + 1 To n
            d(j) = 0#
            For i = 1 To n
                d(j) = d(j) + b(i, k) * b(i, j)
            Next i
        Next j
        For j = k + 1 To n
            For i = 1 To n
                b(i, j) = b(i, j) - b(i, k) * d(j)
            Next i
        Next j
    Next k
    ReDim d(Na)
    k = 0
    For j = 1 To n
        For i = 1 To n
            k = k + 1
            d(k) = b(i, j)
        Next i
    Next j
    GrmSmt = d

End Function
Function GrnLng(d As yymmdd, t As hhmmss) As Double

'グリニッジ平均恒星時（平均春分点からグリニッジまでの赤経）

' d      ：入力、日付（年、月、日）
' T      ：入力、時刻（時、分、秒）
' GrnLng ：出力、平均春分点からグリニッジまでの赤経（deg）

    Dim J19 As Double, J20 As Double, Jr As Double
    Dim Jy As Double, Jm As Double, Ds As Double, Glng As Double

    If (d.Mn > 3) Then Jy = d.Yr - 1900 Else Jy = d.Yr - 1901
    If (d.Mn > 2) Then Jm = d.Mn Else Jm = d.Mn + 12
    J19 = 365# * Jy + 30# * Jm + d.Dy - 33.5 + Int(CDbl(0.6 * (Jm + 1#))) + Int(CDbl(0.25 * Jy))
    J20 = 365# * 99# + 30# * 13# + 1# - 33.5 + Int(CDbl(0.6 * (14#))) + Int(CDbl(0.25 * 99#)) + 0.5
    Jr = (J19 - J20) / J20
    Ds = t.hr * 3600# + t.min * 60# + t.sec
    Glng = JDW0 + JDW1 * Jr + JDW2 * Jr ^ 2 + JDW3 * Jr ^ 3 + WEERD * Ds
    GrnLng = Glng - CDbl(Int(Glng / 360#)) * 360#

End Function
Function GrvAcc(r As Vector3D) As Vector3D

'地球重力加速度

' r      ：入力、地心半径ベクトル（m）
' GrvAcc ：出力、地球重力加速度ベクトル（m/s^2）

    Dim ri As Double, R2 As Double, R3 As Double
    Dim sLtc As Double, sLtc2 As Double, sLtc4 As Double
    Dim Ga2 As Double, Ga3 As Double, Ga4 As Double, Ga5 As Double
    Dim Gm As Double, Geq As Double, gNp As Double
    
    R2 = r.x ^ 2 + r.y ^ 2 + r.z ^ 2
    ri = Sqr(R2)
    R3 = ri * R2
    sLtc = r.z / ri
    sLtc2 = r.z ^ 2 / R2
    sLtc4 = sLtc2 ^ 2
    Ga2 = GER2 / R2
    Ga3 = GER3 / R3
    Ga4 = GER4 / (R2 * R2)
    Ga5 = GER5 / (R3 * R2)
    Gm = -GMER / R3
    Geq = Gm * (1# _
        - Ga2 * (5# * sLtc2 - 1#) _
        - Ga3 * (35# * sLtc2 - 15#) * sLtc _
        - Ga4 * (63# * sLtc4 - 42# * sLtc2 + 3#) _
        - Ga5 * (231# * sLtc4 - 210# * sLtc2 + 35#) * sLtc)
    If (sLtc = 0#) Then
        gNp = 0#
    Else
        gNp = Gm * (1# _
            - Ga2 * (5# * sLtc2 - 3#) _
            - Ga3 * (35# * sLtc4 - 30# * sLtc2 + 3#) / sLtc _
            - Ga4 * (63# * sLtc4 - 70# * sLtc2 + 15#) _
            - Ga5 * (231# * sLtc4 * sLtc2 - 315# * sLtc4 + 105# * sLtc2 - 5#) / sLtc)
    End If
    GrvAcc.x = Geq * r.x
    GrvAcc.y = Geq * r.y
    GrvAcc.z = gNp * r.z

End Function
Function IIP(Rv As Vector3D, Vv As Vector3D) As Vector3D

'真空中瞬時落下点（IIP）の計算

'Rv     ：入力、地心半径ベクトル [m]
'Vv     ：入力、慣性速度ベクトル [m/s]
'IIP    ：出力、IIP測地緯度[度]、経度[度]、落下時間[s]

    Dim RpV As Vector3D, VpV As Vector3D
    
    IIP = IIPx(Rv, Vv, AXER, RpV, VpV)
    
End Function
Function IIPr(Rv As Vector3D, Vv As Vector3D, AER As Double) As Vector3D

'真空中瞬時落下点（IIP）の地心半径ベクトル

'Rv     ：入力、地心半径ベクトル [m]
'Vv     ：入力、慣性速度ベクトル [m/s]
'Aer    ：入力、IIP計算用地球長半径（赤道半径）[m]
'IIPr   ：出力、落下点（IIP）の地心半径ベクトル[m]

    Dim RpV As Vector3D, VpV As Vector3D
    
    If (Rv.x <> Temp0(1) Or Rv.y <> Temp0(2) Or Rv.z <> Temp0(3) Or _
        Vv.x <> Temp0(4) Or Vv.y <> Temp0(5) Or Vv.z <> Temp0(6) Or AER <> Temp0(7)) Then
        IIPr = IIPx(Rv, Vv, AER, RpV, VpV)
    End If
    IIPr.x = Temp0(8)
    IIPr.y = Temp0(9)
    IIPr.z = Temp0(10)

End Function
Function IIPv(Rv As Vector3D, Vv As Vector3D, AER As Double) As Vector3D

'真空中瞬時落下点（IIP）の速度ベクトル

'Rv     ：入力、地心半径ベクトル [m]
'Vv     ：入力、慣性速度ベクトル [m/s]
'Aer    ：入力、IIP計算用地球長半径（赤道半径）[m]
'IIPv   ：出力、落下点（IIP）の速度ベクトル[m]

    Dim RpV As Vector3D, VpV As Vector3D, Arglp As Double
    
    If (Rv.x <> Temp0(1) Or Rv.y <> Temp0(2) Or Rv.z <> Temp0(3) Or _
        Vv.x <> Temp0(4) Or Vv.y <> Temp0(5) Or Vv.z <> Temp0(6) Or AER <> Temp0(7)) Then
        IIPv = IIPx(Rv, Vv, AER, RpV, VpV)
    End If
    IIPv.x = Temp0(11)
    IIPv.y = Temp0(12)
    IIPv.z = Temp0(13)

End Function
Function IIPx(Rv As Vector3D, Vv As Vector3D, AER As Double, ByRef RetR As Vector3D, ByRef RetV As Vector3D) As Vector3D

'真空中瞬時落下点（IIP）の計算（IIP基本関数）

'Rv     ：入力、地心半径ベクトル [m]
'Vv     ：入力、慣性速度ベクトル [m/s]
'Aer    ：入力、IIP計算用地球長半径（赤道半径）[m]
'RetR   ：出力、落下点地心半ベクトル [m]
'RetV   ：出力、落下点速度ベクトル [m/s]
'IIPx   ：出力、IIP測地緯度[度]、経度[度]、落下時間[s]

    Dim r As Double, rP As Double, v As Double, RpV As Vector3D
    Dim a As Double, e As Double, e2 As Double, perg As Double, a3Mu As Double
    Dim eSinE0 As Double, eCosE0 As Double
    Dim eSinEP As Double, eCosEP As Double
    Dim sinEE As Double, cosEE As Double
    Dim phi0 As Double, phi As Double
    Dim F As Double, g As Double
    Dim t001 As Double, t002 As Double, ctr As Integer
    Dim Hv As Vector3D, VpV As Vector3D, h As Double
    Dim asndp As Double, aincp As Double, Arglp As Double, anep As Double
    
    '現在の軌道要素
    r = VctMag(Rv)
    v = VctMag(Vv)
    eCosE0 = r * v ^ 2 / GMER - 1#              'E0：離心近点離角
    a = r / (1# - eCosE0)                       '軌道長半径
    eSinE0 = VctInp(Rv, Vv) / Sqrt(GMER * a)
    e2 = eSinE0 ^ 2 + eCosE0 ^ 2
    e = Sqrt(e2)                                '軌道離心率
    a3Mu = Sqrt(a ^ 3 / GMER)
    
    '落下するか否かの判定
    perg = a * (1 - e)
    If (perg > AER) Then '近地点半径が赤道半径以上で落下しない
        ctr = 100
    End If
        
    'イタレーションの初期値設定
    'IIP地心緯度
    phi = Artand(Rv.z, Sqrt(Rv.x ^ 2 + Rv.y ^ 2))
    
    'IIP地心緯度が前回値から変化しなくなるまで
    Do
        ctr = ctr + 1
        If (ctr > 100) Then
            Exit Do '異常終了
        End If
        
        'IIP地心半径
        rP = AER * (1# - EER2 / (2# * ER12) * Sind(phi) ^ 2)
        
        'IIP離心近点離角
        eCosEP = (a - rP) / a
        t001 = e2 - eCosEP ^ 2
        If (t001 < 0#) Then
            ctr = 100
            Exit Do
        End If
        eSinEP = -Sqrt(t001)
        cosEE = (eCosEP * eCosE0 + eSinEP * eSinE0) / e2
        sinEE = (eSinEP * eCosE0 - eCosEP * eSinE0) / e2
        
        'ラグランジュ級数
        F = (cosEE - eCosE0) / (1# - eCosE0)
        g = a3Mu * (sinEE + eSinE0 - eSinEP)
        
        'IIP地心半径ベクトル
        RpV.x = Rv.x * F + Vv.x * g
        RpV.y = Rv.y * F + Vv.y * g
        RpV.z = Rv.z * F + Vv.z * g
        
        'IIP地心緯度の更新
        phi0 = phi
        phi = Artand(RpV.z, Sqrt(RpV.x ^ 2 + RpV.y ^ 2))
        
        'イタレーション終了判定
    Loop Until (Abs(phi - phi0) < 0.0001)
    
    
    '出力データの設定
    If (ctr < 100) Then
        IIPx.x = LatCtG(phi)                                                            '測地緯度
        IIPx.y = Actand(RpV.y, RpV.x)                                                   '地球自転の補正していない経度（X軸からの角度）
        IIPx.z = a3Mu * (CRDDG * Actand(sinEE, cosEE) + eSinE0 - eSinEP)                '落下時間
        RetR = RpV                                                                      '落下点の地心半径ベクトル
        Hv = VctCrp(Rv, Vv)
        h = VctMag(Hv)
        asndp = Actand(Hv.x, -Hv.y)                                                     '昇交点経度[度]
        aincp = Artand(Sqrt(Hv.x ^ 2 + Hv.y ^ 2), Hv.z)                                 '軌道傾斜角[度]
        Arglp = Actand(RpV.z * h, (RpV.y * Hv.x - RpV.x * Hv.y))                        '緯度引数[度]
        VpV.x = Sqrt(GMER / a) * (eSinEP / (1# - eCosEP))
        VpV.y = h / rP
        VpV.z = 0
        VpV = Mx31Mp(MatRtn(MatRtn(RtnMat(asndp, 3), aincp, 1), Arglp, 3), VpV)         '落下点での速度ベクトル
        RetV = VpV
    Else
        IIPx = Vct3D(999.999, 999.999, 999999#)
        RetR = Vct3D(0#, 0#, 0#)
        RetV = Vct3D(0#, 0#, 0#)
    End If
    ReDim Temp0(13)
    Temp0(1) = Rv.x
    Temp0(2) = Rv.y
    Temp0(3) = Rv.z
    Temp0(4) = Vv.x
    Temp0(5) = Vv.y
    Temp0(6) = Vv.z
    Temp0(7) = AXER
    Temp0(8) = RetR.x
    Temp0(9) = RetR.y
    Temp0(10) = RetR.z
    Temp0(11) = RetV.x
    Temp0(12) = RetV.y
    Temp0(13) = RetV.z
    
End Function
Function IntIni(n As Integer) As Boolean

'積分制御の初期化

' n      ：入力、積分変数の個数
' IntIni ：出力、完了コード　常にTRUE

    ReDim INDX(n)   'ORDN/DRVS内の積分する要素のインデックス
    ReDim ORDN(n)   '積分値
    ReDim DRVS(n)   '微係数
    ReDim TPI1(n)   '中間データ
    Dim i As Integer, k As Integer
    
    'ルンゲクッタ･ギル法の数値積分定数
    RKG1(1) = 0.5
    RKG1(2) = 0.292893218813452
    RKG1(3) = 1.70710678118655
    RKG1(4) = 0.166666666666667
    RKG2(1) = 2#
    RKG2(2) = 1#
    RKG2(3) = 1#
    RKG2(4) = 2#
    RKG3(1) = 0.5
    RKG3(2) = 0.292893218813452
    RKG3(3) = 1.70710678118655
    RKG3(4) = 0.5
    RKG4(1) = 0.5
    RKG4(2) = 0#
    RKG4(3) = 0.5
    RKG4(4) = 0#

    '積分制御変数の初期化
    INTG.nInt = n
    INTG.t = 0#
    INTG.dti = 1#
    INTG.tDis = vbTmax
    INTG.jIms = 5
    For k = 1 To n
        INDX(k) = k
        ORDN(k) = 0#
        DRVS(k) = 0#
        TPI1(k) = 0#
    Next k
    
    IntIni = True

End Function
Function LatCtG(c As Double) As Double

'地心緯度から測地緯度へ変換

' Vltc   ：入力、地心緯度（deg）
' LatCtG ：出力、測地緯度（deg）

    Dim a As Double, sa As Double, Ca As Double

    a = c * CRDDG
    sa = Sin(a)
    Ca = Cos(a)
    LatCtG = Artand(sa, ER12 * Ca)

End Function
Function LatGtC(g As Double) As Double

'測地緯度から地心緯度へ変換

' Vltg   ：入力、測地緯度（deg）
' LatGtC ：出力、地心緯度（deg）

    Dim a As Double, sa As Double, Ca As Double

    a = g * CRDDG
    sa = Sin(a)
    Ca = Cos(a)
    LatGtC = Artand(ER12 * sa, Ca)

End Function
Function LchPad(ID As Integer) As SiteLocation

'射点位置データ

' ID        ：入力、射点識別番号（０：設定されているデータの取得、１：H-IIA射点の設定、２：H-IIB射点の設定）
' LchPad    ：出力、射点位置データ

   If (ID = 1) Then
        '射点位置（Ｈ-IIＡ射点）
        Cpad.alt = 62.314            '高度（m）
        Cpad.Ltg = 30.38569          '測地緯度（deg）
        Cpad.lng = 130.96254         '経度（deg）
        Cpad.Azl = 125#              '発射台方位角（deg）
        Cpad.lat = Cpad.Ltg           '天文緯度（deg）
        Cpad.Gsp = 0#                'ジオイドのずれ（m）
        Cpad.Def = 0#                '東西方向の鉛直からの傾き（deg）
    ElseIf (ID = 2) Then
        '射点位置（Ｈ-IIＢ射点）
        Cpad.alt = 93.958            '高度（m）
        Cpad.Ltg = 30.40104008       '測地緯度（deg）
        Cpad.lng = 130.97545785      '経度（deg）
        Cpad.Azl = 125#              '発射台方位角（deg）
        Cpad.lat = Cpad.Ltg           '天文緯度（deg）
        Cpad.Gsp = 0#                'ジオイドのずれ（m）
        Cpad.Def = 0#                '東西方向の鉛直からの傾き（deg）
    End If
    LchPad = Cpad

End Function
Function LclPos(t As Double, a As SiteLocation) As Vector3D

'地表面位置ベクトルの計算

' t     ：入力、発射後秒時（sec）
' a     ：入力、地上局の位置データ
' LclPos：出力、地上局の地心半径ベクトル

    Dim Glat As Double, sGlat As Double, cGlat As Double
    Dim Glng As Double, sGlng As Double, cGlng As Double
    Dim Rsl As Double, alt As Double, Xai As Double, Eta As Double
    
    Glat = a.lat * CRDDG                        'a.Lat      ：地上局の天文緯度（deg）
    sGlat = Sin(Glat)                           'a.Alt      ：地上局の高度（m）
    cGlat = Cos(Glat)                           'a.Gsp      ：地上局でのジオイドのずれ（m）
    Rsl = AXER / Sqr(1# - EER2 * sGlat ^ 2)     'a.Lng      ：地上局の経度（deg）
    alt = a.alt + a.Gsp                         'INCO.Ilng  ：慣性基準設定時のグリニッジから慣性座標系基準点までの経度（deg）
    Xai = (Rsl + alt) * cGlat
    Eta = (ER12 * Rsl + alt) * sGlat
    Glng = (a.lng - INCO.Ilng + WEERD * t) * CRDDG
    sGlng = Sin(Glng)
    cGlng = Cos(Glng)
    LclPos.x = Xai * cGlng
    LclPos.y = Xai * sGlng
    LclPos.z = Eta

End Function
Function LeepSec(mjd As Double) As Double

'閏秒の検索

' mjd    ：入力、修正ユリウス日
' LeepSec：出力、閏秒

        If ((mjd >= 41317#) And (mjd < 41499#)) Then LeepSec = 10#      ' 〜1972.7.1
        ElseIf ((mjd >= 41499#) And (mjd < 41683#)) Then LeepSec = 11#  ' 〜1973.1.1
        ElseIf ((mjd >= 41683#) And (mjd < 42048#)) Then LeepSec = 12#  ' 〜1974.1.1
        ElseIf ((mjd >= 42048#) And (mjd < 42413#)) Then LeepSec = 13#  ' 〜1975.1.1
        ElseIf ((mjd >= 42413#) And (mjd < 42778#)) Then LeepSec = 14#  ' 〜1976.1.1
        ElseIf ((mjd >= 42778#) And (mjd < 43144#)) Then LeepSec = 15#  ' 〜1977.1.1
        ElseIf ((mjd >= 43144#) And (mjd < 43509#)) Then LeepSec = 16#  ' 〜1978.1.1
        ElseIf ((mjd >= 43509#) And (mjd < 43874#)) Then LeepSec = 17#  ' 〜1979.1.1
        ElseIf ((mjd >= 43874#) And (mjd < 44239#)) Then LeepSec = 18#  ' 〜1980.1.1
        ElseIf ((mjd >= 44239#) And (mjd < 44786#)) Then LeepSec = 19#  ' 〜1981.7.1
        ElseIf ((mjd >= 44786#) And (mjd < 45151#)) Then LeepSec = 20#  ' 〜1982.7.1
        ElseIf ((mjd >= 45151#) And (mjd < 45516#)) Then LeepSec = 21#  ' 〜1983.7.1
        ElseIf ((mjd >= 45516#) And (mjd < 46247#)) Then LeepSec = 22#  ' 〜1985.7.1
        ElseIf ((mjd >= 46247#) And (mjd < 47161#)) Then LeepSec = 23#  ' 〜1988.1.1
        ElseIf ((mjd >= 47161#) And (mjd < 47892#)) Then LeepSec = 24#  ' 〜1990.1.1
        ElseIf ((mjd >= 47892#) And (mjd < 48257#)) Then LeepSec = 25#  ' 〜1991.1.1
        ElseIf ((mjd >= 48257#) And (mjd < 48804#)) Then LeepSec = 26#  ' 〜1992.7.1
        ElseIf ((mjd >= 48804#) And (mjd < 49169#)) Then LeepSec = 27#  ' 〜1993.7.1
        ElseIf ((mjd >= 49169#) And (mjd < 49534#)) Then LeepSec = 28#  ' 〜1994.7.1
        ElseIf ((mjd >= 49534#) And (mjd < 50083#)) Then LeepSec = 29#  ' 〜1996.1.1
        ElseIf ((mjd >= 50083#) And (mjd < 50630#)) Then LeepSec = 30#  ' 〜1997.7.1
        ElseIf ((mjd >= 50630#) And (mjd < 51179#)) Then LeepSec = 31#  ' 〜1999.1.1
        ElseIf ((mjd >= 51179#) And (mjd < 53736#)) Then LeepSec = 32#  ' 〜2006.1.1
        Else: LeepSec = 33#                                             '   2006.1.1〜
        End If
        
End Function
Function Lneqsl(a() As Double, m As Integer, n As Integer) As Boolean

'線形方程式を解く

' a     = 入出力：行列データ（a(1〜m,1〜m)に係数、a(1〜m,m+1〜n)に解）
' m     = 入力　, 行数（方程式の数）
' n     = 入力　, 列数（行数＋独立変数の数）

    Dim kx() As Integer
    Dim i As Integer, j As Integer, k As Integer, L As Integer, i1 As Integer
    Dim m1 As Integer, ktmp As Integer
    Dim tmp As Double, pvt As Double

    ReDim kx(m)
    ReDim Preserve a(m, n)

    m1 = m + 1      '独立変数の開始列
    For i = 1 To m  '列インデックスの初期化
        kx(i) = i
    Next i

    For i = 1 To m
        i1 = i + 1

        'ピボットにするゼロでないデータを探す
        Do While a(i, i) = 0#

            '行方向に探し、ゼロでない行があったら入れ替える
            For j = i1 To m
                If (a(j, i) <> 0#) Then
                    For L = 1 To n
                        tmp = a(i, L)
                        a(i, L) = a(j, L)
                        a(j, L) = tmp
                    Next L
                    Exit Do
                End If
            Next j
            
            '行方向には見つからなかったので列方向に探し、ゼロでない列があったら入れ替える
            For k = i1 To m
                If (a(i, k) <> 0#) Then
                    ktmp = kx(i)
                    kx(i) = kx(k)
                    kx(k) = ktmp
                    For L = 1 To m
                        tmp = a(L, i)
                        a(L, i) = a(L, k)
                        a(L, k) = tmp
                    Next L
                    Exit Do
                End If
            Next k
            
            '行方向及び列方向に見つからなかったので全データを探し、
            '見つかったら行及び列共に入れ替える
            For j = i1 To m
                For k = i1 To m
                    If (a(j, k) <> 0#) Then
                        For L = 1 To n
                            tmp = a(i, L)
                            a(i, L) = a(j, L)
                            a(j, L) = tmp
                        Next L
                        ktmp = kx(i)
                        kx(i) = kx(k)
                        kx(k) = ktmp
                        For L = 1 To m
                            tmp = a(L, i)
                            a(L, i) = a(L, k)
                            a(L, k) = tmp
                        Next L
                        Exit Do
                    End If
                Next k
            Next j
            
            '残りのデータは全てゼロで、計算続行不可
            For j = i1 To m
                For k = m1 To n
                    a(j, k) = 0#
                Next k
            Next j
            Lneqsl = False
            Exit Function
        Loop
        
        'ガウス･ジョルダン消去法
        pvt = a(i, i)
        For j = i To n
            a(i, j) = a(i, j) / pvt
        Next j
        For j = 1 To m
            If (i <> j) Then
                tmp = a(j, i)
                For k = i To n
                    a(j, k) = a(j, k) - a(i, k) * tmp
                Next k
            End If
        Next j
    Next i
    
    '入れ替えた列を元の順序に戻す
    For i = 1 To m
        For j = 1 To m
            If (i <> j And i = kx(j)) Then
                ktmp = kx(i)
                kx(i) = kx(j)
                kx(j) = ktmp
                For k = 1 To m
                  tmp = a(k, i)
                  a(k, i) = a(k, j)
                  a(k, j) = tmp
                Next k
                For k = m1 To n
                  tmp = a(i, k)
                  a(i, k) = a(j, k)
                  a(j, k) = tmp
                Next k
            End If
        Next j
    Next i
    Lneqsl = True

End Function
Function LokAng(t As Double, F As StateVariables, rS As SiteLocation) As ViewQuantities

'ルックアングル計算

' T      ：入力、発射後秒時(sec)
' F      ：入力、飛行状態量
' Rs     ：入力、地上局位置データ
' Lokang ：出力、視野情報諸量

    Dim Dirm As Matrix3x3, Ls As Vector3D
    Dim Lsx As Double, Lsy As Double, Lsz As Double
    Dim Lax As Double, Lay As Double, Laz As Double
    
    LokAng.r = LclPos(t, rS)                              '地上局の地心半径ベクトル
    Dirm = MatRtn(MatRtn(RtnMat(rS.lng - INCO.Ilng + WEERD * (INCO.t + t), 3) _
                            , -rS.lat, 2), rS.Def, 3)   '地上局位置での局地座標系（鉛直−東−北）
    Ls = VctSub(F.r, LokAng.r)                          '視線ベクトル
    Lsx = VctInp(Dirm.x, Ls)
    Lsy = VctInp(Dirm.y, Ls)
    Lsz = VctInp(Dirm.z, Ls)
    LokAng.Rg = VctMag(Ls)                              '直距離
    LokAng.Az = Artand(Lsy, Lsz)                        '方位角
    LokAng.El = Artand(Lsx, Sqr(Lsy ^ 2 + Lsz ^ 2))     '上下角
    Lax = VctInp(F.Dibm.x, Ls)
    Lay = VctInp(F.Dibm.y, Ls)
    Laz = VctInp(F.Dibm.z, Ls)
    LokAng.Tla = Artand(Sqr(Lay ^ 2 + Laz ^ 2), Lax)    'トータルルックアングル
    LokAng.Rla = Artand(Laz, Lay)                       'ロールルックアングル

End Function
Function LsqBep(x() As Double, y() As Double, nD As Long, nY As Integer, c() As Double, nP As Integer) As Integer

'最小自乗法による最適な次数での多項式近似係数を求める

' x(nD)         ：入力、独立変数テーブル
' y(nD, nY)     ：入力、従属変数（複数可）テーブル
' nD            ：入力、独立変数及び従属変数のデータ個数
' nY            ：入力、従属変数の個数
' c(nP+1, nY)   ：出力、多項式近似係数テーブル
' nP            ：入力、多項式近似次数
' LsqBep        ：出力、nP＞0の時：0、nP≦0の時：求めた最適次数

    Dim L As Boolean
    Dim C1() As Double      '多項式近似係数の一時記憶テーブル
    Dim nQ As Integer       '最適な多項式近似次数
    Dim nP1 As Integer      '多項式近似次数＋１
    Dim i As Long           'ループ制御子
    Dim j As Long           'ループ制御子
    Dim rS As Double        '残差
    Dim aV As Double        '残差の平均
    Dim dV As Double        '残差の分散
    Dim Dvp As Double       '残差の分散の最小値記憶領域

    ReDim Preserve x(nD)
    ReDim Preserve y(nD, nY)

    If (nP > 0) Then    '指定された次数で近似する
        nP1 = nP + 1
        ReDim c(nP1, nY)
        L = LsqPol(x, y, nD, nY, c, nP)
    Else                '残差の分散が最小となる次数を見つけ、その次数で近似する
        Dvp = vbInf
        For i = 1 To 50 '次数１から最大５０次まで探索
            nQ = i
            nP1 = nQ + 1
            ReDim c(nP1, nY)
            ReDim C1(nP1)
            L = LsqPol(x, y, nD, nY, c, nQ)
            aV = 0#
            dV = 0#
            For j = 1 To nP1
                C1(j) = c(j, 1)
            Next j
            For j = 1 To nD
                rS = PolyEx(C1, nQ, x(j)) - y(j, 1)
                aV = aV + rS
                dV = dV + rS ^ 2
            Next j
            aV = aV / nD                    '残差の平均
            dV = dV / (nD - 1) - aV ^ 2     '残差の分散
            If (dV >= Dvp) Then Exit For    '分散が大きくなる方向に転じたらループを抜ける
            Dvp = dV
        Next i
        nQ = nQ - 1                         '最適次数での近似係数を求める
        nP1 = nQ + 1
        ReDim c(nP1, nY)
        L = LsqPol(x, y, nD, nY, c, nQ)
    End If

    LsqBep = nQ

End Function
Function LsqPol(x() As Double, y() As Double, nD As Long, nY As Integer, c() As Double, nP As Integer) As Boolean

'最小自乗法による多項式近似係数を求める

' x(nD)         ：入力、独立変数テーブル
' y(nD, nY)     ：入力、従属変数（複数可）テーブル
' nD            ：入力、独立変数及び従属変数のデータ個数
' nY            ：入力、従属変数の個数
' c(nP+1, nY)   ：出力、多項式近似係数テーブル
' nP            ：入力、多項式近似次数

    Dim a() As Double
    Dim nP1 As Integer, nP1m As Integer, i As Long, j As Long, k As Long
    Dim xi As Double, xx As Double
    
    nP1 = nP + 1
    nP1m = nP1 + nY
    ReDim a(nP1, nP1m)
    ReDim c(nP1, nY)
    ReDim Preserve x(nD)
    ReDim Preserve y(nD, nY)
    
    For i = 1 To nP1
        For j = 1 To nP1m
            a(i, j) = 0#
        Next j
    Next i
    For i = 1 To nD
        xi = x(i)
        xx = 1#
        For j = 1 To nP1
            a(j, 1) = a(j, 1) + xx
            For k = 1 To nY
                a(j, nP1 + k) = a(j, nP1 + k) + xx * y(i, k)
            Next k
            xx = xx * xi
        Next j
        For j = 2 To nP1
            a(nP1, j) = a(nP1, j) + xx
            xx = xx * xi
        Next j
    Next i
    For k = 2 To nP1
        For j = 1 To nP
            a(j, k) = a(j + 1, k - 1)
        Next j
    Next k
    LsqPol = Lneqsl(a, nP1, nP1m)
    For k = 1 To nY
        For j = 1 To nP1
            c(j, k) = a(j, nP1 + k)
        Next j
    Next k

End Function
Function Mx33(d11 As Double, d21 As Double, d31 As Double, _
              d12 As Double, d22 As Double, d32 As Double, _
              d13 As Double, d23 As Double, d33 As Double) As Matrix3x3

'９要素から３ｘ３マトリクスの生成

' dij    ：入力、i行j列の成分
' Mx33    ：出力、３ｘ３マトリクス

    Mx33.x.x = d11
    Mx33.x.y = d21
    Mx33.x.z = d31
    Mx33.y.x = d12
    Mx33.y.y = d22
    Mx33.y.z = d32
    Mx33.z.x = d13
    Mx33.z.y = d23
    Mx33.z.z = d33

End Function
Function Mx33V3(vx As Vector3D, vy As Vector3D, vz As Vector3D) As Matrix3x3

'３ベクトルから３ｘ３マトリクスの生成

' vx    ：入力、ｘ軸ベクトル
' vy    ：入力、ｙ軸ベクトル
' vz    ：入力、ｚ軸ベクトル
' Mx33  ：出力、３ｘ３マトリクス

        Mx33V3.x = vx
        Mx33V3.y = vy
        Mx33V3.z = vz

End Function
Function MatAng(iv As Matrix3x3, ir As Matrix3x3, im As Matrix3x3) As Vector3D

'ある視点の座標系から見た２つの方向余弦マトリクス間の回転角

' 視点マトリクス(iv)、基準マトリクス(ir)、回転角を求めるマトリクス(im)は
' それぞれ共通の座標系iを基準として表された方向余弦マトリクスとする
' [a1]･[a2]･[a3]=Ang[(iv)T･im･(ir)T･iv]

' iV     ：入力、視点とする座標系の方向余弦マトリクス
' ir     ：入力、基準とする座標系の方向余弦マトリクス
' im     ：入力、回転角を求める座標系の方向余弦マトリクス
' MatAng ：出力、回転角（deg）

    MatAng = QtnAng(DcmQtn(iv), DcmQtn(ir), DcmQtn(im))

End Function
Function MatAry(m As Matrix3x3) As Double()

'３ｘ３マトリクスを１次元配列に変換

' m      ：入力、３ｘ３マトリクス
' MatAry ：出力、１次元配列

    Dim ary(9) As Double
    
    ary(1) = m.x.x
    ary(2) = m.x.y
    ary(3) = m.x.z
    ary(4) = m.y.x
    ary(5) = m.y.y
    ary(6) = m.y.z
    ary(7) = m.z.x
    ary(8) = m.z.y
    ary(9) = m.z.z
    
    MatAry = ary
    
End Function
Function MatRtn(a As Matrix3x3, b As Double, ix As Integer) As Matrix3x3

'マトリクスの回転

' a      ：入力、３ｘ３マトリクス
' b      ：入力、角度（deg）
' ix     ：入力、回転軸　１：第1軸（Ｘ軸）、　２：第2軸（Ｙ軸）、　３：第3軸（Ｚ軸）
' Matrtn ：出力、３ｘ３マトリクス

    MatRtn = Mx33Mp(a, RtnMat(b, ix))

End Function
Function mjd(t As Double) As Double

'修正ユリウス日の計算

' t    ：入力、発射後秒時(sec)
' mjd  ：出力、修正ユリウス日

    Dim monthT As Long, yearT As Long, midnight As Long, leapDay As Long

    '1月と2月は、前年の13月と14月と見なす
      If (INCO.Y0Day.Mn <= 2) Then
          monthT = INCO.Y0Day.Mn + 12
          yearT = INCO.Y0Day.Yr - 1
      Else
          monthT = INCO.Y0Day.Mn
          yearT = INCO.Y0Day.Yr
      End If

      '閏日の計算
      If ((10000 * yearT + 100 * monthT + INCO.Y0Day.Dy) <= 15821004) Then
          leapDay = -2 + ((yearT + 4716) / 4) - 1179              'ユリウス暦
      Else
          leapDay = (yearT / 400) - (yearT / 100) + (yearT / 4)   'グレゴリオ暦
      End If

      '午前0時における修正ユリウス日
          '西暦1858年11月17日(水) (グレゴリオ暦) 0時 を 暦元とした通日
      midnight = 365 * yearT - 679004 + leapDay + Int(30.6001 * (monthT + 1)) + INCO.Y0Day.Dy

      '小数日の付加
      mjd = midnight + (INCO.Xtime.hr * 3600 + INCO.Xtime.min * 60 + INCO.Xtime.sec + t) / 86400#
      
End Function
Function Mx31Mp(a As Matrix3x3, b As Vector3D) As Vector3D

'マトリクスとベクトルの乗算

' a      ：入力、３ｘ３マトリクス
' b      ：入力、３次元ベクトル
' Mx31mp ：出力、３次元ベクトル

    Mx31Mp.x = a.x.x * b.x + a.y.x * b.y + a.z.x * b.z
    Mx31Mp.y = a.x.y * b.x + a.y.y * b.y + a.z.y * b.z
    Mx31Mp.z = a.x.z * b.x + a.y.z * b.y + a.z.z * b.z

End Function
Function Mx33Dt(a As Matrix3x3) As Double

'マトリクスのデターミナント

' a      ：入力、３ｘ３マトリクス
' Mx33dt ：出力、デターミナント

    Mx33Dt = a.x.x * (a.y.y * a.z.z - a.z.y * a.y.z) _
           + a.y.x * (a.z.y * a.x.z - a.x.y * a.z.z) _
           + a.z.x * (a.x.y * a.y.z - a.y.y * a.x.z)

End Function
Function Mx33Iv(a As Matrix3x3) As Matrix3x3

'逆マトリクス

' a      ：入力、３ｘ３マトリクス
' Mx33iv ：出力、３ｘ３マトリクス

    Dim d As Double, p As Double, q As Double, r As Double
    
    p = a.y.y * a.z.z - a.z.y * a.y.z
    q = a.z.y * a.x.z - a.x.y * a.z.z
    r = a.x.y * a.y.z - a.y.y * a.x.z
    d = a.x.x * p + a.y.x * q + a.z.x * r
    If (d = 0#) Then
        Mx33Iv.x.x = vbNull
        Mx33Iv.x.y = vbNull
        Mx33Iv.x.z = vbNull
        Mx33Iv.y.x = vbNull
        Mx33Iv.y.y = vbNull
        Mx33Iv.y.z = vbNull
        Mx33Iv.z.x = vbNull
        Mx33Iv.z.y = vbNull
        Mx33Iv.z.z = vbNull
    Else
        Mx33Iv.x.x = p / d
        Mx33Iv.x.y = q / d
        Mx33Iv.x.z = r / d
        Mx33Iv.y.x = (a.y.z * a.z.x - a.z.z * a.y.x) / d
        Mx33Iv.y.y = (a.z.z * a.x.x - a.x.z * a.z.x) / d
        Mx33Iv.y.z = (a.x.z * a.y.x - a.y.z * a.x.x) / d
        Mx33Iv.z.x = (a.y.x * a.z.y - a.z.x * a.y.y) / d
        Mx33Iv.z.y = (a.z.x * a.x.y - a.x.x * a.z.y) / d
        Mx33Iv.z.z = (a.x.x * a.y.y - a.y.x * a.x.y) / d
    End If

End Function
Function Mx33Mp(a As Matrix3x3, b As Matrix3x3) As Matrix3x3

'マトリクスの乗算

' a      ：入力、３ｘ３マトリクス
' b      ：入力、３ｘ３マトリクス
' Mx33mp ：出力、３ｘ３マトリクス

    Mx33Mp.x.x = a.x.x * b.x.x + a.y.x * b.x.y + a.z.x * b.x.z
    Mx33Mp.x.y = a.x.y * b.x.x + a.y.y * b.x.y + a.z.y * b.x.z
    Mx33Mp.x.z = a.x.z * b.x.x + a.y.z * b.x.y + a.z.z * b.x.z
    Mx33Mp.y.x = a.x.x * b.y.x + a.y.x * b.y.y + a.z.x * b.y.z
    Mx33Mp.y.y = a.x.y * b.y.x + a.y.y * b.y.y + a.z.y * b.y.z
    Mx33Mp.y.z = a.x.z * b.y.x + a.y.z * b.y.y + a.z.z * b.y.z
    Mx33Mp.z.x = a.x.x * b.z.x + a.y.x * b.z.y + a.z.x * b.z.z
    Mx33Mp.z.y = a.x.y * b.z.x + a.y.y * b.z.y + a.z.y * b.z.z
    Mx33Mp.z.z = a.x.z * b.z.x + a.y.z * b.z.y + a.z.z * b.z.z

End Function
Function Mx33Tp(a As Matrix3x3) As Matrix3x3

'マトリクスの転置

' a      ：入力、３ｘ３マトリクス
' Mx33tp ：出力、３ｘ３マトリクス

    Mx33Tp.x.x = a.x.x
    Mx33Tp.x.y = a.y.x
    Mx33Tp.x.z = a.z.x
    Mx33Tp.y.x = a.x.y
    Mx33Tp.y.y = a.y.y
    Mx33Tp.y.z = a.z.y
    Mx33Tp.z.x = a.x.z
    Mx33Tp.z.y = a.y.z
    Mx33Tp.z.z = a.z.z
    
End Function
Function NrmVel(a As Vector3D, b As Vector3D, c As Vector3D) As Double

'軌道面外速度

' a      ：入力、基準位置ベクトル
' b      ：入力、基準速度ベクトル
' c      ：入力、対象速度ベクトル
' NrmVel ：出力、軌道面外速度

    NrmVel = VctInp(VctNrm(VctCrp(a, b)), c)

End Function
Function OdrAng(a As Matrix3x3, ix As Integer) As Vector3D

'回転角（回転順）

' a      ：入力、方向余弦マトリクス
' ix     ：入力、回転順序
'          ix＝ 1の時：　i=1、j=2、k=3　　ix＝ 2の時：　i=2、j=3、k=1　　ix＝ 3の時：　i=3、j=1、k=2
'          ix＝ 4の時：　i=3、j=2、k=1　　ix＝ 5の時：　i=1、j=3、k=2　　ix＝ 6の時：　i=2、j=1、k=3
'          ix＝ 7の時：　i=1、j=2、k=1　　ix＝ 8の時：　i=2、j=3、k=2　　ix＝ 9の時：　i=3、j=1、k=3
'          ix＝10の時：　i=3、j=2、k=3　　ix＝11の時：　i=1、j=3、k=1　　ix＝12の時：　i=2、j=1、k=2
' OdrAng ：出力、[θa]i･[θb]j･[θc]k＝aとなる回転角（θa、θb、θc）（回転順に配列。回転軸順ではない）

    Select Case ix
    Case 1, 123
        OdrAng.x = Artand(-a.z.y, a.z.z)
        OdrAng.y = Arsind(a.z.x)
        OdrAng.z = Artand(-a.y.x, a.x.x)
    Case 2, 231
        OdrAng.x = Artand(-a.x.z, a.x.x)
        OdrAng.y = Arsind(a.x.y)
        OdrAng.z = Artand(-a.z.y, a.y.y)
    Case 3, 312
        OdrAng.x = Artand(-a.y.x, a.y.y)
        OdrAng.y = Arsind(a.y.z)
        OdrAng.z = Artand(-a.x.z, a.z.z)
    Case 4, 321
        OdrAng.x = Artand(a.x.y, a.x.x)
        OdrAng.y = Arsind(-a.x.z)
        OdrAng.z = Artand(a.y.z, a.z.z)
    Case 5, 132
        OdrAng.x = Artand(a.y.z, a.y.y)
        OdrAng.y = Arsind(-a.y.x)
        OdrAng.z = Artand(a.z.x, a.x.x)
    Case 6, 213
        OdrAng.x = Artand(a.z.x, a.z.z)
        OdrAng.y = Arsind(-a.z.y)
        OdrAng.z = Artand(a.x.y, a.y.y)
    Case 7, 121
        OdrAng.x = Artand(a.x.y, -a.x.z)
        OdrAng.y = Arcosd(a.x.x)
        OdrAng.z = Artand(a.y.x, a.z.x)
        If (OdrAng.y = 0#) Then OdrAng.x = Artand(a.y.z - a.z.y, a.y.y + a.z.z)
    Case 8, 232
        OdrAng.x = Artand(a.y.z, -a.y.x)
        OdrAng.y = Arcosd(a.y.y)
        OdrAng.z = Artand(a.z.y, a.x.y)
        If (OdrAng.y = 0#) Then OdrAng.x = Artand(a.z.x - a.x.z, a.z.z + a.x.x)
    Case 9, 313
        OdrAng.x = Artand(a.z.x, -a.z.y)
        OdrAng.y = Arcosd(a.z.z)
        OdrAng.z = Artand(a.x.z, a.y.z)
        If (OdrAng.y = 0#) Then OdrAng.x = Artand(a.x.y - a.y.x, a.x.x + a.y.y)
    Case 10, 323
        OdrAng.x = Artand(a.z.y, a.z.x)
        OdrAng.y = Arcosd(a.z.z)
        OdrAng.z = Artand(a.y.z, -a.x.z)
        If (OdrAng.y = 0#) Then OdrAng.x = Artand(a.x.y - a.y.x, a.x.x + a.y.y)
    Case 11, 131
        OdrAng.x = Artand(a.x.z, a.x.y)
        OdrAng.y = Arcosd(a.x.x)
        OdrAng.z = Artand(a.z.x, -a.y.x)
        If (OdrAng.y = 0#) Then OdrAng.x = Artand(a.y.z - a.z.y, a.y.y + a.z.z)
    Case 12, 212
        OdrAng.x = Artand(a.y.x, a.y.z)
        OdrAng.y = Arcosd(a.y.y)
        OdrAng.z = Artand(a.x.y, -a.z.y)
        If (OdrAng.y = 0#) Then OdrAng.x = Artand(a.z.x - a.x.z, a.z.z + a.x.x)
    Case Else
        OdrAng.x = vbNull
        OdrAng.y = vbNull
        OdrAng.z = vbNull
    End Select

End Function
Function OrbElm(t As Double, F As StateVariables) As OrbitalElements

'軌道要素

' t      ：入力、発射後秒時(sec)
' F      ：入力、飛行状態量
' Orbelm ：出力、軌道要素

    Dim Ri2 As Double, Vi2 As Double, ri As Double, Vi As Double
    Dim He2 As Double, Hi2 As Double, He As Double
    Dim aInv As Double, rvIn As Double, mInv As Double
    Dim e1p As Double, e1m As Double, sAne As Double
    
    Ri2 = F.r.x ^ 2 + F.r.y ^ 2 + F.r.z ^ 2
    Vi2 = F.v.x ^ 2 + F.v.y ^ 2 + F.v.z ^ 2
    ri = Sqr(Ri2)
    Vi = Sqr(Vi2)
    OrbElm.am = VctCrp(F.r, F.v)                                '軌道角運動量ベクトル（m^2/s）
    He2 = OrbElm.am.x ^ 2 + OrbElm.am.y ^ 2
    Hi2 = He2 + OrbElm.am.z ^ 2
    He = Sqr(He2)
    OrbElm.h = Sqr(Hi2)                                         '軌道角運動量（m^2/s）
    aInv = 2# / ri - Vi2 / GMER
    rvIn = VctInp(F.r, F.v)
    mInv = GMER * aInv
    
    OrbElm.p = Hi2 / GMER                                       '半通径（m）
    OrbElm.ecc = Sqr(1# - OrbElm.p * aInv)                      '離心率
    OrbElm.envv = -mInv / 2#                                    '軌道エネルギー（m^2/s^2）
    
    OrbElm.asndx = Actand(OrbElm.am.x, -OrbElm.am.y)                                    'X軸から昇交点までの経度 (deg)
    OrbElm.asnd = INCO.Ilng - WEERD * t + OrbElm.asndx                                  '昇降点経度（deg）
    If (OrbElm.asnd > 360#) Then OrbElm.asnd = OrbElm.asnd - 360#
    OrbElm.Raan = Gmst(t) + OrbElm.asnd                                                 '昇降点赤経（deg）
    If (OrbElm.Raan > 360#) Then OrbElm.Raan = OrbElm.Raan - 360#
    
    OrbElm.ainc = Actand(He, OrbElm.am.z)                                               '軌道傾斜角（deg）
    OrbElm.argl = Actand(F.r.z * OrbElm.h, F.r.y * OrbElm.am.x - F.r.x * OrbElm.am.y)   '緯度引数（deg）
    OrbElm.ant = Actand(OrbElm.h * rvIn, Hi2 - GMER * ri)                               '真近点離角（deg）
    OrbElm.argp = OrbElm.argl - OrbElm.ant                                              '近地点引数（deg）
    If (OrbElm.argp > 360#) Then OrbElm.argp = OrbElm.argp - 360#
    If (aInv <= 0#) Then
        OrbElm.a = vbNull
        OrbElm.perd = vbNull
        OrbElm.apg = vbNull
        OrbElm.prg = vbNull
        OrbElm.apf = vbNull
        OrbElm.prf = vbNull
        OrbElm.vapf = vbNull
        OrbElm.vprf = vbNull
        OrbElm.ane = vbNull
        OrbElm.anm = vbNull
    Else
        OrbElm.a = 1# / aInv                                    '軌道長半径（m）
        OrbElm.perd = TOPI * Sqr(OrbElm.a ^ 2 / mInv)           '周期（s）
        e1p = (1# + OrbElm.ecc)
        e1m = (1# - OrbElm.ecc)
        OrbElm.apg = OrbElm.a * e1p                             '遠地点半径（m）
        OrbElm.prg = OrbElm.a * e1m                             '近地点半径（m）
        OrbElm.apf = OrbElm.apg - ARER                          '遠地点高度（m）
        OrbElm.prf = OrbElm.prg - ARER                          '近地点高度（m）
        OrbElm.vapf = Sqr(mInv * e1m / e1p)                     '遠地点速度（m/s）
        OrbElm.vprf = Sqr(mInv * e1p / e1m)                     '近地点速度（m/s）
        sAne = rvIn / Sqr(GMER * OrbElm.a)
        OrbElm.ane = Actand(sAne, 1# - ri * aInv)               '離心近点離角（deg）
        OrbElm.anm = OrbElm.ane - sAne * CDGRD                  '平均近点離角（deg）
    End If

End Function
Function OrbXyz(t As Double, O As OrbitalElements) As StateVariables

'軌道要素から慣性座標系位置及び速度の計算

' t      ：入力、発射後秒時(sec)
' O      ：入力、軌道要素（下記の6要素で計算可能）
'               apf  :遠地点高度[m]
'               prf  :近地点高度[m]
'               ainc :軌道傾斜角[度]
'               argl :緯度引数  [度]
'               argp :近地点引数[度]
'               asnd :昇交点経度[度]
' Orbxyz ：出力、飛行状態量（昇交点経度のゼロ点をＸ軸とした座標系）

    Dim rA As Double, rP As Double, a As Double, ecc As Double, p As Double, asndx As Double
    Dim ri As Double, h As Double, vR As Double, vT As Double
    Dim ant As Double, sAnt As Double, cAnt As Double, xasnd As Double
    Dim Dilm As Matrix3x3
    
    rA = O.apf + ARER               '遠地点半径（m）
    rP = O.prf + ARER               '近地点半径（m）
    a = (rA + rP) / 2#              '軌道長半径（m）
    ecc = (rA - rP) / (rA + rP)     '離心率
    p = a * (1# - ecc ^ 2)          '半通径（m）
    ant = (O.argl - O.argp) * CRDDG '真近点離角（deg）
    sAnt = Sin(ant)
    cAnt = Cos(ant)
    ri = p / (1# + ecc * cAnt)      '地心半径（m）
    h = Sqr(GMER * p)               '角運動量（m2/s）
    vR = ecc * GMER * sAnt / h      '地心半径方向速度（m/s）
    vT = h / ri                     '接線方向速度（m/s）
    asndx = O.asnd - INCO.Ilng + WEERD * t 'x軸から昇交点までの経度（deg）
    
    Dilm = MatRtn(MatRtn(RtnMat(asndx, 3), O.ainc, 1), O.argl, 3)
    OrbXyz.r.x = ri * Dilm.x.x                  '地心半径ベクトル（m）
    OrbXyz.r.y = ri * Dilm.x.y
    OrbXyz.r.z = ri * Dilm.x.z
    OrbXyz.v.x = vR * Dilm.x.x + vT * Dilm.y.x  '慣性速度ベクトル（m/s）
    OrbXyz.v.y = vR * Dilm.x.y + vT * Dilm.y.y
    OrbXyz.v.z = vR * Dilm.x.z + vT * Dilm.y.z
    
End Function
Function PolyEx(c() As Double, n As Integer, a As Double) As Double

'多項式展開

' c     = 入力：多項式係数
' n     = 入力：次数
' a     = 入力：引数
' PolyEx= 出力：展開結果

    Dim i As Integer, n1 As Integer

    n1 = n + 1
    PolyEx = c(n1)
    For i = 1 To n
        PolyEx = PolyEx * a + c(n1 - i)
    Next i

End Function
Function Qtn(c As Double, sx As Double, sy As Double, sz As Double) As Quaternion

' 四元数の生成

' s     ：入力、スカラー部、cos(θ/2)
' sx    ：入力、ベクトル部第1要素、(θx/θ)*sin(θ/2)
' sy    ：入力、ベクトル部第2要素、(θy/θ)*sin(θ/2)
' sz    ：入力、ベクトル部第3要素、(θz/θ)*sin(θ/2)

    Qtn.c = c
    Qtn.s.x = sx
    Qtn.s.y = sy
    Qtn.s.z = sz

End Function
Function QtnAng(iv As Quaternion, ir As Quaternion, iq As Quaternion) As Vector3D

'ある視点の座標系から見た２つの四元数間の回転角

' 視点四元数(iv)、基準四元数(ir)、回転角を求める四元数(iq)は
' それぞれ共通の座標系iを基準として表された四元数とする
' [a1]i･[a2]j･[a3]k=Ang[iv~*iq*ir~*iv]
'
' iv     ：入力、視点とする座標系の四元数ス
' ir     ：入力、基準とする座標系の四元数
' iq     ：入力、回転角を求める座標系の四元数
' QtnAng ：出力、回転角（deg）
    
    QtnAng = QtnRav(QtqqMp(QtqqMp(QtqqMp(QtnCnj(iv), iq), QtnCnj(ir)), iv))

End Function
Function QtnAry(q As Quaternion) As Double()

'四元数を１次元配列に変換

' q      ：入力、四元数
' QtnAry ：出力、１次元配列

    Dim ary(4) As Double
    
    ary(1) = q.c
    ary(2) = q.s.x
    ary(3) = q.s.y
    ary(4) = q.s.z
    
    QtnAry = ary
    
End Function
Function QtnCnj(a As Quaternion) As Quaternion

'共役四元数

' a      ：入力、四元数
' QtnCnj ：出力、共役四元数

    QtnCnj.c = a.c
    QtnCnj.s.x = -a.s.x
    QtnCnj.s.y = -a.s.y
    QtnCnj.s.z = -a.s.z

End Function
Function QtnDcm(a As Quaternion) As Matrix3x3

'四元数から方向余弦マトリクスへ変換

' A      ：入力、四元数
' Qtndcm ：出力、方向余弦マトリクス

    QtnDcm.x.x = a.c ^ 2 + a.s.x ^ 2 - a.s.y ^ 2 - a.s.z ^ 2
    QtnDcm.x.y = 2# * (a.s.x * a.s.y + a.c * a.s.z)
    QtnDcm.x.z = 2# * (a.s.x * a.s.z - a.c * a.s.y)
    
    QtnDcm.y.x = 2# * (a.s.x * a.s.y - a.c * a.s.z)
    QtnDcm.y.y = a.c ^ 2 - a.s.x ^ 2 + a.s.y ^ 2 - a.s.z ^ 2
    QtnDcm.y.z = 2# * (a.s.y * a.s.z + a.c * a.s.x)
    
    QtnDcm.z.x = 2# * (a.s.x * a.s.z + a.c * a.s.y)
    QtnDcm.z.y = 2# * (a.s.y * a.s.z - a.c * a.s.x)
    QtnDcm.z.z = a.c ^ 2 - a.s.x ^ 2 - a.s.y ^ 2 + a.s.z ^ 2

End Function
Function QtnRav(q As Quaternion) As Vector3D

'四元数から回転角ベクトルへ変換

' q     ：入力、四元数
' QtnRav：出力、回転角ベクトル

    Dim qAng As Double, qSin As Double
    
    qAng = Artand(Sqrt(q.s.x ^ 2 + q.s.y ^ 2 + q.s.z ^ 2), q.c)
    qSin = Sind(qAng)
    If (Abs(qSin) < vbSmall) Then
        QtnRav.x = 0#
        QtnRav.y = 0#
        QtnRav.z = 0#
    Else
        QtnRav.x = 2# * q.s.x * qAng / qSin
        QtnRav.y = 2# * q.s.y * qAng / qSin
        QtnRav.z = 2# * q.s.z * qAng / qSin
    End If

End Function
Function QtnRtn(a As Quaternion, b As Double, ix As Integer) As Quaternion

'四元数の回転

' a      ：入力、四元数
' b      ：入力、角度（deg）
' ix     ：入力、回転軸　１：第1軸（Ｘ軸）、　２：第2軸（Ｙ軸）、　３：第3軸（Ｚ軸）
' QtnRtn ：出力、回転した四元数

    QtnRtn = QtqqMp(a, RtnQtn(b, ix))

End Function
Function QtqqMp(a As Quaternion, b As Quaternion) As Quaternion

'四元数の乗算

' a      ：入力、四元数
' b      ：入力、四元数
' QtqqMp ：出力、四元数

    QtqqMp.c = a.c * b.c - a.s.x * b.s.x - a.s.y * b.s.y - a.s.z * b.s.z
    QtqqMp.s.x = a.c * b.s.x + a.s.x * b.c + a.s.y * b.s.z - a.s.z * b.s.y
    QtqqMp.s.y = a.c * b.s.y + a.s.y * b.c + a.s.z * b.s.x - a.s.x * b.s.z
    QtqqMp.s.z = a.c * b.s.z + a.s.z * b.c + a.s.x * b.s.y - a.s.y * b.s.x

End Function
Function QtqvMp(a As Quaternion, b As Vector3D) As Quaternion

'四元数とベクトルの乗算

' a      ：入力、四元数
' b      ：入力、３次元ベクトル
' QtqvMp ：出力、四元数

    QtqvMp.c = -a.s.x * b.x - a.s.y * b.y - a.s.z * b.z
    QtqvMp.s.x = a.c * b.x + a.s.y * b.z - a.s.z * b.y
    QtqvMp.s.y = a.c * b.y + a.s.z * b.x - a.s.x * b.z
    QtqvMp.s.z = a.c * b.z + a.s.x * b.y - a.s.y * b.x

End Function
Function quickSort(sD() As Variant, odr As Integer) As Variant()

' クイック･ソート

' sD        ：入出力、データ配列
' odr       ：入力  、ソート･オーダー　0：昇順、1：降順
' quickSort ：出力  、ソート済みデータ配列

    ReDim quickSort(UBound(sD))
    Dim L As Boolean
    
    If (odr = 0) Then
        L = quickSort_ascend(sD, 1, UBound(sD))     '昇順クイック･ソート
    Else
        L = quickSort_descend(sD, 1, UBound(sD))    '降順クイック･ソート
    End If
    
    quickSort = sD
    
End Function
Function quickSort_ascend(sD() As Variant, i As Integer, j As Integer) As Boolean

' 昇順クイック･ソート

' sD                ：入出力、データ配列
' i                 ：入力　、ソート開始位置
' j                 ：入力　、ソート終了位置
' quickSort_ascend  ：出力　、完了コード

    Dim L As Boolean, p As Integer, k As Integer
    
    If (i <> j) Then
        p = pivot_ascend(sD, i, j)  '軸要素の選択
        If (p <> -1) Then           '全データが等しくなければ
            k = partition_ascend(sD, i, j, sD(p))   '軸要素以上のデータと未満のデータに分割して配置する
            L = quickSort_ascend(sD, i, k - 1)      '軸要素以上のデータをソートする
            L = quickSort_ascend(sD, k, j)          '軸要素未満のデータをソートする
        End If
    End If
End Function
Function pivot_ascend(sD() As Variant, i As Integer, j As Integer) As Integer

' 軸要素の選択
' 順に見て、先頭と異なる要素を見つけ、大きいほうの番号を返す。
' 全部同じ要素の場合は -1 を返す。

' sD            ：入力、データ配列
' i             ：入力、探索開始位置
' j             ：入力、探索終了位置
' pivot_ascend  ：出力、軸要素の位置

    Dim k As Integer
    
    k = i
    Do  '先頭と異なる要素を探す
        k = k + 1
        If (k > j) Then Exit Do
        If (sD(i) <> sD(k)) Then Exit Do
    Loop
               
    If (k > j) Then
        pivot_ascend = -1           '全要素が等しい
    ElseIf (sD(i) >= sD(k)) Then
        pivot_ascend = i            '先頭要素が大きい
    Else
        pivot_ascend = k            '2番目以降に大きい要素が在った
    End If

End Function
Function partition_ascend(sD() As Variant, i As Integer, j As Integer, ByVal pivot As Variant) As Integer

' パーティション分割
' sD(i)〜sD(j)の間で、pivotを軸として分割する。
' pivotより小さい要素は前に、大きい要素はうしろに寄せ。大きい要素の開始位置を返す。

' sD                ：入出力、データ配列
' i                 ：入力　、探索開始位置
' j                 ：入力　、探索終了位置
' pivot             ：入力　、軸要素
' partition_ascend  ：出力　、軸要素より大きい要素の開始位置

    Dim p As Integer, q As Integer, temp As Variant
    
    p = i
    q = j
    While (p <= q)  '検索が交差するまで要素の入れ替えを繰り返す
        Do  '軸要素以上のデータを探す
            If (p > j) Then Exit Do
            If (sD(p) >= pivot) Then Exit Do
            p = p + 1
        Loop
        Do  '軸要素未満のデータを探す
            If (q < i) Then Exit Do
            If (sD(q) < pivot) Then Exit Do
            q = q - 1
        Loop
        If (p <= q) Then    '軸要素以上のデータと軸要素未満のデータを入れ替える
            temp = sD(p)
            sD(p) = sD(q)
            sD(q) = temp
            p = p + 1
            q = q - 1
        End If
    Wend
    partition_ascend = p
    
End Function
Function quickSort_descend(sD() As Variant, i As Integer, j As Integer) As Boolean

' 降順クイック･ソート

' sD                ：入出力、データ配列
' i                 ：入力　、ソート開始位置
' j                 ：入力　、ソート終了位置
' quickSort_descend ：出力　、完了コード

    Dim L As Boolean, p As Integer, k As Integer
    
    If (i <> j) Then
        p = pivot_descend(sD, i, j) '軸要素の選択
        If (p <> -1) Then           '全データが等しくなければ
            k = partition_descend(sD, i, j, sD(p))   '軸要素以下のデータと超のデータに分割して配置する
            L = quickSort_descend(sD, i, k - 1)      '軸要素以下のデータをソートする
            L = quickSort_descend(sD, k, j)          '軸要素超のデータをソートする
        End If
    End If
End Function
Function pivot_descend(sD() As Variant, i As Integer, j As Integer) As Integer

' 軸要素の選択
' 順に見て、先頭と異なる要素を見つけ、小さいほうの位置を返す。
' 全部同じ要素の場合は -1 を返す。

' sD            ：入力、データ配列
' i             ：入力、探索開始位置
' j             ：入力、探索終了位置
' pivot_descend ：出力、軸要素の位置

    Dim k As Integer
    
    k = i
    Do  '先頭と異なる要素を探す
        k = k + 1
        If (k > j) Then Exit Do
        If (sD(i) <> sD(k)) Then Exit Do
    Loop
    
    If (k > j) Then
        pivot_descend = -1           '全要素が等しい
    ElseIf (sD(i) < sD(k)) Then
        pivot_descend = i            '先頭要素が小さい
    Else
        pivot_descend = k            '2番目以降に小さい要素が在った
    End If

End Function
Function partition_descend(sD() As Variant, i As Integer, j As Integer, ByVal pivot As Variant) As Integer

' パーティション分割
' sD(i)〜sD(j)の間で、pivotを軸として分割する。
' pivotより大きい要素は前に、小さい要素はうしろに寄せ。小さい要素の開始位置を返す。

' sD                ：入出力、データ配列
' i                 ：入力　、探索開始位置
' j                 ：入力　、探索終了位置
' pivot             ：入力　、軸要素
' partition_descend ：出力　、軸要素より小さい要素の開始位置

    Dim p As Integer, q As Integer, temp As Variant
    
    p = i
    q = j
    While (p <= q) '検索が交差するまで要素の入れ替えを繰り返す
        Do  '軸要素以下のデータを探す
            If (p > j) Then Exit Do
            If (sD(p) <= pivot) Then Exit Do
            p = p + 1
        Loop
        Do  '軸要素超のデータを探す
            If (q < i) Then Exit Do
            If (sD(q) > pivot) Then Exit Do
            q = q - 1
        Loop
        If (p <= q) Then    '軸要素超のデータと軸要素以下のデータを入れ替える
            temp = sD(p)
            sD(p) = sD(q)
            sD(q) = temp
            p = p + 1
            q = q - 1
        End If
    Wend
    partition_descend = p
    
End Function
Function Rad(a As Double) As Double

'度からラジアンへ単位変換

' a     ：入力、角度（deg）
' Rad   ：出力、角度（rad）

    Rad = CRDDG * a

End Function
Function RavDcm(a As Vector3D) As Matrix3x3

'回転角ベクトルから方向余弦マトリクスへ変換

' a      ：入力、回転角ベクトル（deg）
' RavDcm ：出力、方向余弦マトリクス

    Dim rA As Double, Rax As Double, Ray As Double, Raz As Double
    Dim sa As Double, Sax As Double, Say As Double, Saz As Double
    Dim SRa As Double, CRa As Double, Sina As Double, Cosa As Double

    Rax = CRDDG * a.x
    Ray = CRDDG * a.y
    Raz = CRDDG * a.z
    Sax = Rax ^ 2
    Say = Ray ^ 2
    Saz = Raz ^ 2
    sa = Sax + Say + Saz
    rA = Sqr(sa)
    If (rA < vbSmall) Then '改良４次
        Cosa = 0.5 - sa / 30#
        Sina = 1# - sa / 6#
    Else
        Cosa = (1# - Cos(rA)) / sa
        Sina = Sin(rA) / rA
    End If
    RavDcm.x.x = 1# - Cosa * (Say + Saz)
    RavDcm.x.y = Cosa * Rax * Ray + Sina * Raz
    RavDcm.x.z = Cosa * Rax * Raz - Sina * Ray
    RavDcm.y.x = Cosa * Rax * Ray - Sina * Raz
    RavDcm.y.y = 1# - Cosa * (Saz + Sax)
    RavDcm.y.z = Cosa * Ray * Raz + Sina * Rax
    RavDcm.x = VctNrm(RavDcm.x)
    RavDcm.z = VctNrm(VctCrp(RavDcm.x, RavDcm.y))
    RavDcm.y = VctCrp(RavDcm.z, RavDcm.x)

End Function
Function RavQtn(a As Vector3D) As Quaternion

'回転角ベクトルから四元数へ変換

' a      ：入力、回転角ベクトル（deg）
' RavQtn ：出力、四元数

    Dim rA As Double
    Dim sa As Double

    rA = CRDDG * VctMag(a)
    RavQtn.c = Cos(0.5 * rA)
    sa = Sin(0.5 * rA)
    RavQtn.s = VctSmp(VctNrm(a), sa)

End Function
Function Rer() As Double

'地球赤道半径

    Rer = AXER

End Function
Function RkgInt() As Boolean

'ルンゲクッタ･ギル法による数値積分

' RkgInt ：出力、完了コード＝True：正常終了、False：異常終了

    Dim dtn As Double, dtd As Double
    Dim dtS As Double, dTdr As Double, dCor As Double
    Dim Rk1 As Double, Rk2 As Double, Rk3 As Double
    Dim i As Integer, k As Integer

    Do
        dtd = INTG.tDis - INTG.t                                                '不連続点までの時間
        If (dtd = 0#) Then Exit Do
        dtn = CDbl(Int((INTG.t + vbTacr) / INTG.dti) + 1) * INTG.dti - INTG.t   '通常積分点までの時間
        If (0# < dtd And dtd < dtn) Then INTG.dt = dtd Else INTG.dt = dtn
        For INTG.jIms = 1 To 4
            Rk1 = RKG1(INTG.jIms)
            Rk2 = RKG2(INTG.jIms)
            Rk3 = RKG3(INTG.jIms)
            dtS = RKG4(INTG.jIms) * INTG.dt
            INTG.t = INTG.t + dtS
            For k = 1 To INTG.nInt
                i = INDX(k)
                dTdr = INTG.dt * DRVS(i)
                dCor = Rk1 * (dTdr - Rk2 * TPI1(i))
                ORDN(i) = ORDN(i) + dCor
                TPI1(i) = TPI1(i) + 3# * dCor - Rk3 * dTdr
            Next k
            RkgInt = Derivs         '微係数評価
        Next INTG.jIms
    Loop Until (INTG.t >= INTG.tDis - vbTacr)
End Function
Function RtnAng(a As Matrix3x3, ix As Integer) As Vector3D

'回転角（オイラー角）

' a      ：入力、方向余弦マトリクス
' ix     ：入力、回転順序
' RtnAng ：出力、[θi]i･[θj]j･[θk]k＝aとなる回転角（オイラー角）（deg）（θi、θj、θk）（回転軸順に配列）

    Select Case ix
    Case 1, 123
        RtnAng.x = Artand(-a.z.y, a.z.z)
        RtnAng.y = Arsind(a.z.x)
        RtnAng.z = Artand(-a.y.x, a.x.x)
    Case 2, 231
        RtnAng.y = Artand(-a.x.z, a.x.x)
        RtnAng.z = Arsind(a.x.y)
        RtnAng.x = Artand(-a.z.y, a.y.y)
    Case 3, 312
        RtnAng.z = Artand(-a.y.x, a.y.y)
        RtnAng.x = Arsind(a.y.z)
        RtnAng.y = Artand(-a.x.z, a.z.z)
    Case 4, 321
        RtnAng.z = Artand(a.x.y, a.x.x)
        RtnAng.y = Arsind(-a.x.z)
        RtnAng.x = Artand(a.y.z, a.z.z)
    Case 5, 132
        RtnAng.x = Artand(a.y.z, a.y.y)
        RtnAng.z = Arsind(-a.y.x)
        RtnAng.y = Artand(a.z.x, a.x.x)
    Case 6, 213
        RtnAng.y = Artand(a.z.x, a.z.z)
        RtnAng.x = Arsind(-a.z.y)
        RtnAng.z = Artand(a.x.y, a.y.y)
    Case 7, 121
        RtnAng.y = Artand(a.x.y, -a.x.z)
        RtnAng.x = Arcosd(a.x.x)
        RtnAng.z = Artand(a.y.x, a.z.x)
    Case 8, 232
        RtnAng.y = Artand(a.y.z, -a.y.x)
        RtnAng.x = Arcosd(a.y.y)
        RtnAng.z = Artand(a.z.y, a.x.y)
    Case 9, 313
        RtnAng.y = Artand(a.z.x, -a.z.y)
        RtnAng.x = Arcosd(a.z.z)
        RtnAng.z = Artand(a.x.z, a.y.z)
    Case 10, 323
        RtnAng.y = Artand(a.z.y, a.z.x)
        RtnAng.x = Arcosd(a.z.z)
        RtnAng.z = Artand(a.y.z, -a.x.z)
    Case 11, 131
        RtnAng.y = Artand(a.x.z, a.x.y)
        RtnAng.x = Arcosd(a.x.x)
        RtnAng.z = Artand(a.z.x, -a.y.x)
    Case 12, 212
        RtnAng.y = Artand(a.y.x, a.y.z)
        RtnAng.x = Arcosd(a.y.y)
        RtnAng.z = Artand(a.x.y, -a.z.y)
    Case Else
        RtnAng.x = vbNull
        RtnAng.y = vbNull
        RtnAng.z = vbNull
    End Select

End Function
Function RtnMat(a As Double, ix As Integer) As Matrix3x3

'回転マトリクス

' a      ：入力、角度（deg）
' ix     ：入力、回転軸　１：第1軸（Ｘ軸）、　２：第2軸（Ｙ軸）、　３：第3軸（Ｚ軸）
' RtnMat ：出力、回転マトリクス（方向余弦マトリクス）[a]ix

    Dim d As Double, s As Double, c As Double
    d = a * CRDDG
    s = Sin(d)
    c = Cos(d)
    Select Case ix
    Case 1
        RtnMat.x.x = 1#
        RtnMat.x.y = 0#
        RtnMat.x.z = 0#
        RtnMat.y.x = 0#
        RtnMat.y.y = c
        RtnMat.y.z = s
        RtnMat.z.x = 0#
        RtnMat.z.y = -s
        RtnMat.z.z = c
    Case 2
        RtnMat.x.x = c
        RtnMat.x.y = 0#
        RtnMat.x.z = -s
        RtnMat.y.x = 0#
        RtnMat.y.y = 1#
        RtnMat.y.z = 0#
        RtnMat.z.x = s
        RtnMat.z.y = 0#
        RtnMat.z.z = c
    Case 3
        RtnMat.x.x = c
        RtnMat.x.y = s
        RtnMat.x.z = 0#
        RtnMat.y.x = -s
        RtnMat.y.y = c
        RtnMat.y.z = 0#
        RtnMat.z.x = 0#
        RtnMat.z.y = 0#
        RtnMat.z.z = 1#
    End Select

End Function
Function RtnQtn(a As Double, ix As Integer) As Quaternion

'回転を表す四元数

' a      ：入力、角度（deg）
' ix     ：入力、回転軸　１：第1軸（Ｘ軸）、　２：第2軸（Ｙ軸）、　３：第3軸（Ｚ軸）
' RtnQtn ：出力、四元数

    Dim A2 As Double

    A2 = a * CRDDG / 2#
    RtnQtn.c = Cos(A2)
    Select Case ix
    Case 1
        RtnQtn.s.x = Sin(A2)
        RtnQtn.s.y = 0#
        RtnQtn.s.z = 0#
    Case 2
        RtnQtn.s.x = 0#
        RtnQtn.s.y = Sin(A2)
        RtnQtn.s.z = 0#
    Case 3
        RtnQtn.s.x = 0#
        RtnQtn.s.y = 0#
        RtnQtn.s.z = Sin(A2)
    End Select

End Function
Function SetPad(alt As Double, lat As Double, lng As Double, Az As Double) As Boolean

'射点位置の設定

' Alt  ：入力、高度（m）
' Lat  ：入力、測地緯度（deg）
' Lng  ：入力、経度（deg）
' Az   ：入力、発射台方位角（deg）

    Cpad.alt = alt         '高度（m）
    Cpad.Ltg = lat         '測地緯度（deg）
    Cpad.lng = lng         '経度（deg）
    Cpad.Azl = Az          '発射台方位角（deg）
    Cpad.lat = lat         '天文緯度（deg）
    Cpad.Gsp = 0#          'ジオイドのずれ（m）
    Cpad.Def = 0#          '東西方向の鉛直からの傾き（deg）

End Function
Function SetRef(ID As Integer, tI As Double) As Boolean

'慣性座標系基準の設定

' ID    ：入力、慣性座標系X軸（０：射点基準、１：グリニッジ基準、２：平均春分点基準）
' tI    ：入力、慣性基準固定から発射までの時間(s)

    INCO.ID = ID
    INCO.t = tI
    INCO.Glng = Gmst(0#)                        '発射時のグリニッジ平均恒星時（平均春分点からグリニッジまでの経度）
    If (ID = 0) Then                            '射点基準
        If (Cpad.lng = 0#) Then
            INCO.Ilng = Cpad.lng - WEERD * tI
        Else
            INCO.Ilng = Cpad.lng - WEERD * tI
        End If
    ElseIf (ID = 1) Then                        'グリニッジ基準
        INCO.Ilng = 0#
    Else                                        '平均春分点基準
        INCO.Ilng = -INCO.Glng
    End If
    
End Function
Function SetTime(day() As Double, time() As Double) As Boolean

'発射日時の設定

' day   ：入力、発射年、月、日
' time  ：入力、発射時、分、秒

    INCO.Y0Day.Yr = day(1)
    INCO.Y0Day.Mn = day(2)
    INCO.Y0Day.Dy = day(3)
    INCO.Xtime.hr = time(1)
    INCO.Xtime.min = time(2)
    INCO.Xtime.sec = time(3)
    
End Function

Function Sind(a As Double) As Double

'角度（度）の正弦

' a      ：入力、角度（度）
' Sind   ：出力、正弦

    Sind = Sin(CRDDG * a)

End Function
Function Site(alt As Double, lat As Double, lng As Double, Az As Double) As SiteLocation

'データ型「SiteLocation」の生成

' Alt    ：入力、基準楕円面からの高度（m）
' Lat    ：入力、測地緯度（deg）
' Lng    ：入力、経度（deg）
' Az     ：入力、局設置方位角（deg）
' Site   ：出力、地上局位置データ

    Site.alt = alt
    Site.Ltg = lat
    Site.lng = lng
    Site.Azl = Az
    Site.lat = lat
    Site.Def = 0#
    Site.Gsp = 0#

End Function
Function Sqrt(a As Double) As Double

'平方根の計算

' a      ：入力、二乗値
' Sqrt   ：出力、平方根

    Sqrt = Sqr(a)

End Function
Function StaVar(r As Vector3D, v As Vector3D, qIB As Quaternion) As StateVariables

'データ型「StateVariables」の生成

' r      ：入力、慣性座標系での地心半径ベクトル
' v      ：入力、慣性座標系での慣性速度ベクトル
' Qib    ：入力、慣性座標系から見た機体座標系の四元数
' StaVar ：出力、飛行状態量

    StaVar.r = r
    StaVar.v = v
    StaVar.qIB = qIB
    StaVar.Dibm = QtnDcm(qIB)

End Function
Function StdRnd() As Double

'標準正規乱数の発生

'関数呼び出しせず、必要箇所に展開して使用すること。

    Dim Cor As Double, R1 As Double, R2 As Double
    Dim Ru1 As Double, Ru2 As Double, p As Double

    '１個の正規分布の乱数
    StdRnd = Sqr(-2# * Log(rNd())) * Sin(TOPI * rNd())
    Exit Function

    '２個の正規分布の乱数
    Ru1 = rNd()
    Ru2 = rNd()
    p = Sqr(-2# * Log(Ru1))
    R1 = p * Sin(TOPI * Ru2)
    R2 = p * Cos(TOPI * Ru2)

    '相関係数を持つ２変数正規分布の乱数
    ' Cor：相関係数
    Ru1 = rNd()
    Ru2 = rNd()
    p = Sqr(-2# * Log(Ru1))
    R1 = p * Sin(TOPI * Ru2)
    R2 = Cor * R1 + Sqr(1 - Cor ^ 2) * p * Cos(TOPI * Ru2)

End Function
Function TblInt(F As Table, aI As Double) As Double

'汎用テーブルの補間

' F       ：入力、汎用テーブル
' ai      ：入力、引数
' TblInt  ：出力、関数値

    Dim arg As Double, cF As Double, slpP As Double, t001 As Double, t002 As Double, t003 As Double, i As Integer

    '入力引数値
    arg = F.Mlt(0) * (aI + F.Add(0))
    
    '不連続点到達の通報は一度だけとする
    If (F.jDis = 2) Then F.jDis = 0
    
    'テーブル･インデックスの探索
    'インデックスは、入力引数より小さい直近のテーブル引数か等しいテーブル引数とする。
    
    '最初の探索時
    If (F.ixp < 0) Then
        If (F.ix < 1) Then F.ix = 1                     'テーブル始端
        If (F.ix > F.nD) Then F.ix = F.nD               'テーブル終端
        If (arg < F.Data(F.ix, 0) * vbMsS) Then
            '入力引数より小さいテーブル引数側から捜す
            Do While arg < F.Data(F.ix, 0) * vbMsS
                F.ix = F.ix - 1
                If (F.ix < 1) Then Exit Do
            Loop
        Else
            '入力引数より大きいテーブル引数側からを捜す
            Do
                F.ix = F.ix + 1
                If (F.ix > F.nD) Then Exit Do
            Loop Until arg < F.Data(F.ix, 0) * vbMsS
            F.ix = F.ix - 1
        End If
    '2回目以降
    Else
        'ステップ関数か節点制御が"積分"
        If (F.Intp = "ステップ" Or F.cInt = "積分") Then
            '積分完了点の時のみ、テーブル･インデックス更新の判定を行う
            If (INTG.jIms >= 4) Then
                '引数が次の節点上に達しているならば、1つだけ進める。
                If (F.aDis * vbMsS <= arg And F.tDis - vbTacr <= INTG.t) Then
                    F.ix = F.ix + 1
                    If (F.ix > F.nD) Then F.ix = F.nD
                    F.jDis = 2  '不連続点に到達とする
                End If
            End If
        
        'ステップ関数でなく節点制御が"積分"でもない
        Else
            '前回引数値と異なるときのみ更新
            If (arg <> F.arg) Then
                If (F.ix < 1) Then F.ix = 1                     'テーブル始端
                If (F.ix > F.nD) Then F.ix = F.nD               'テーブル終端
                If (arg < F.Data(F.ix, 0) * vbMsS) Then
                    '入力引数より小さいテーブル引数側から捜す
                    Do While arg < F.Data(F.ix, 0) * vbMsS
                        F.ix = F.ix - 1
                        If (F.ix < 1) Then Exit Do
                    Loop
                Else
                    '入力引数より大きいテーブル引数側からを捜す
                    Do
                        F.ix = F.ix + 1
                        If (F.ix > F.nD) Then Exit Do
                    Loop Until arg < F.Data(F.ix, 0) * vbMsS
                    F.ix = F.ix - 1
                End If
            End If
        End If
    End If

    '入力引数値またはインデックスが前回から変化していれば、関数出力値を求める。
    If (arg < F.arg * vbMsS Or F.arg * vbMsL < arg Or F.ix <> F.ixp) Then
        If (F.ix >= 1) Then
            If (F.ix < F.nD) Then   '２つのデータで補間する
                Select Case F.Intp
                Case "ステップ"
                    For i = 1 To F.Nf
                        F.Val(i) = F.Mlt(i) * F.Data(F.ix, i) + F.Add(i)
                    Next i
                Case Else '一次
                    cF = F.Data(F.ix + 1, 0) - F.Data(F.ix, 0)
                    If (cF <> 0#) Then
                        cF = (arg - F.Data(F.ix, 0)) / cF
                        For i = 1 To F.Nf
                            F.Val(i) = F.Mlt(i) * (F.Data(F.ix, i) + cF * (F.Data(F.ix + 1, i) - F.Data(F.ix, i))) + F.Add(i)
                        Next i
                    Else
                        For i = 1 To F.Nf
                            F.Val(i) = F.Mlt(i) * F.Data(F.ix, i) + F.Add(i)
                        Next i
                    End If
                End Select
                F.aDis = F.Data(F.ix + 1, 0)
            Else                    'テーブルの最後から外れた
                Select Case F.bEnd
                Case "ゼロ"
                    For i = 1 To F.Nf
                        F.Val(i) = 0#
                    Next i
                Case "一定"
                    For i = 1 To F.Nf
                        F.Val(i) = F.Mlt(i) * F.Data(F.nD, i) + F.Add(i)
                    Next i
                Case Else '一次
                    cF = F.Data(F.nD, 0) - F.Data(F.nD - 1, 0)
                    If (cF <> 0#) Then
                        cF = (arg - F.Data(F.nD, 0)) / cF
                        For i = 1 To F.Nf
                            F.Val(i) = F.Mlt(i) * (F.Data(F.nD, i) + cF * (F.Data(F.nD, i) - F.Data(F.nD - 1, i))) + F.Add(i)
                        Next i
                    Else
                        For i = 1 To F.Nf
                            F.Val(i) = F.Mlt(i) * F.Data(F.nD, i) + F.Add(i)
                        Next i
                    End If
                End Select
                F.aDis = vbInf
            End If
        Else                        'テーブルの先頭に達しない
            Select Case F.fEnd
            Case "ゼロ"
                For i = 1 To F.Nf
                    F.Val(i) = 0#
                Next i
            Case "一定"
                For i = 1 To F.Nf
                    F.Val(i) = F.Mlt(i) * F.Data(1, i) + F.Add(i)
                Next i
            Case Else '一次
                cF = F.Data(2, 0) - F.Data(1, 0)
                If (cF <> 0#) Then
                    cF = (arg - F.Data(1, 0)) / cF
                    For i = 1 To F.Nf
                        F.Val(i) = F.Mlt(i) * (F.Data(1, i) + cF * (F.Data(2, i) - F.Data(1, i))) + F.Add(i)
                    Next i
                Else
                    For i = 1 To F.Nf
                        F.Val(i) = F.Mlt(i) * F.Data(1, i) + F.Add(i)
                    Next i
                End If
            End Select
            F.aDis = F.Data(1, 0)
        End If
    End If
    
    
    '次の不連続点（節点）までの時間の設定
    
    'ステップ関数か節点制御が"積分"
    If (F.Intp = "ステップ" Or F.cInt = "積分") Then
        
        '積分完了点及び中間点で次の節点までの時間を計算するための勾配を求める
        If (INTG.jIms = 2 Or INTG.jIms >= 4) Then

            '**********************************************
            If (F.Fnam = "ThrustTable") Then
                t001 = t001
            End If
            '**********************************************

            If (arg - F.slpArg <> 0#) Then
                cF = (INTG.t - F.slpT) / (arg - F.slpArg)
                If (cF <> 0#) Then
                    F.slpP = F.slp
                    F.slp = cF
                    F.slpT = INTG.t
                    F.slpArg = arg
                End If
            End If
        End If
        
        '積分完了点
        If (INTG.jIms >= 4) Then

            '**********************************************
            If (F.Fnam = "ThrustTable") Then
                t001 = t001
            End If
            '**********************************************
 
            '不連続点到達
            If (F.jDis = 2) Then
                F.slpN = 0
            End If
            If (F.jDis = 1 And F.tDis - vbTacr <= INTG.t) Then F.jDis = 0
            
            '次の不連続点までの時間を飛び越さないよう、補正係数を設定する
            If (F.slp = F.slpP) Then
                If (F.slpN = 0) Then
                    F.slpM = 0#
                End If
            Else
                If (F.slpN = 0) Then
                    F.slpM = 0.75
                    F.slpN = 1
                End If
            End If
            If (F.slpN <> 0) Then
                If (INTG.jIms = 4) Then
                    F.slpN = F.slpN + 1
                    F.slpM = F.slpM * 0.25
                    If (F.slpN >= 8) Then
                        F.slpM = 0#
                        F.slpN = 0
                    End If
                End If
            End If
            
            '不連続点通過後処理中でないなら、次の不連続点までの時間を設定する
            If (F.jDis <> 1) Then

                '**********************************************
                If (F.Fnam = "ThrustTable") Then
                    t001 = t001
                End If
                '**********************************************

                F.tDis = INTG.t + (F.aDis - arg) * F.slp * (1# - F.slpM)
            
                '引数インデックスが更新されている
                If (F.ix <> F.ixp) Then
                    'ステップ関数なら、勾配を求めるため微小時間積分する
                    If (F.Intp = "ステップ") Then
                        F.tDis = INTG.t + 2# * vbTacr
                        F.jDis = 1  '不連続点通過後処理中とする
                    End If
                    '最初のテーブル参照でなく、同じテーブル引数が続いていたら不連続点とする
                    If (F.ixp > 0) Then
                        If (F.Data(F.ixp, 0) * vbMsS < F.Data(F.ix, 0) And F.Data(F.ix, 0) < F.Data(F.ixp, 0) * vbMsL) Then
                            F.tDis = INTG.t
                            F.jDis = 2  '不連続点に到達とする
                            F.slpM = 0#
                            F.slpN = 0
                        End If
                    End If
                End If
            End If
        End If
    
    'ステップ関数でなく節点制御が"積分"でもない
    Else
        F.tDis = vbTmax
    End If

    F.ixp = F.ix
    F.arg = arg
    TblInt = F.Val(1) '１番目の関数値を戻す。それ以外はテーブル内の値（F.Val）を参照

End Function
Function Vct3D(x As Double, y As Double, z As Double) As Vector3D

'３次元ベクトルの生成

' x      ：入力、ｘ成分
' y      ：入力、ｙ成分
' z      ：入力、ｚ成分
' Vct3D  ：出力、３次元ベクトル

    Vct3D.x = x
    Vct3D.y = y
    Vct3D.z = z

End Function
Function VctAdd(a As Vector3D, b As Vector3D) As Vector3D

'ベクトルの加算

' a      ：入力、３次元ベクトル
' b      ：入力、３次元ベクトル
' VctAdd ：出力、３次元ベクトル

    VctAdd.x = a.x + b.x
    VctAdd.y = a.y + b.y
    VctAdd.z = a.z + b.z

End Function
Function VctAng(a As Vector3D, b As Vector3D) As Double

'２つのベクトルの間の角度

' a      ：入力、３次元ベクトル
' b      ：入力、３次元ベクトル
' VctAng ：出力、角度 [0〜180deg]

    VctAng = Arcosd(VctInp(VctNrm(a), VctNrm(b)))

End Function
Function VctAry(v As Vector3D) As Double()

'３次元ベクトルを１次元配列に変換

' v      ：入力、３次元ベクトル
' VctAry ：出力、１次元配列

    Dim ary(3) As Double
    
    ary(1) = v.x
    ary(2) = v.y
    ary(3) = v.z
    
    VctAry = ary
    
End Function
Function VctCrp(a As Vector3D, b As Vector3D) As Vector3D

'ベクトルの外積

' a      ：入力、３次元ベクトル
' b      ：入力、３次元ベクトル
' VctCrp ：出力、３次元ベクトル

    VctCrp.x = a.y * b.z - a.z * b.y
    VctCrp.y = a.z * b.x - a.x * b.z
    VctCrp.z = a.x * b.y - a.y * b.x

End Function
Function VctInp(a As Vector3D, b As Vector3D) As Double

'ベクトルの内積

' a      ：入力、３次元ベクトル
' b      ：入力、３次元ベクトル
' VctInp ：出力、内積

    VctInp = a.x * b.x + a.y * b.y + a.z * b.z
       

End Function
Function VctMag(a As Vector3D) As Double

'ベクトルの絶対値

' a      ：入力、３次元ベクトル
' Vctmag ：出力、ベクトルａの大きさ（絶対値）

    VctMag = Sqr(a.x ^ 2 + a.y ^ 2 + a.z ^ 2)

End Function
Function VctNrm(a As Vector3D) As Vector3D

'ベクトルの正規化

' a      ：入力、３次元ベクトル
' Vctnrm ：出力、３次元ベクトル

    Dim d As Double
    d = Sqr(a.x ^ 2 + a.y ^ 2 + a.z ^ 2)
    If (d <> 0#) Then
        VctNrm.x = a.x / d
        VctNrm.y = a.y / d
        VctNrm.z = a.z / d
    Else
        VctNrm.x = 0#
        VctNrm.y = 0#
        VctNrm.z = 0#
    End If

End Function
Function VctRtn(v As Vector3D, a As Double, ix As Integer) As Vector3D

'ベクトルの回転　VctRtn=[A]ix・V

' V      ：入力、３次元ベクトル
' a      ：入力、角度（deg）
' ix     ：入力、回転軸　１：Ｘ軸、２：Ｙ軸、３：Ｚ軸
' VctRtn ：出力、３次元ベクトル

    VctRtn = Mx31Mp(RtnMat(a, ix), v)

End Function
Function VctSmp(v As Vector3D, s As Double) As Vector3D

'ベクトルとスカラーの積

' V      ：入力、３次元ベクトル
' s      ：入力、スカラー
' VctSmp ：出力、３次元ベクトル

    VctSmp.x = s * v.x
    VctSmp.y = s * v.y
    VctSmp.z = s * v.z

End Function
Function VctSub(a As Vector3D, b As Vector3D) As Vector3D

'ベクトルの減算

' a      ：入力、３次元ベクトル
' b      ：入力、３次元ベクトル
' Vctsub ：出力、３次元ベクトル

    VctSub.x = a.x - b.x
    VctSub.y = a.y - b.y
    VctSub.z = a.z - b.z
    
End Function
Function Wer() As Double

'地球自転レート(deg/s)

    Wer = WEERD

End Function
Function WgsCd7(Rw As Vector3D, Vw As Vector3D, ByRef vc As Vector3D) As Vector3D

'ＷＧＳ８４系からＣ-７系へ変換

' Rw    ：入力、ＷＧＳ８４系位置ベクトル
' Vw    ：入力、ＷＧＳ８４系速度ベクトル
' WgsC7R：出力、Ｃ-７系位置ベクトル
' Vc    ：出力、Ｃ-７系速度ベクトル

    Dim a As Vector3D, b As Vector3D, DcwM As Matrix3x3
    
    a.x = Rw.x + 140#
    a.y = Rw.y - 516#
    a.z = Rw.z - 677.5
    DcwM = RtnMat(-0.554 / 3600#, 3)
    b = Mx31Mp(DcwM, a)
    WgsCd7.x = b.x / 1.0000002263 - 140#
    WgsCd7.y = b.y / 1.0000002263 + 510#
    WgsCd7.z = b.z / 1.0000002263 + 689#
    b = Mx31Mp(DcwM, Vw)
    vc.x = b.x / 1.0000002263
    vc.y = b.y / 1.0000002263
    vc.z = b.z / 1.0000002263

End Function

