src/Core/e_physics.c - master - Zamar_Terrier/TigorEngine

src/Core/e_physics.c
543 строки21 KB
Zamar_Terrier
Офигенное обновление движка
31 июл 2026, 21:03
31 июл 2026, 21:031682686
#include "Core/e_physics.h"
#include "Tools/e_math.h"
#include "Tools/intersections3D.h"
#include <float.h>
#include <math.h>
#include <string.h>
#include <stdio.h>

// ============ НАСТРОЙКИ ОТЛАДКИ ============
#define DEBUG_PHYSICS 0  // Установите в 0 для релиза (отключает вывод в консоль)
#define DEBUG_PRINT_INTERVAL 30 

#if DEBUG_PHYSICS
static int g_debug_frame = 0;
#define DEBUG_LOG(...) \
    do { \
        if (++g_debug_frame % DEBUG_PRINT_INTERVAL == 0) { \
            printf(__VA_ARGS__); \
        } \
    } while(0)
#else
#define DEBUG_LOG(...) ((void)0)
#endif
// ============================================

// ============ НАСТРОЙКИ АРКАДНОЙ ФИЗИКИ ============
#define MAX_PHY_OBJS     64
#define MAX_COLLISIONS   256
#define SUBSTEPS         8
#define IMPULSE_ITER     6

// ИСПРАВЛЕНО: Стандартная гравитация. -98.81 вызывала слишком сильные удары о пол.
#define GRAVITY_CONST    vec3_f(0.0f, -48.81f, 0.0f)
#define LINEAR_DAMPING   0.95f  // Чуть увеличено для более плавного затухания
#define ANGULAR_DAMPING  0.90f
#define BOUNCE           0.2f
#define FRICTION         0.9f

#define SLEEP_THRESHOLD 0.15f  // Порог скорости для засыпания
#define SLEEP_FRAMES 20        // Кадров покоя перед засыпанием

#define MAX_VELOCITY 50.0f // Добавьте этот макрос в начало файла

typedef struct {
    RigidBody *rigids[MAX_PHY_OBJS];
    uint32_t num_rigids;
} ColliderPair;

static ColliderPair collidersA;
static ColliderPair collidersB;
static CollisionManifold collisions[MAX_COLLISIONS];
static uint32_t num_collisions = 0;

static RigidBody phy_objs[MAX_PHY_OBJS];
static uint32_t num_phy_objs = 0;

// ---- ВСПОМОГАТЕЛЬНЫЕ ФУНКЦИИ ----
float CorrectDegrees(float degrees) {
    while (degrees > 360.0f) degrees -= 360.0f;
    while (degrees < -360.0f) degrees += 360.0f;
    return degrees;
}

float RAD2DEG(float radians) {
    float degrees = radians * 57.295754f;
    return CorrectDegrees(degrees);
}

static float RigidBodyInvMass(RigidBody *rb) {
    if (rb->mass <= 0.0f || !rb->isDynamic) return 0.0f;
    return 1.0f / rb->mass;
}

static mat3 RigidBodyInvTensor(RigidBody *rb) {
    mat3 zero; memset(&zero, 0, sizeof(mat3));
    if (rb->mass <= 0.0f || !rb->isDynamic) return zero;

    float ix = 0, iy = 0, iz = 0;
    if (rb->type == TIGOR_RIGIDBODY_TYPE_BOX) {
        vec3 s = rb->params.size; // ПОЛНЫЕ размеры (width, height, depth)
        float m = rb->mass;
        // Математически верная формула для ПОЛНЫХ размеров: 1/12 * m * (h^2 + d^2)
        ix = 1.0f / (m * (s.y * s.y + s.z * s.z) / 12.0f);
        iy = 1.0f / (m * (s.x * s.x + s.z * s.z) / 12.0f);
        iz = 1.0f / (m * (s.x * s.x + s.y * s.y) / 12.0f);
    } else {
        float r2 = rb->params.radius * rb->params.radius;
        float I = r2 * rb->mass * 0.4f; // 2/5 * m * r^2 для сферы
        ix = iy = iz = (I > 0.0f) ? (1.0f / I) : 0.0f;
    }

    mat3 localInv; memset(&localInv, 0, sizeof(mat3));
    localInv.m[0][0] = ix; localInv.m[1][1] = iy; localInv.m[2][2] = iz;

    mat3 R = rb->params.orientation_mat;
    mat3 Rt = mat3_transpose(R);
    return m3_mult(m3_mult(R, localInv), Rt);
}

