calc_contact_force｜接触の力を計算（多次元対応、Python関数）

# NumPy配列（ndarray）を入力する場合
import numpy as np                                      # 配列計算に使用するライブラリ

# 警告を出力する場合
import warnings                                         # 警告の表示に使用する標準ライブラリ

# 別のpyファイルからインポートする関数
from kelvin_voigt_force import calc_kelvin_voigt_force  # 接触時の粘弾性力の計算に使用する関数

def calc_contact_force(pos_a, v_a, pos_a_init, gap_init=0.0,
                       pos_b=None, v_b=None, pos_b_init=None,
                       k=1.0e+9, c=1.0e+4, damp_limit=1.0e-6):

    """
    ばね・ダンパ（フォークトモデル）に基づく接触力の計算（任意の方向からの接触・等方性・多次元対応）
    接触時の粘弾性力の計算には calc_kelvin_voigt_force を使用する
    Args:
        pos_a      : float | tuple[float] | list[float] | ndarray         質点Aの現在位置 [m]（有限値）
        v_a        : float | tuple[float] | list[float] | ndarray         質点Aの現在速度 [m/s]（有限値）
        pos_a_init : float | tuple[float] | list[float] | ndarray         質点Aの初期位置 [m]（有限値）
        gap_init   : float                                                初期位置間距離から接触が始まるまでの距離（初期間隔・遊び） [m] (>= 0, 初期距離以下, 有限値、省略時は0.0)
        pos_b      : float | tuple[float] | list[float] | ndarray | None  質点Bの現在位置 [m]（有限値、省略時は0（固定点）。pos_b_initのみ指定した場合はpos_b_initの位置に固定）
        v_b        : float | tuple[float] | list[float] | ndarray | None  質点Bの現在速度 [m/s]（有限値、省略時は0（固定点））
        pos_b_init : float | tuple[float] | list[float] | ndarray | None  質点Bの初期位置 [m]（有限値、省略時は0）
        k          : float                                                接触剛性（ばね定数） [N/m] (>= 0, 有限値、省略時は1.0e+9)
        c          : float                                                接触減衰（フルダンピング時の粘性係数） [N・s/m] (>= 0, 有限値、省略時は1.0e+4)
        damp_limit : float                                                フルダンピングに達するまでのめり込み距離 [m] (> 0, 有限値、省略時は1.0e-6)
        ※ 質点Bの各引数を指定する場合は、必ず質点Aと同一の座標系で入力してください。
        ※ ndarray は1次元配列（例：np.array([1.0, 2.0])）で指定してください。
        ※ 各要素が各成分（x, y, z）に対応します。
        ※ 0次元配列（例：np.array(1.0)）はスカラーとして扱います。
        ※ 本関数のみを単体で使用する場合は、float または ndarray のみ対応します。
        ※ list・tuple を使う場合や、入力値のエラーチェックを行う場合は、次の内部関数も必要です。
           ・_to_array_1dsim_com        : 入力値（数値・list・tuple・ndarray）を1次元ndarrayに変換
           ・_validate_values_1dsim_com : 配列内の各要素が実数・有限値・指定範囲内であるか確認
           ・_format_output_1dsim_com   : 計算結果の桁あふれを確認し、基準入力と同じ形式に変換
    Returns:
        f_a : float | tuple[float] | list[float] | ndarray  質点Aに働く接触力 [N]
        f_b : float | tuple[float] | list[float] | ndarray  質点Bに働く接触力 [N]
        ※ いずれも pos_a と同じ形式（float / tuple / list / ndarray）で返します。
    Raises:
        TypeError  : 各引数の要素が実数でない場合、スカラー引数（gap_init, k, c, damp_limit）に配列等を渡した場合、
                     またはベクトル引数に数値・list・tuple・ndarray 以外を渡した場合
        ValueError : gap_init, k, c が0以上の有限値でない場合、damp_limit が正の有限値でない場合、
                     gap_init が初期位置間距離を超えている場合、各引数が有限値でない場合、
                     ベクトル引数に2次元以上の ndarray を渡した場合、ベクトル引数の要素数が不一致・空の場合、
                     pos_b と v_b の片方のみを指定した場合、または計算結果が桁あふれした場合
        ※ 接触力の計算過程（calc_kelvin_voigt_force の内部）で桁あふれした場合は、エラーメッセージの先頭に内部の関数名が表示されます。
    Warns:
        UserWarning    : pos_b, v_b を省略して pos_b_init のみを指定した場合（質点Bを pos_b_init の位置に固定された点として計算します。pos_b_init がすべて0の場合は出しません）
        RuntimeWarning : 接触時に質点AとBの現在座標と速度が完全に一致し、反発方向（法線ベクトル）が特定できない場合（接触力を0とします）

    ========================================================
    Copyright (c) 2026 1Dsim.com

    本関数の利用・改変・再配布の条件は、
    1Dsim.com の「プライバシーポリシー・免責事項」
    （https://www.1dsim.com/privacy-policy/）に従います。
    このCopyright表記は削除しないでください。
    ========================================================
    """

    # エラーメッセージの先頭に表示する関数名を定義
    func_name = "calc_contact_force"

    # 1次元ndarray（またはスカラー値）同士の内積を計算するローカル関数
    def _dot(a, b):
        # 要素ごとの積を計算します。
        prod = a * b
        # 1次元配列の場合は全成分の和（内積）を返し、スカラー値の場合はそのまま返します。
        return sum(prod) if getattr(prod, "ndim", 0) > 0 else prod

    # --- 1. 計算準備と入力値の妥当性チェック ---
    # pos_bとv_bの片方だけが省略され、「固定点なのに動いている」などの矛盾状態になるのを防ぎます。
    if (pos_b is None) != (v_b is None):
        raise ValueError(f"{func_name}: pos_bとv_bは両方指定するか、両方省略する必要があります。")

    # pos_b, v_b を省略して pos_b_init のみが指定された場合、そのままでは
    # 「質点Bが pos_b_init から 0 へ動いた」と扱われ、初期距離と現在距離の基準がずれてしまいます。
    # これを防ぐため、質点Bは初期位置から動いていない（pos_b = pos_b_init）とみなします。
    is_fixed_b_at_init = (pos_b is None and pos_b_init is not None)
    if is_fixed_b_at_init:
        pos_b = pos_b_init

    # 内部関数（_to_array_1dsim_com）が定義されている場合は、入力形式の変換を行います。
    if "_to_array_1dsim_com" in globals():
        # numpyの数値（np.float64 など）や0次元ndarrayが渡された場合は、計算結果に混ざらないようPythonの数値に変換します。
        gap_init_calc   = gap_init.item() if getattr(gap_init, "ndim", None) == 0 else gap_init
        k_calc          = k.item() if getattr(k, "ndim", None) == 0 else k
        c_calc          = c.item() if getattr(c, "ndim", None) == 0 else c
        damp_limit_calc = damp_limit.item() if getattr(damp_limit, "ndim", None) == 0 else damp_limit

        # 入力の形式（数値・list・tuple・ndarray）にかかわらず同じ数式で計算できるよう、
        # ベクトル引数をすべて1次元ndarrayに変換します。
        pos_a_calc      = _to_array_1dsim_com(pos_a, func_name)
        v_a_calc        = _to_array_1dsim_com(v_a, func_name)
        pos_a_init_calc = _to_array_1dsim_com(pos_a_init, func_name)

        # 省略された引数は、0とみなします（pos_aの要素数に合わせます）。
        dim      = len(pos_a_calc)
        zero_vec = np.array([0.0] * dim, dtype=object)
        ones_vec = np.array([1.0] * dim, dtype=object)
        pos_b_calc      = zero_vec if pos_b is None else _to_array_1dsim_com(pos_b, func_name)
        v_b_calc        = zero_vec if v_b is None else _to_array_1dsim_com(v_b, func_name)
        pos_b_init_calc = zero_vec if pos_b_init is None else _to_array_1dsim_com(pos_b_init, func_name)

        # 位置・速度の要素数（成分の数）が揃っているか確認します。
        if not (len(v_a_calc) == len(pos_a_init_calc) == len(pos_b_calc) == len(v_b_calc) == len(pos_b_init_calc) == dim):
            raise ValueError(f"{func_name}: 位置・速度の要素数が一致していません。")

        # 要素数0（空のリストなど）では計算対象がないため、エラーとします。
        if dim == 0:
            raise ValueError(f"{func_name}: 入力値が空です。")
    else:
        # _to_array_1dsim_com を使用しない場合は、変換を行わず、数値またはndarrayとしてそのまま計算します。
        # （+ 0.0 により、整数型のndarrayが入力された場合でもfloat型に変換し、常に浮動小数点数で計算します）
        gap_init_calc   = gap_init + 0.0
        k_calc          = k + 0.0
        c_calc          = c + 0.0
        damp_limit_calc = damp_limit + 0.0

        pos_a_calc      = pos_a + 0.0
        v_a_calc        = v_a + 0.0
        pos_a_init_calc = pos_a_init + 0.0

        # pos_a_calc と同じ形式（数値またはndarray）の0と1を用意します。
        zero_vec        = pos_a_calc * 0.0 + 0.0
        ones_vec        = zero_vec + 1.0

        pos_b_calc      = zero_vec if pos_b is None else pos_b + 0.0
        v_b_calc        = zero_vec if v_b is None else v_b + 0.0
        pos_b_init_calc = zero_vec if pos_b_init is None else pos_b_init + 0.0

    # 内部関数（_validate_values_1dsim_com）が定義されている場合は、入力値の妥当性チェックを行います。
    if "_validate_values_1dsim_com" in globals():
        # 初期間隔gap_init・接触剛性k・接触減衰c・フルダンピング距離damp_limitが実数であるか確認します。
        # （boolは True=1 として計算が通ってしまうため、明示的に除外します）
        for name, val in (
            ("初期間隔gap_init", gap_init_calc),
            ("接触剛性k", k_calc),
            ("接触減衰c", c_calc),
            ("フルダンピング距離damp_limit", damp_limit_calc),
        ):
            if isinstance(val, bool) or not isinstance(val, numbers.Real):
                raise TypeError(f"{func_name}: {name}は実数（スカラー）で指定してください。")

        # 範囲外の値・無限大・NaNを除外します。
        if not (0 <= gap_init_calc < float("inf")):
            raise ValueError(f"{func_name}: 初期間隔gap_initは0以上の有限の値である必要があります。")
        if not (0 <= k_calc < float("inf")):
            raise ValueError(f"{func_name}: 接触剛性kは0以上の有限の値である必要があります。")
        if not (0 <= c_calc < float("inf")):
            raise ValueError(f"{func_name}: 接触減衰cは0以上の有限の値である必要があります。")
        if not (0 < damp_limit_calc < float("inf")):
            raise ValueError(f"{func_name}: フルダンピング距離damp_limitは0より大きい有限の値である必要があります。")

        # 位置・速度の各要素が実数・有限値であるか確認します（省略された引数は除く）。
        for arr in (pos_a, v_a, pos_a_init, pos_b, v_b, pos_b_init):
            if arr is not None:
                _validate_values_1dsim_com(arr, func_name, "位置・速度")

    # --- 2. 接触開始距離の計算 ---
    # 初期位置間の相対位置ベクトルとユークリッド距離（初期距離）を計算します [m]
    r_init = pos_a_init_calc - pos_b_init_calc
    d_init = _dot(r_init, r_init) ** 0.5

    # 初期距離の計算で桁あふれ（オーバーフロー）していないか確認します。
    if not (d_init < float("inf")):
        raise ValueError(f"{func_name}: 計算結果が桁あふれしました。入力値の大きさを確認してください。")

    # 初期間隔が初期距離を超えている場合、接触開始距離が負になり物理的に矛盾するため、エラーとします。
    if gap_init_calc > d_init:
        raise ValueError(f"{func_name}: 初期間隔gap_initが初期位置間の距離を超えています（物理的に矛盾した設定です）。")

    # 初期距離から初期間隔（遊び）を引くことで、接触が始まる距離を計算します [m]
    d_contact = d_init - gap_init_calc

    # --- 3. 接触判定と接触力の計算 ---
    # 現在位置間の相対位置ベクトルとユークリッド距離（現在距離）を計算します [m]
    r_curr = pos_a_calc - pos_b_calc
    d_curr = _dot(r_curr, r_curr) ** 0.5

    # 現在距離の計算で桁あふれ（オーバーフロー）していないか確認します。
    if not (d_curr < float("inf")):
        raise ValueError(f"{func_name}: 計算結果が桁あふれしました。入力値の大きさを確認してください。")

    # 接触開始距離から現在距離を引くことで、めり込み量を計算します [m]
    penetration = d_contact - d_curr

    # めり込み量が0より大きい場合（接触している場合）のみ、接触力を計算します。
    if penetration > 0.0:
        # 質点Aと質点Bの相対速度を計算します [m/s]
        v_rel = v_a_calc - v_b_calc

        # 質点Bから質点Aへ向かう単位法線ベクトル（反発方向）を計算します [-]
        if d_curr > 0.0:
            n = r_curr / d_curr
        else:
            # 質点AとBの現在位置が完全に重なり（d_curr == 0）、位置から法線ベクトルを計算できない場合は、
            # 相対速度の向きから反発方向を判定するため、まず相対速度の大きさを計算します [m/s]
            v_norm = _dot(v_rel, v_rel) ** 0.5
            if not (v_norm < float("inf")):
                raise ValueError(f"{func_name}: 計算結果が桁あふれしました。入力値の大きさを確認してください。")

            if v_norm > 0.0:
                # 相対速度がある場合は、相対速度の逆向きを反発方向（法線ベクトル）とします [-]
                n = -v_rel / v_norm
            else:
                # 現在位置と速度がともに完全一致している場合は、反発方向が特定できないため警告を出し、
                # 法線ベクトルを0（接触力0）とします。
                if "warnings" in globals():
                    warnings.warn(
                        f"{func_name}: 質点AとBの現在座標と速度が完全に一致したため、反発方向を計算できません。"
                        "暫定的に接触力を0とします。接触パラメータを見直してください。",
                        RuntimeWarning,
                        stacklevel=2,  # 警告の発生箇所として、この関数の呼び出し元の行を表示します
                    )
                n = zero_vec

        # 高速接触時の不連続な減衰力（衝撃）を防ぐため、
        # めり込み量に応じて減衰係数を0からフルダンピング（c_calc）まで線形に変化させます [N・s/m]
        damping_factor = min(1.0, penetration / damp_limit_calc)
        c_effective    = c_calc * damping_factor

        # 外部関数（calc_kelvin_voigt_force）の要素数に合わせるため、
        # スカラーの剛性・減衰係数を各成分に等しく拡張（等方性）します。
        k_iso = ones_vec * k_calc
        c_iso = ones_vec * c_effective

        # 横滑りによる不要な粘性力（接線方向のブレーキ）を防ぐため、
        # 相対速度と法線ベクトルの内積から「法線方向の速度成分」のみを抽出します [m/s]
        v_rel_n_scalar = _dot(v_rel, n)
        if not (-float("inf") < v_rel_n_scalar < float("inf")):
            raise ValueError(f"{func_name}: 計算結果が桁あふれしました。入力値の大きさを確認してください。")
        v_rel_n_vec = v_rel_n_scalar * n

        # 質点Bを基準（位置0・速度0）とし、質点Aが法線と逆向きにめり込み量（penetration）だけ
        # 押し込まれた相対的な仮想状態（位置 [m]・速度 [m/s]）を作成します。
        pos_v_a = -penetration * n
        v_rel_a = v_rel_n_vec
        pos_v_b = zero_vec
        v_rel_b = zero_vec

        # 等方性に拡張した剛性・減衰係数と仮想状態を calc_kelvin_voigt_force に渡し、反発力を計算します [N]
        f_a_res, f_b_res = calc_kelvin_voigt_force(k_iso, c_iso, pos_v_a, v_rel_a, pos_b=pos_v_b, v_b=v_rel_b)

        # 離脱時に粘性項によって引張力（吸着する向きの力）が生じないよう、
        # 計算された力ベクトルと法線ベクトルの内積から、力の向きを確認します [N]
        force_n = _dot(f_a_res, n)
        if not (-float("inf") < force_n < float("inf")):
            raise ValueError(f"{func_name}: 計算結果が桁あふれしました。入力値の大きさを確認してください。")

        # 内積が負（引張方向）になった場合は力を0とし、正（反発方向）の場合のみそのまま採用します [N]
        # （+ 0.0 により、力が0のとき -0.0 ではなく 0.0 を返します）
        if force_n < 0.0:
            f_a = zero_vec + 0.0
            f_b = zero_vec + 0.0
        else:
            f_a = f_a_res + 0.0
            f_b = f_b_res + 0.0
    else:
        # 非接触（めり込み量が0以下）の場合は、接触力を0とします [N]
        # （+ 0.0 により、-0.0 ではなく 0.0 を返します）
        f_a = zero_vec + 0.0
        f_b = zero_vec + 0.0

    # --- 4. 出力形式の調整 ---
    # 出力形式は pos_a の形式（型）のみで決定します。
    # 内部関数（_format_output_1dsim_com）が定義されている場合は、桁あふれを確認し、pos_a と同じ形式に変換します。
    if "_format_output_1dsim_com" in globals():
        f_a, f_b = _format_output_1dsim_com(pos_a, func_name, f_a, f_b)

    # すべてのエラーチェックが完了した後に、
    # pos_b, v_b を省略して pos_b_init（0以外）のみが指定されていた場合は警告を出します。
    # （pos_b_init がすべて0の場合は置き換えても結果が変わらないため、警告は出しません）
    if "warnings" in globals() and is_fixed_b_at_init:
        # _to_array_1dsim_com の有無にかかわらず判定できるよう、pos_b_init の各要素をリストに取り出します。
        raw_b_init = pos_b_init.tolist() if hasattr(pos_b_init, "tolist") else pos_b_init
        b_init_vals = raw_b_init if isinstance(raw_b_init, (list, tuple)) else [raw_b_init]
        if any(x != 0 for x in b_init_vals):
            warnings.warn(
                f"{func_name}: pos_b, v_bを省略してpos_b_initのみが指定されたため、"
                "質点Bをpos_b_initの位置に固定された点（pos_b = pos_b_init）として計算します。",
                stacklevel=2,  # 警告の発生箇所として、この関数の呼び出し元の行を表示します
            )

    # 計算結果を返します（_format_output_1dsim_com を使用しない場合は、数値またはndarrayのまま返ります）。
    return f_a, f_b

# ===== 1次元のテスト（スカラー入力） =====
res_1d = calc_contact_force(0.02, -0.5, 0.05, 0.02, k=100.0, c=10.0, damp_limit=0.005)
print("1D Result:", res_1d, "\n")

# ===== 多次元のテスト（ndarray入力） =====
res_nd = calc_contact_force(
    np.array([2.0, 3.0]), np.array([1.0, -1.0]), np.array([0.0, 5.0]), 1.0,
    pos_b=np.array([2.5, 2.5]), v_b=np.array([-1.0, 1.0]), pos_b_init=np.array([5.0, 0.0]),
    k=50.0, c=5.0, damp_limit=1.0
)
print("ndarray Result:", res_nd)