game2001/server/gameserver/collision.cc

#include "precompile.h"

#include <algorithm>
#include <cfloat>
#include <glm/gtx/intersect.hpp>
#include <glm/vec3.hpp>

#include "collision.h"

bool IntersectSegmentCircle(Vector2D p0, Vector2D p1, Vector2D pos, float rad)
{
    Vector2D t = p1 - p0;
    float d = std::max(t.Norm(), 0.0001f);
    Vector2D n = p0 - pos;
    float v = n.Dot(t);
    float z = n.Dot(n) - rad*rad;
    if (z > 0 && v > 0) {
        return false;
    }
    float u = v*v - z;
    if (u < 0) {
        return false;
    }
    float i = sqrt(u);
    float x = -v - i;
    if (x < 0 && (x = -v + i), x <= d) {
        return true;
    }
    return false;
}

bool IntersectSegmentAabb(Vector2D p0, Vector2D p1, Vector2D _min, Vector2D _max)
{
#if 1
    Vector2D c = (_min + _max) * 0.5f;
    Vector2D e = _max - c;
    Vector2D m = (p0 + p1) * 0.5f;
    Vector2D d = p1 - m;
    m = m - c;

    float adx = std::abs(d.x);
    if (std::abs(m.x) > e.x + adx) {
        return false;
    }
    float ady = std::abs(d.y);
    if (std::abs(m.y) > e.y + ady) {
        return false;
    }
#if 1
    if (p0.x > p1.x) {
        float tmp = p0.x;
        p0.x = p1.x;
        p1.x = tmp;
    }
    if (p0.y > p1.y) {
        float tmp = p0.y;
        p0.y = p1.y;
        p1.y = tmp;
    }
    return IntersectAabbAabb(p0, p1, _min, _max);
#else
    return true;
#endif
#else
    float t = 0.0f;
    float d = FLT_MAX;
    Vector2D n = p0;
    Vector2D v = p1 - p0;
    float z = v.Norm();
    v = z > (float)1e-5 ? v / z : Vector2D(1, 0);
    if (std::abs(v.x) < (float)1e-5) {
        v.x = float(2e-5);
    }
    if (std::abs(v.y) < (float)1e-5) {
        v.y = (float)2e-5;
    }
    if (std::abs(v.x) > (float)1e-5) {
        float u = (_min.x - n.x) / v.x;
        float i = (_max.x - n.x) / v.x;
        t = std::max(t, std::min(u, i));
        d = std::min(d, std::max(u, i));
        if (t > d) {
            return false;
        }
    }
    if (std::abs(v.y) > (float)1e-5) {
        float x = (_min.y - n.y) / v.y;
        float l = (_max.y - n.y) / v.y;
        t = std::max(t, std::min(x, l));
        d = std::min(d, std::max(x, l));
        if (t > d) {
            return false;
        }
    }
    if (t > z) {
        return false;
    }
    return true;
#endif
}

bool IntersectAabbCircle(Vector2D a_min, Vector2D a_max, Vector2D b_pos, float b_rad)
{
    if (b_pos.x >= a_min.x && b_pos.x <= a_max.x &&
        b_pos.y >= a_min.y && b_pos.y <= a_max.y) {
        return true;
    }
    Vector2D nearest_point(a8::Clamp(b_pos.x, a_min.x, a_max.x),
                           a8::Clamp(b_pos.y, a_min.y, a_max.y));
    Vector2D i = b_pos - nearest_point;
    float n = a8::LengthSqr(i);
    return n < b_rad*b_rad;
}

bool IntersectAabbAabb(Vector2D a_min, Vector2D a_max, Vector2D b_min, Vector2D b_max)
{
    Vector2D a_v = (a_max - a_min) * 0.5f;
    Vector2D a_center = a_min + a_v;
    Vector2D b_v = (b_max - b_min) * 0.5f;
    Vector2D b_center = b_min + b_v;
    Vector2D z = b_center - a_center;
    float u = a_v.x + b_v.x - std::abs(z.x);
    return u > 0;
}

bool IntersectCircleCircle(Vector2D a_pos, float a_rad, Vector2D b_pos, float b_rad)
{
    float t = a_rad + b_rad;
    Vector2D d = b_pos - a_pos;
    float n = a8::LengthSqr(d);
    return n < t*t;
}

bool CircleContainCircle(Vector2D a_pos, float a_rad, Vector2D b_pos, float b_rad)
{
    float distance = (a_pos - b_pos).Norm();
    return distance < a_rad - b_rad;
}

bool CalcCircleAabbSafePoint(Vector2D a_pos, float b_rad, Vector2D b_min, Vector2D b_max,
                             Vector2D& new_pos)
{
    new_pos = a_pos;
    bool at_left = std::abs(a_pos.x - b_min.x) < std::abs(a_pos.x - b_max.x);
    bool at_down = std::abs(a_pos.y - b_min.y) < std::abs(a_pos.y - b_max.y);
    float x_len = at_left ? std::abs(a_pos.x - b_min.x) : std::abs(a_pos.x - b_max.x);
    float y_len = at_down ? std::abs(a_pos.y - b_min.y) : std::abs(a_pos.y - b_max.y);
    if (at_left) {
        if (x_len < y_len) {
            //左
            new_pos.x = b_min.x - b_rad - 1;
        } else {
            if (at_down) {
                new_pos.y = b_min.y - b_rad - 1;
            } else {
                new_pos.y = b_max.y + b_rad + 1;
            }
        }
    } else {
        if (x_len < y_len) {
            //右
            new_pos.x = b_max.x + b_rad + 1;
        } else {
            if (at_down) {
                new_pos.y = b_min.y - b_rad - 1;
            } else {
                new_pos.y = b_max.y + b_rad + 1;
            }
        }
    }
    return true;
}

bool CalcCircleCircleSafePoint(Vector2D a_pos, float a_rad, Vector2D b_pos, float b_rad,
                               Vector2D& new_pos)
{
    Vector2D dir = a_pos - b_pos;
    dir.Normalize();
    new_pos = b_pos + dir*(a_rad + b_rad) + 1;
    return true;
}

bool CalcAabbAabbSafePoint(Vector2D a_min, Vector2D a_max, Vector2D b_min, Vector2D b_max,
                           Vector2D& new_pos)
{
    Vector2D a_pos = a_min + (a_max - a_min)/2.0f;
    float a_rad = (a_max - a_min).Norm();
    Vector2D b_pos = b_min + (b_max - b_min)/2.0f;
    float b_rad = (b_max - b_min).Norm();
    Vector2D dir = (a_pos - b_pos);
    dir.Normalize();
    new_pos = b_pos + dir*(a_rad + b_rad) + 1;
    return true;
}

bool CalcAabbCircleSafePoint(Vector2D a_min, Vector2D a_max, Vector2D b_pos, float b_rad,
                             Vector2D& new_pos)
{
    Vector2D a_pos = a_min + (a_max - a_min)/2.0f;
    float a_rad = (a_max - a_min).Norm();
    Vector2D dir = a_pos - b_pos;
    dir.Normalize();
    new_pos = b_pos + dir*(a_rad + b_rad) + 1;
    return true;
}