void RigidBodySynchCollisionVolumes(RigidBody *rb) {
    rb->params.orientation_mat = quat_to_mat3(rb->params.orientation);
    rb->params.rotating = quat_to_euler(rb->params.orientation);
}

static vec3 RigidBodyClosestPoint(const ColParams *obb, const vec3 *point) {
    vec3 result = obb->position;
    vec3 dir = v3_sub(*point, obb->position);
    float *arr = (float *)&obb->size;

    for (int i = 0; i < 3; ++i) {
        const float *orientation = &obb->orientation_mat.arr[i * 3];
        vec3 axis = vec3_f(orientation[0], orientation[1], orientation[2]);
        float distance = v3_dot(dir, axis);
        if (distance > arr[i]) distance = arr[i];
        if (distance < -arr[i]) distance = -arr[i];
        result = v3_add(result, v3_muls(axis, distance));
    }
    return result;
}

// ---- ДЕТЕКЦИЯ КОЛЛИЗИЙ ----
static bool IntervalsOverlap(float min1, float max1, float min2, float max2) {
    return (min1 <= max2) && (min2 <= max1);
}

static void RigidBodyGetInterval(const ColParams *obb, const vec3 *axis, float *outMin, float *outMax) {
    float projC = v3_dot(obb->position, *axis);
    const float *o = obb->orientation_mat.arr;
    vec3 A[] = { {o[0], o[1], o[2]}, {o[3], o[4], o[5]}, {o[6], o[7], o[8]} };
    
    float radius = fabsf(v3_dot(A[0], *axis)) * obb->size.x +
                   fabsf(v3_dot(A[1], *axis)) * obb->size.y +
                   fabsf(v3_dot(A[2], *axis)) * obb->size.z;

    *outMin = projC - radius;
    *outMax = projC + radius;
}

static vec3 GetDeepestVertex(const ColParams *A, const ColParams *B, const vec3 *normal) {
    vec3 C = A->position;
    vec3 E = A->size;
    const float *o = A->orientation_mat.arr;
    vec3 axes[3] = { {o[0], o[1], o[2]}, {o[3], o[4], o[5]}, {o[6], o[7], o[8]} };
    vec3 t[3] = { v3_muls(axes[0], E.x), v3_muls(axes[1], E.y), v3_muls(axes[2], E.z) };

    vec3 verts[8];
    verts[0] = v3_add(v3_add(v3_add(C, t[0]), t[1]), t[2]);
    verts[1] = v3_add(v3_add(v3_sub(C, t[0]), t[1]), t[2]);
    verts[2] = v3_add(v3_sub(v3_add(C, t[0]), t[1]), t[2]);
    verts[3] = v3_sub(v3_add(v3_add(C, t[0]), t[1]), t[2]);
    verts[4] = v3_sub(v3_sub(v3_sub(C, t[0]), t[1]), t[2]);
    verts[5] = v3_sub(v3_sub(v3_add(C, t[0]), t[1]), t[2]);
    verts[6] = v3_sub(v3_add(v3_sub(C, t[0]), t[1]), t[2]);
    verts[7] = v3_add(v3_sub(v3_sub(C, t[0]), t[1]), t[2]);

    vec3 deepest = verts[0];
    float maxProj = -FLT_MAX;

    // ИСПРАВЛЕНИЕ: Находим не просто самую глубокую вершину,
    // а ту, которая находится дальше всего от центра масс B в направлении "от B"
    vec3 dirAwayFromB = v3_sub(A->position, B->position);
    float dirLen = v3_magnitude(&dirAwayFromB);
    if (dirLen > 0.001f) {
        dirAwayFromB = v3_muls(dirAwayFromB, 1.0f / dirLen);
    } else {
        dirAwayFromB = vec3_f(1.0f, 0.0f, 0.0f);
    }

    for (int i = 0; i < 8; ++i) {
        float proj = v3_dot(verts[i], *normal);
        // Дополнительный вес: насколько вершина удалена от B в направлении "наружу"
        float edgeWeight = v3_dot(v3_sub(verts[i], B->position), dirAwayFromB);
        float score = proj + edgeWeight * 0.3f; // Коэффициент влияния
        
        if (score > maxProj) {
            maxProj = score;
            deepest = verts[i];
        }
    }
    return deepest;
}

