円の衝突から反発させたい

フォーラム(掲示板)ルール
フォーラム(掲示板)ルールはこちら  ※コードを貼り付ける場合は [code][/code] で囲って下さい。詳しくはこちら
DaK3
記事: 1
登録日時: 2年前

円の衝突から反発させたい

#1

投稿記事 by DaK3 » 2年前

DXライブラリでc言語を使用し円同士の衝突に関するシステムを作っています。
https://hakuhin.jp/as/collide.html
こちらのサイトを参考にしながら衝突から反発まで作っていたのですが、上手くいかなかったです。
どこに問題があるか調べても私ではわからなく質問させていただきました。

円の中心座標をpx,pyとしています。
実行するとpy座標が高速でプラスされていき座標が#QNANOになってしまいます。
実行してもエラーメッセージは出ませんでした。

コード:

#include "DxLib.h"
#include <math.h>

const int WIN_WIDTH = 600; //ウィンドウ横幅
const int WIN_HEIGHT = 400;//ウィンドウ縦幅


int WINAPI WinMain(_In_ HINSTANCE hInstance, _In_opt_ HINSTANCE hPrevInstance, _In_ LPSTR lpCmdLine, _In_ int nCmdShow)
{
    ChangeWindowMode(TRUE);                        //ウィンドウモードに設定
    //ウィンドウサイズを手動では変更できず、かつウィンドウサイズに合わせて拡大できないようにする
    SetWindowSizeChangeEnableFlag(FALSE, FALSE);
    SetGraphMode(WIN_WIDTH, WIN_HEIGHT, 32);    //画面サイズの最大サイズ、カラービット数を設定(モニターの解像度に合わせる)
    SetWindowSizeExtendRate(1.0);                //画面サイズを設定(解像度との比率で設定)
    SetBackgroundColor(0x00, 0x00, 0x00);        // 画面の背景色を設定する

    //Dxライブラリの初期化
    if (DxLib_Init() == -1) { return -1; }

    //(ダブルバッファ)描画先グラフィック領域は裏面を指定
    SetDrawScreen(DX_SCREEN_BACK);

    //ゲームループで使う変数の宣言
    char keys[256] = { 0 }; //最新のキーボード情報用
    char oldkeys[256] = { 0 };//1ループ(フレーム)前のキーボード情報

    float vx = 0;
    float vy = 0;
    float len = 0;
    float distance = 0;

    float ax = 150;
    float ay = 150;
    float bx = 180;
    float by = 150;

    float _a = 0;
    float _b = 0;
    float _c = 0;
    float _d = 0;

    float f0 = 0;
    float f1 = 0;

    float t = 0;
    float amx = 0;//移動運動を発生させるベクトルx
    float amy = 0;//移動運動を発生させるベクトルy
    float bmx = 0;//移動運動を発生させるベクトルx
    float bmy = 0;//移動運動を発生させるベクトルy
    float arx = 0;//回転運動を発生させるベクトルx
    float ary = 0;//回転運動を発生させるベクトルy
    float brx = 0;//回転運動を発生させるベクトルx
    float bry = 0;//回転運動を発生させるベクトルy

    float e = 1.0;//反発係数

    float adx = 0;
    float bdx = 0;
    float ady = 0;
    float bdy = 0;

    //円A
    struct circle1 {
        float px;
        float py;
        float dx;
        float dy;
        float r;
        int m;
    };

    struct circle1 A;
    A.px = 150;
    A.py = 150;
    A.dx = 2;
    A.dy = 3;
    A.r = 10;
    A.m = 1;

    struct circle1 B;
    B.px = 180;
    B.py = 150;
    B.dx = 2;
    B.dy = 3;
    B.r = 10;
    B.m = 1;

    // ゲームループ
    while (1)
    {
        //最新のキーボード情報だったものは1フレーム前のキーボード情報として保存

        //最新のキーボード情報を取得
        GetHitKeyStateAll(keys);

        //画面クリア
        ClearDrawScreen();
        //---------  ここからプログラムを記述  ----------//

        //更新処理
        //移動
        if (keys[KEY_INPUT_A])
        {
            A.px--;
        }
        if (keys[KEY_INPUT_D])
        {
            A.px++;
        }
        if (keys[KEY_INPUT_W])
        {
            A.py--;
        }
        if (keys[KEY_INPUT_S])
        {
            A.py++;
        }

        //円Aと円Bのめり込んだ量を調べる
        vx = A.px - B.px;
        vy = A.py - B.py;
        len = sqrt(vx * vx + vy * vy);
        distance = (A.r + B.r) - len;

        //円Aの補正方向を調べる
        if (len > 0)    len = 1 / len;

        vx *= len;
        vy *= len;

        //円Aと円Bを重ならない位置まで補正する
        distance /= 2.0;
        A.px += vx * distance;
        A.py += vy * distance;
        B.px -= vx * distance;
        B.py -= vy * distance;

        //フレーム時間を計算する
        _a = (A.dx * A.dx) - 2 * (A.dx * B.dx) + (B.dx * B.dx) + (A.dy * A.dy) - 2 * (A.dy * B.dy) + (B.dy * B.dy);
        _b = 2 * (A.px * A.dx) - 2 * (A.px * B.dx) - 2 * (A.dx * B.px) + 2 * (B.px * B.dx) + 2 * (A.py * A.dy) - 2 * (A.py * B.dy) - 2 * (A.dy * B.py) + 2 * (B.py * B.dy);
        _c = (A.px * A.px) - 2 * (A.px * B.px) + (B.px * B.px) + (A.py * A.py) - 2 * (A.py * B.py) + (B.py * B.py) - (A.r + B.r) * (A.r + B.r);
        _d = sqrt(_b * _b - 4 * _a * _c);

        if (_d <= 0)
        {
            //当たり無し
        }
        else
        {
            //当たりあり
            _d = sqrt(_d);

            f0 = (-_b - _d) / (2 * _a);        //接触する瞬間
            f1 = (-_b + _d) / (2 * _a);        //離れる瞬間
        }

        //衝突する瞬間の座標を計算する
        ax = A.px + A.dx * f0;
        ay = A.py + A.dy * f0;
        bx = B.px + B.dx * f0;
        by = B.py + B.dy * f0;

        //速度ベクトルを重心方向と垂直な方向に分離する
        vx = (B.px - A.px);
        vy = (B.py - A.py);

        t = -(vx * A.dx + vy * A.dy) / (vx * vx + vy * vy);
        arx = A.dx + vx * t;
        ary = A.dy + vy * t;

        t = -(-vy * A.dx + vx * A.dy) / (vy * vy + vx * vx);
        amx = A.dx - vy * t;
        amy = A.dy - vx * t;

        t = -(vx * B.dx + vy * B.dy) / (vx * vx + vy * vy);
        brx = B.dx + vx * t;
        bry = B.dy + vy * t;

        t = -(-vy * B.dx + vx * B.dy) / (vy * vy + vx * vx);
        bmx = B.dx - vy * t;
        bmy = B.dy - vx * t;

        adx = (A.m * amx + B.m * bmx + bmx * e * B.m - amx * e * B.m) / (A.m + B.m);
        bdx = -e * (bmx - amx) + adx;
        ady = (A.m * amy + B.m * bmy + bmy * e * B.m - amy * e * B.m) / (A.m + B.m);
        bdy = -e * (bmy - amy) + ady;

        A.dx = adx + arx;
        A.dy = ady + ary;
        B.dx = bdx + brx;
        B.dy = bdy + bry;

        A.px += A.dx;
        A.py += A.dy;
        B.px += B.dx;
        B.py += B.dy;

        //描画処理
        DrawFormatString(300, 0, GetColor(0, 255, 255), "len %f", len);
        DrawFormatString(300, 20, GetColor(0, 255, 255), "px %f", A.px);
        DrawFormatString(300, 40, GetColor(0, 255, 255), "px %f", B.px);
        DrawFormatString(300, 60, GetColor(0, 255, 255), "py %f", A.py);
        DrawFormatString(300, 80, GetColor(0, 255, 255), "py %f", B.py);
        DrawCircle(A.px, A.py, A.r, GetColor(255, 255, 255), TRUE);
        DrawCircle(B.px, B.py, B.r, GetColor(0, 255, 255), TRUE);

        //---------  ここまでにプログラムを記述  ---------//
        ScreenFlip();//(ダブルバッファ)裏面
        // 20ミリ秒待機(疑似60FPS)
        WaitTimer(20);
        // Windows システムからくる情報を処理する
        if (ProcessMessage() == -1)
        {
            break;
        }
        // ESCキーが押されたらループから抜ける
        if (CheckHitKey(KEY_INPUT_ESCAPE) == 1)
        {
            break;
        }
    }
    //Dxライブラリ終了処理
    DxLib_End();

    return 0;
}
A.px += A.dx;
A.py += A.dy;
B.px += B.dx;
B.py += B.dy;
こちらの4行をコメントアウトしてみました。
すると、実行開始時から円Bに円Aがくっついている状態で座標を直接動かしても離れませんでした。
VisualStudio2019を使用しています。

“C言語何でも質問掲示板” へ戻る