FcLibs/FcMath/src/FcMath_Collision.h - master - vTech/FcApps

FcLibs/FcMath/src/FcMath_Collision.h
84 строки4 KB
vTech
alpha_00
03 дек 2025, 17:45
03 дек 2025, 17:45ab8382d
#ifndef __FCMATH_COLLISION_H__
#define __FCMATH_COLLISION_H__

typedef struct _FcMath_SphereV_t {
    FcVec3f_t mPosition;
    FcVec3f_t mVelocity;
    float mRadius;
    float mReserved;
} FcMath_SphereV_t;

typedef struct _FcMath_Sphere_t {
    FcVec3f_t mPosition;
    float mRadius;
} FcMath_Sphere_t;

typedef struct _FcMath_Circle_t {
    FcVec2f_t mPosition;
    float mRadius;
    float mReserved;
} FcMath_Circle_t;

FCDEF bool FcMath_Collision_Sphere_collide(const FcMath_Sphere_t* p0, const FcMath_Sphere_t* p1);
FCDEF bool FcMath_Collision_Sphere_collideV(const FcMath_SphereV_t* p0, const FcMath_SphereV_t* p1);
FCDEF void FcMath_Collision_Sphere_performCollision(FcMath_SphereV_t* p0, FcMath_SphereV_t* p1);

FCDEF bool FcMath_Collision_Sphere_collideV(const FcMath_SphereV_t* p0, const FcMath_SphereV_t* p1) {
    FcVec3f_t mDistance = FcVec3f_sub(p0->mPosition, p1->mPosition);
    float rSquared = p0->mRadius + p1->mRadius;
    float dSquared = FcVec3f_dot(mDistance, mDistance);
    rSquared *= rSquared;
    return dSquared < rSquared;
}

FCDEF bool FcMath_Collision_Sphere_collide(const FcMath_Sphere_t* p0, const FcMath_Sphere_t* p1) {
    FcVec3f_t mDistance = FcVec3f_sub(p0->mPosition, p1->mPosition);
    float rSquared = p0->mRadius + p1->mRadius;
    float dSquared = FcVec3f_dot(mDistance, mDistance);
    rSquared *= rSquared;
    return dSquared < rSquared;
}

FCDEF FcVec4f_t FcMath_Collision_Circle_collideCoord_vec2x2(const FcMath_Circle_t* p0, const FcMath_Circle_t* p1) {
    FcVec4f_t mIntersection = FcVec4f(NAN, NAN, NAN, NAN);
    FcVec2f_t mDistance = FcVec2f_sub(p0->mPosition, p1->mPosition);
    float rSquared = p0->mRadius + p1->mRadius;
    float dSquared = FcVec2f_dot(mDistance, mDistance);
    float mLength = sqrt(dSquared);
    rSquared *= rSquared;
    if (dSquared <= rSquared){
        float a = (p0->mRadius * p0->mRadius - p1->mRadius * p1->mRadius + dSquared) / (2 * mLength);
        float h = sqrt(p0->mRadius * p0->mRadius - a * a);
        FcVec2f_t mCenterPoint = FcVec2f_add(p0->mPosition, FcVec2f_div(FcVec2f_muls( FcVec2f_sub(p1->mPosition, p0->mPosition), a), FcVec2f(mLength, mLength)));
        mIntersection.v[0].m[0] = p1->mPosition.m[1] - p0->mPosition.m[1];
        mIntersection.v[0].m[1] = p1->mPosition.m[0] - p0->mPosition.m[0];
        mIntersection.v[1].m[0] = p1->mPosition.m[1] - p0->mPosition.m[1];
        mIntersection.v[1].m[1] = p1->mPosition.m[0] - p0->mPosition.m[0];
        mIntersection.v[0] = FcVec2f_div(mIntersection.v[0], FcVec2f(mLength, mLength));
        mIntersection.v[1] = FcVec2f_div(mIntersection.v[1], FcVec2f(mLength, mLength));
        mIntersection.v[0] = FcVec2f_muls(mIntersection.v[0], h);
        mIntersection.v[1] = FcVec2f_muls(mIntersection.v[1], h);
        mIntersection.v[0].m[0] = mCenterPoint.m[0] + mIntersection.v[0].m[0];
        mIntersection.v[0].m[1] = mCenterPoint.m[1] - mIntersection.v[0].m[1];
        mIntersection.v[1].m[0] = mCenterPoint.m[0] - mIntersection.v[1].m[0];
        mIntersection.v[1].m[1] = mCenterPoint.m[1] + mIntersection.v[1].m[1];
    }
    return mIntersection;
}

FCDEF void FcMath_Collision_Sphere_performCollision(FcMath_SphereV_t* p0, FcMath_SphereV_t* p1) {
    FcVec3f_t nv1;
    FcVec3f_t nv2;
    nv1 = p0->mVelocity;
    nv2 = p1->mVelocity;
    FcVec3f_add(nv1, FcVec3f_ProjectionAonB(p1->mVelocity, FcVec3f_sub(p1->mPosition, p0->mPosition)));
    FcVec3f_add(nv2, FcVec3f_ProjectionAonB(p0->mVelocity, FcVec3f_sub(p1->mPosition, p0->mPosition)));
    FcVec3f_sub(nv1, FcVec3f_ProjectionAonB(p0->mVelocity, FcVec3f_sub(p0->mPosition, p1->mPosition)));
    FcVec3f_sub(nv2, FcVec3f_ProjectionAonB(p1->mVelocity, FcVec3f_sub(p0->mPosition, p1->mPosition)));
    p0->mVelocity = nv1;
    p1->mVelocity = nv2;
}



#endif //__FCMATH_COLLISION_H__