static CollisionManifold FindCollisionFeaturesOBBOOBB(const ColParams *A, const ColParams *B) {
    CollisionManifold result;
    memset(&result, 0, sizeof(CollisionManifold));
    result.depth = FLT_MAX;
    result.colliding = false;

    const float *o1 = A->orientation_mat.arr;
    const float *o2 = B->orientation_mat.arr;

    vec3 test[15] = {
        {o1[0], o1[1], o1[2]}, {o1[3], o1[4], o1[5]}, {o1[6], o1[7], o1[8]},
        {o2[0], o2[1], o2[2]}, {o2[3], o2[4], o2[5]}, {o2[6], o2[7], o2[8]}
    };
    for (int i = 0; i < 3; ++i) {
        test[6 + i * 3 + 0] = v3_cross(test[i], test[3]);
        test[6 + i * 3 + 1] = v3_cross(test[i], test[4]);
        test[6 + i * 3 + 2] = v3_cross(test[i], test[5]);
    }

    vec3 bestNormal = vec3_f(0, 1, 0);
    float minDepth = FLT_MAX;

    for (int i = 0; i < 15; ++i) {
        float lenSq = v3_magnitudesq(&test[i]);
        if (lenSq < 0.0001f) continue;

        vec3 axis = v3_muls(test[i], 1.0f / sqrtf(lenSq));
        float min1, max1, min2, max2;
        RigidBodyGetInterval(A, &axis, &min1, &max1);
        RigidBodyGetInterval(B, &axis, &min2, &max2);

        if (!IntervalsOverlap(min1, max1, min2, max2)) {
            return result;
        }

        float depth = fminf(max1, max2) - fmaxf(min1, min2);
        if (depth < minDepth) {
            minDepth = depth;
            bestNormal = axis;
            vec3 dirAB = v3_sub(B->position, A->position);
            if (v3_dot(bestNormal, dirAB) < 0.0f) {
                bestNormal = v3_muls(bestNormal, -1.0f);
            }
        }
    }

    result.colliding = true;
    result.depth = minDepth;
    result.normal = bestNormal;

	if (fabsf(result.normal.y) > 0.9f) {
        result.normal = vec3_f(0.0f, result.normal.y > 0.0f ? 1.0f : -1.0f, 0.0f);
    }

    result.num_contacts = 1;
    result.contacts[0] = GetDeepestVertex(A, B, &bestNormal);

    DEBUG_LOG("[COLLISION] Depth: %.4f | Normal(%.2f, %.2f, %.2f)\n", 
              result.depth, result.normal.x, result.normal.y, result.normal.z);

    return result;
}

static CollisionManifold FindCollisionFeaturesSphereSphere(const ColParams *A, const ColParams *B) {
    CollisionManifold result;
    memset(&result, 0, sizeof(CollisionManifold));
    vec3 d = v3_sub(B->position, A->position);
    float distSq = v3_magnitudesq(&d);
    float r = A->radius + B->radius;
    if (distSq >= r * r || distSq == 0.0f) return result;
    
    float dist = sqrtf(distSq);
    result.colliding = true;
    result.normal = v3_muls(d, 1.0f / dist);
    result.depth = r - dist;
    result.num_contacts = 1;
    result.contacts[0] = v3_add(A->position, v3_muls(result.normal, A->radius - result.depth * 0.5f));
    return result;
}

static CollisionManifold FindCollisionFeaturesOBBSphere(const ColParams *obb, const ColParams *sphere) {
    CollisionManifold result;
    memset(&result, 0, sizeof(CollisionManifold));
    vec3 closest = RigidBodyClosestPoint(obb, &sphere->position);
    vec3 d = v3_sub(sphere->position, closest);
    float distSq = v3_magnitudesq(&d);
    if (distSq >= sphere->radius * sphere->radius) return result;
    
    float dist = sqrtf(distSq);
    result.colliding = true;
    result.num_contacts = 1;
    result.contacts[0] = closest;
    if (dist < 0.001f) {
        result.normal = vec3_f(0, 1, 0);
        result.depth = sphere->radius;
    } else {
        result.normal = v3_muls(d, 1.0f / dist);
        result.depth = sphere->radius - dist;
    }
    return result;
}

