a8/a8/collision.cc

#include <a8/a8.h>

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

#include <a8/collision.h>

namespace a8
{
    bool IntersectSegmentCircle(a8::Vec2 p0, a8::Vec2 p1, a8::Vec2 pos, float rad)
    {
        a8::Vec2 t = p1 - p0;
        float d = std::max(t.Norm(), 0.0001f);
        a8::Vec2 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(a8::Vec2 p0, a8::Vec2 p1, a8::Vec2 _min, a8::Vec2 _max)
    {
        a8::Vec2 c = (_min + _max) * 0.5f;
        a8::Vec2 e = _max - c;
        a8::Vec2 m = (p0 + p1) * 0.5f;
        a8::Vec2 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 (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);
    }

    bool IntersectAabbCircle(a8::Vec2 a_min, a8::Vec2 a_max, a8::Vec2 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;
        }
        a8::Vec2 nearest_point(a8::Clamp(b_pos.x, a_min.x, a_max.x),
                               a8::Clamp(b_pos.y, a_min.y, a_max.y));
        a8::Vec2 i = b_pos - nearest_point;
        float n = a8::LengthSqr(i);
        return n < b_rad*b_rad;
    }

    bool IntersectAabbAabb(a8::Vec2 a_min, a8::Vec2 a_max, a8::Vec2 b_min, a8::Vec2 b_max)
    {
        a8::Vec2 a_v = (a_max - a_min) * 0.5f;
        a8::Vec2 a_center = a_min + a_v;
        a8::Vec2 b_v = (b_max - b_min) * 0.5f;
        a8::Vec2 b_center = b_min + b_v;
        a8::Vec2 z = b_center - a_center;
        float u1 = a_v.x + b_v.x - std::abs(z.x);
        float u2 = a_v.y + b_v.y - std::abs(z.y);
        return u1 > 0.000001f && u2 > 0.000001;
    }

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

    bool IntersectSectorCircle(a8::Vec2 a_pos, float angle, float a_rad, a8::Vec2 b_pos, float b_rad)
    {
        {
            a8::Vec2 d = b_pos - a_pos;
            float n = a8::LengthSqr(d);
            if (n < a_rad || n < b_rad) {
                return true;
            }
            if (n > a_rad + b_rad) {
                return false;
            }
        }
        {

        }
        return false;
    }

    bool IntersectSectorAabb(a8::Vec2 a_pos, float angle, float a_rad, a8::Vec2 b_min, a8::Vec2 b_max)
    {
        return false;
    }

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

    bool CalcCircleAabbSafePoint(a8::Vec2 a_pos, float b_rad, a8::Vec2 b_min, a8::Vec2 b_max,
                                 a8::Vec2& 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(a8::Vec2 a_pos, float a_rad, a8::Vec2 b_pos, float b_rad,
                                   a8::Vec2& new_pos)
    {
        a8::Vec2 dir = a_pos - b_pos;
        dir.Normalize();
        new_pos = b_pos + dir*(a_rad + b_rad) + 1;
        return true;
    }

    bool CalcAabbAabbSafePoint(a8::Vec2 a_min, a8::Vec2 a_max, a8::Vec2 b_min, a8::Vec2 b_max,
                               a8::Vec2& new_pos)
    {
        a8::Vec2 a_pos = a_min + (a_max - a_min)/2.0f;
        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 - (a_max.x - a_min.x)/2.0f - 1;
            } else {
                if (at_down) {
                    new_pos.y = b_min.y - (a_max.y - a_min.y)/2.0f - 1;
                } else {
                    new_pos.y = b_max.y + (a_max.y - a_min.y)/2.0f + 1;
                }
            }
        } else {
            if (x_len < y_len) {
                //右
                new_pos.x = b_max.x + (a_max.x - a_min.x)/2.0f + 1;
            } else {
                if (at_down) {
                    new_pos.y = b_min.y - (a_max.y - a_min.y)/2.0f - 1;
                } else {
                    new_pos.y = b_max.y + (a_max.y - a_min.y)/2.0f + 1;
                }
            }
        }
        return true;
    }

    bool CalcAabbCircleSafePoint(a8::Vec2 a_min, a8::Vec2 a_max, a8::Vec2 b_pos, float b_rad,
                                 a8::Vec2& new_pos)
    {
        a8::Vec2 a_pos = a_min + (a_max - a_min)/2.0f;
        new_pos = a_pos;
        bool at_left = std::abs(a_min.x - b_pos.x) < std::abs(a_max.x - b_pos.x);
        bool at_down = std::abs(a_min.y - b_pos.y) < std::abs(a_max.y - b_pos.y);
        float x_len = at_left ? std::abs(a_min.x - b_pos.x) : std::abs(a_max.x - b_pos.x);
        float y_len = at_down ? std::abs(a_min.y - b_pos.y) : std::abs(a_max.y - b_pos.y);
        if (at_left) {
            if (x_len < y_len) {
                //左
                new_pos.x = b_pos.x - (a_max.x - a_min.x)/2.0f - 1;
            } else {
                if (at_down) {
                    new_pos.y = b_pos.y - (a_max.y - a_min.y)/2.0f - 1;
                } else {
                    new_pos.y = b_pos.y + (a_max.y - a_min.y)/2.0f + 1;
                }
            }
        } else {
            if (x_len < y_len) {
                //右
                new_pos.x = b_pos.x + (a_max.x - a_min.x)/2.0f + 1;
            } else {
                if (at_down) {
                    new_pos.y = b_pos.y - (a_max.y - a_min.y)/2.0f - 1;
                } else {
                    new_pos.y = b_pos.y + (a_max.y - a_min.y)/2.0f + 1;
                }
            }
        }
        return true;
    }
}