src/python/contact.py - master - Loylik/fastsim

src/python/contact.py
141 строка3 KB
GAlekseyV
unify API between C++ and Python versions
28 янв 2026, 13:30
28 янв 2026, 13:30075d3ca
import math
from typing import Tuple


def compute_contact(
    a: float = 7.367E-3,
    b: float = 6.99E-3,
    G: float = 21E+10,
    xi_x: float = 0.00047,
    xi_y: float = 0.000001,
    fi: float = 0.0925,
    f_stat: float = 0.2,
    f_kin: float = 0.2,
    Fz: float = 106200.0,
    m: int = 10,
    n: int = 20,
) -> Tuple[float, float, float]:
    """Compute tangential contact forces Fx and Fy.

    This is a direct translation of the original C++ algorithm from
    `src/main.cpp`. Defaults match the original values.

    Returns (Fx, Fy).
    """
    return _compute_contact_impl(a, b, G, xi_x, xi_y, fi, f_stat, f_kin, Fz, m, n)


def _compute_contact_impl(
    a: float,
    b: float,
    G: float,
    xi_x: float,
    xi_y: float,
    fi: float,
    f_stat: float,
    f_kin: float,
    Fz: float,
    m: int,
    n: int,
) -> Tuple[float, float, float]:
    L1 = 0.630 / G * a
    L2 = 0.700 / G * a
    L3 = 0.520 / G * a

    c = math.sqrt(a * b)

    L = (abs(xi_x) * L1 + abs(xi_y) * L2 + c * abs(fi) * L3) / \
        math.sqrt(xi_x * xi_x + xi_y * xi_y + c * c * fi * fi)

    dy = 2.0 * b / m
    max_pz = 0.0
    Fx = 0.0
    Fy = 0.0

    for j in range(1, m + 1):
        y = -b + (j - 1.0 / 2.0) * dy
        x_L = a * math.sqrt(1.0 - y * y / (b * b))
        dx = 2.0 * x_L / n
        p_y = 0.0
        p_x = 0.0
        Z1 = False
        Z2 = True
        Z3 = True

        for k in range(1, n + 1):
            x = x_L - (k - 1.0 / 2.0) * dx
            f_local = f_kin if not Z1 else f_stat

            p_z = 2.0 * Fz * (x_L * x_L - x * x) / (math.pi * a * a * a * b)
            max_pz = max(p_z, max_pz)
            w_x = xi_x - fi * y
            w_y = xi_y + fi * (x + 1.0 / 2.0 * dx)
            p_x = p_x - w_x * dx / L
            p_y = p_y - w_y * dx / L
            p = math.hypot(p_x, p_y)

            if p > f_local * p_z:
                if p != 0.0:
                    p_x = p_x * f_kin * p_z / p
                    p_y = p_y * f_kin * p_z / p
                else:
                    p_x = 0.0
                    p_y = 0.0
                Z2 = False
                Z3 = False
            else:
                Z2 = True

            Fx += p_x * dx * dy
            Fy += p_y * dx * dy

    return Fx, Fy, max_pz


def calculate_contact_patch(
    normalForce: float,
    youngModulus: float,
    poissonRatio: float,
    wheelRadius: float,
    railRadius: float,
) -> Tuple[float, float, float, float]:
    """Calculate contact patch parameters.

    Returns (a, b, contactArea, maxPressure).
    """
    K1 = (1 - poissonRatio * poissonRatio) / (math.pi * youngModulus)
    K2 = (1 - poissonRatio * poissonRatio) / (2 * math.pi * youngModulus)
    K3 = 0.5 * (1 / wheelRadius + 1 / railRadius)
    K4 = 0.5 * ((1 / wheelRadius) - (1 / railRadius))

    theta = math.acos(K4 / K3)
    m = 1.018
    n = 0.9835

    a = m * ((3 * normalForce * math.pi * (K1 + K2) / (4 * K3)) ** (1/3))
    b = n * ((3 * normalForce * math.pi * (K1 + K2) / (4 * K3)) ** (1/3))

    contactArea = math.pi * a * b
    maxPressure = 3 * normalForce / (2 * contactArea)

    return a, b, contactArea, maxPressure


def compute_contact_new(
    a: float,
    b: float,
    xi_x: float,
    xi_y: float,
    fi: float,
    f_stat: float,
    f_kin: float,
    Fz: float,
    G: float,
    m: int = 10,
    n: int = 20,
) -> Tuple[float, float, float]:
    """Compute contact forces with unified API matching Python naming.

    Returns (Fx, Fy, maxPz).
    """
    return _compute_contact_impl(a, b, G, xi_x, xi_y, fi, f_stat, f_kin, Fz, m, n)