static CollisionManifold FindCollisionFeatures(RigidBody *ra, RigidBody *rb) {
    CollisionManifold result;
    memset(&result, 0, sizeof(CollisionManifold));
    if (ra->type == TIGOR_RIGIDBODY_TYPE_SPHERE) {
        if (rb->type == TIGOR_RIGIDBODY_TYPE_SPHERE) result = FindCollisionFeaturesSphereSphere(&ra->params, &rb->params);
        else result = FindCollisionFeaturesOBBSphere(&rb->params, &ra->params);
    } else {
        if (rb->type == TIGOR_RIGIDBODY_TYPE_BOX) result = FindCollisionFeaturesOBBOOBB(&ra->params, &rb->params);
        else result = FindCollisionFeaturesOBBSphere(&ra->params, &rb->params);
    }
    return result;
}

// ---- РАЗРЕШЕНИЕ КОЛЛИЗИЙ ----
static void RigidBodyApplyImpulse(RigidBody *A, RigidBody *B, const CollisionManifold *M, float subDt) {
    if (M->num_contacts == 0) return;
    if (A->isSleeping && B->isSleeping) return;

    vec3 contact = M->contacts[0];
    vec3 r1 = v3_sub(contact, A->params.position);
    vec3 r2 = v3_sub(contact, B->params.position);
    float invMass1 = RigidBodyInvMass(A);
    float invMass2 = RigidBodyInvMass(B);
    if (invMass1 + invMass2 == 0.0f) return;

    mat3 i1 = RigidBodyInvTensor(A);
    mat3 i2 = RigidBodyInvTensor(B);

    vec3 vel1 = v3_add(A->velocity, v3_cross(A->angVel, r1));
    vec3 vel2 = v3_add(B->velocity, v3_cross(B->angVel, r2));
    vec3 relVel = v3_sub(vel2, vel1);
    vec3 normal = M->normal;
    float vn = v3_dot(relVel, normal);
    
    if (vn > 0.0f) return; // Объекты уже разлетаются

    if (fabsf(vn) > 0.5f) { // СНИЖЕН ПОРОГ пробуждения
        A->isSleeping = false; A->sleepCounter = 0;
        B->isSleeping = false; B->sleepCounter = 0;
    }

    vec3 r1xn = v3_cross(r1, normal);
    vec3 r2xn = v3_cross(r2, normal);
    float invMassTerm = invMass1 + invMass2 + 
                        v3_dot(r1xn, m3_v3_mult_physics(i1, r1xn)) + 
                        v3_dot(r2xn, m3_v3_mult_physics(i2, r2xn));
    if (invMassTerm < 1e-5f) return;
    
    // === КЛЮЧЕВОЕ ИСПРАВЛЕНИЕ: Стабилизация Баумгарте ===
    float slop = 0.02f;          // УВЕЛИЧЕНО с 0.01f (допускаем микро-проникновение)
    float percent = 0.15f;       // УМЕНЬШЕНО с 0.2f (меньше агрессивности)
    
    float v_bias = (percent / subDt) * fmaxf(M->depth - slop, 0.0f);
    
    // ИСПРАВЛЕНИЕ 3: Ограничиваем максимальную скорость коррекции, чтобы не было "подпрыгиваний"
    if (v_bias > 12.0f) v_bias = 12.0f; 
    
    float e = (vn < -0.5f) ? BOUNCE : 0.0f;
    float jn = (-(1.0f + e) * vn + v_bias) / invMassTerm;
    if (jn < 0.0f) jn = 0.0f;
    
    vec3 impulse = v3_muls(normal, jn);

    // ... (далее код трения остается без изменений) ...
    vec3 tangent = v3_sub(relVel, v3_muls(normal, vn));
    float tanLen = v3_magnitude(&tangent);
    if (tanLen > 0.001f) {
        vec3 tDir = v3_muls(tangent, 1.0f / tanLen);
        vec3 r1xt = v3_cross(r1, tDir);
        vec3 r2xt = v3_cross(r2, tDir);
        float invMassTermT = invMass1 + invMass2 + 
                             v3_dot(r1xt, m3_v3_mult_physics(i1, r1xt)) + 
                             v3_dot(r2xt, m3_v3_mult_physics(i2, r2xt));
        float jt = -v3_dot(relVel, tDir) / invMassTermT;
        
        float maxJt = FRICTION * jn;
        if (jt > maxJt) jt = maxJt;
        else if (jt < -maxJt) jt = -maxJt;
        
        impulse = v3_add(impulse, v3_muls(tDir, jt));
    }

    if (A->isDynamic && !A->isSleeping) {
        A->velocity = v3_sub(A->velocity, v3_muls(impulse, invMass1));
        A->angVel = v3_sub(A->angVel, m3_v3_mult_physics(i1, v3_cross(r1, impulse)));
    }
    if (B->isDynamic && !B->isSleeping) {
        B->velocity = v3_add(B->velocity, v3_muls(impulse, invMass2));
        B->angVel = v3_add(B->angVel, m3_v3_mult_physics(i2, v3_cross(r2, impulse)));
    }
}

static void ResolvePosition(RigidBody *A, RigidBody *B, const CollisionManifold *M) {
    float invMass1 = RigidBodyInvMass(A);
    float invMass2 = RigidBodyInvMass(B);
    float totalInvMass = invMass1 + invMass2;
    if (totalInvMass == 0.0f) return;

    float percent = 0.05f;  
    float slop = 0.01f;     
    
    float depth = fmaxf(M->depth - slop, 0.0f);
    if (depth <= 0.0f) return;

    // ИСПРАВЛЕНО: Используем истинную нормаль контакта для сохранения геометрической корректности
    vec3 correction = v3_muls(M->normal, (depth / totalInvMass) * percent);

    if (A->isDynamic && !A->isSleeping) {
        A->params.position = v3_sub(A->params.position, v3_muls(correction, invMass1));
        RigidBodySynchCollisionVolumes(A);
    }
    if (B->isDynamic && !B->isSleeping) {
        B->params.position = v3_add(B->params.position, v3_muls(correction, invMass2));
        RigidBodySynchCollisionVolumes(B);
    }
}

// ---- ГЛАВНЫЙ ЦИКЛ ОБНОВЛЕНИЯ ----
void PhysicsUpdate(float deltaTime) {
    const float MAX_DT = 0.05f;
    float dt = fminf(deltaTime, MAX_DT);
    float subDt = dt / SUBSTEPS;

	float subLinearDamping = powf(LINEAR_DAMPING, 1.0f / SUBSTEPS);
    float subAngularDamping = powf(ANGULAR_DAMPING, 1.0f / SUBSTEPS);


    for (int step = 0; step < SUBSTEPS; ++step) {
        // 1. Применяем силы и интегрируем вращение
        for (uint32_t i = 0; i < num_phy_objs; ++i) {
            RigidBody *rb = &phy_objs[i];
            if (rb->isDynamic && !rb->isSleeping) {
                rb->velocity = v3_add(rb->velocity, v3_muls(GRAVITY_CONST, subDt));
                
                if (rb->type == TIGOR_RIGIDBODY_TYPE_BOX) {
                    mat3 invTensor = RigidBodyInvTensor(rb);
                    vec3 angAccel = m3_v3_mult_physics(invTensor, rb->torques);
                    rb->angVel = v3_add(rb->angVel, v3_muls(angAccel, subDt));
                }
            }
        }

        num_collisions = 0;
        collidersA.num_rigids = 0;
        collidersB.num_rigids = 0;

        // 2. Детекция коллизий
        for (uint32_t i = 0; i < num_phy_objs; ++i) {
            for (uint32_t j = i + 1; j < num_phy_objs; ++j) {
                if (num_collisions >= MAX_COLLISIONS) break;
                if (!phy_objs[i].isDynamic && !phy_objs[j].isDynamic) continue;
                
                // Пропускаем проверку, если оба спят, или если спящий объект проверяется со статикой
                if (phy_objs[i].isSleeping && phy_objs[j].isSleeping) continue;
                if (phy_objs[i].isSleeping && !phy_objs[j].isDynamic) continue;
                if (phy_objs[j].isSleeping && !phy_objs[i].isDynamic) continue;

                CollisionManifold M = FindCollisionFeatures(&phy_objs[i], &phy_objs[j]);
                if (M.colliding) {
                    collidersA.rigids[collidersA.num_rigids] = &phy_objs[i];
                    collidersB.rigids[collidersB.num_rigids] = &phy_objs[j];
                    collisions[num_collisions] = M;
                    num_collisions++;
                    collidersA.num_rigids++;
                    collidersB.num_rigids++;
                }
            }
        }

        // 3. Разрешение коллизий (импульсы)
        for (int iter = 0; iter < IMPULSE_ITER; ++iter) {
            for (uint32_t i = 0; i < num_collisions; ++i) {
                RigidBodyApplyImpulse(collidersA.rigids[i], collidersB.rigids[i], &collisions[i], subDt);
            }
        }

        // 4. Разрешение позиций (выталкивание)
        for (uint32_t i = 0; i < num_collisions; ++i) {
            ResolvePosition(collidersA.rigids[i], collidersB.rigids[i], &collisions[i]);
        }

        // 5. Интегрируем позицию ПОСЛЕ решения коллизий
		for (uint32_t i = 0; i < num_phy_objs; ++i) {
            RigidBody *rb = &phy_objs[i];
            if (rb->isDynamic && !rb->isSleeping) {
                // Используем рассчитанное значение для подшага
                rb->velocity = v3_muls(rb->velocity, subLinearDamping);
                rb->angVel = v3_muls(rb->angVel, subAngularDamping);

                float speedSq = v3_magnitudesq(&rb->velocity);
                if (speedSq > MAX_VELOCITY * MAX_VELOCITY) {
                    float scale = MAX_VELOCITY / sqrtf(speedSq);
                    rb->velocity = v3_muls(rb->velocity, scale);
                }

                rb->params.position = v3_add(rb->params.position, v3_muls(rb->velocity, subDt));
                rb->params.orientation = quat_integrate(rb->params.orientation, rb->angVel, subDt);
                
                rb->forces = vec3_f(0, 0, 0);
                rb->torques = vec3_f(0, 0, 0);
                
                RigidBodySynchCollisionVolumes(rb);
            }
        }
    }

    // 6. Проверка на засыпание ПОСЛЕ всех подшагов
    for (uint32_t i = 0; i < num_phy_objs; ++i) {
        RigidBody *rb = &phy_objs[i];
        if (rb->isDynamic && !rb->isSleeping) {
            float speed = v3_magnitude(&rb->velocity);
            float angSpeed = v3_magnitude(&rb->angVel);
            
            if (speed < SLEEP_THRESHOLD && angSpeed < SLEEP_THRESHOLD) {
                rb->sleepCounter++;
                if (rb->sleepCounter > SLEEP_FRAMES) {
                    rb->isSleeping = true;
                    rb->velocity = vec3_f(0, 0, 0);
                    rb->angVel = vec3_f(0, 0, 0);
                    
                    // Выравниваем куб по горизонтали для стабильности на плоских поверхностях
                    vec3 euler = quat_to_euler(rb->params.orientation);
                    rb->params.orientation = quat_from_euler(vec3_f(0.0f, euler.y, 0.0f));
                    RigidBodySynchCollisionVolumes(rb);
                }
            } else {
                rb->sleepCounter = 0;
            }
        }
    }
}

// ---- ИНИЦИАЛИЗАЦИЯ И ОЧИСТКА ----
RigidBody *PhysicsInitObject(uint32_t type, bool isDynamic) {
    if (num_phy_objs >= MAX_PHY_OBJS) return NULL;
    RigidBody *res = &phy_objs[num_phy_objs];
    memset(res, 0, sizeof(RigidBody));
    res->type = type;
    res->isDynamic = isDynamic;
    res->params.radius = 1.0f;
    res->params.size = vec3_f(1.0f, 1.0f, 1.0f);
    res->mass = isDynamic ? 1.0f : 0.0f;
    res->friction = FRICTION;
    res->cor = BOUNCE;
    res->params.orientation = vec4_f(0.0f, 0.0f, 0.0f, 1.0f);
    res->params.orientation_mat = mat3_f();
	
	RigidBodySynchCollisionVolumes(res);

    res->isSleeping = false;
    res->sleepCounter = 0;
    num_phy_objs++;
    return res;
}

void PhysicsClear() {
    memset(phy_objs, 0, sizeof(RigidBody) * MAX_PHY_OBJS);
    num_phy_objs = 0;
}