Box2D基本2_Jointを使ってRagDollを作る

#include "DrawerRagdoll.h"
#include "DrawerBox.h"
#include "DrawerCircle.h"

DrawerRagdoll *DrawerRagdoll::create(const b2WorldId &worldId,
                                     float x, float y) {
    // New
    auto obj = new DrawerRagdoll(worldId, x, y);
    if (obj && obj->init()) return obj;
    DX_SAFE_DELETE(obj);
    return nullptr;
}

DrawerRagdoll::DrawerRagdoll(const b2WorldId &worldId,
                             float x, float y) :
        DrawerBase(worldId, x, y) {
    //LOGD("Main", "DrawerRagdoll()");
}

DrawerRagdoll::~DrawerRagdoll() {
    //LOGD("Main", "~DrawerRagdoll()");
    // Destroy
    for (auto joint: joints) b2DestroyJoint(joint, true);
    DX_SAFE_DELETE_VECTOR(bodies);
}

bool DrawerRagdoll::init() {

    const int cX = pos.x;
    const int cY = pos.y;
    const int gSize = UtilDebug::getInstance()->getGridSize();

    // Head
    const auto head = DrawerCircle::create(
            worldId, cX, cY, gSize, true);
    bodies.push_back(head);

    // Body
    const auto body = DrawerBox::create(
            worldId, cX, cY + gSize * 3,
            gSize * 2, gSize * 4, 0, true);
    bodies.push_back(body);

    // Head - Body
    auto jdHB = b2DefaultRevoluteJointDef();
    jdHB.base.bodyIdA = head->getBodyId();
    jdHB.base.bodyIdB = body->getBodyId();
    jdHB.base.localFrameA.p = {0.0f, -head->getRadiusM()};
    jdHB.base.localFrameB.p = {0.0f, body->getHeightM() / 2};
    jdHB.base.collideConnected = false;
    jdHB.enableLimit = true;
    jdHB.lowerAngle = DEG_TO_RAD * -30;
    jdHB.upperAngle = DEG_TO_RAD * 30;
    joints.push_back(b2CreateRevoluteJoint(worldId, &jdHB));

    // Left armA
    const auto leftArmA = DrawerBox::create(
            worldId, cX - gSize * 2, cY + gSize * 2,
            gSize * 2, gSize * 1, 0, true);
    bodies.push_back(leftArmA);

    // Body - Left armA
    auto jdBody2LAA = b2DefaultRevoluteJointDef();
    jdBody2LAA.base.bodyIdA = body->getBodyId();
    jdBody2LAA.base.bodyIdB = leftArmA->getBodyId();
    jdBody2LAA.base.localFrameA.p = {-body->getWidthM() / 2, body->getHeightM() / 4};
    jdBody2LAA.base.localFrameB.p = {leftArmA->getWidthM() / 2, 0.0f};
    jdBody2LAA.base.collideConnected = false;
    jdBody2LAA.enableLimit = true;
    jdBody2LAA.lowerAngle = DEG_TO_RAD * -30;
    jdBody2LAA.upperAngle = DEG_TO_RAD * 30;
    joints.push_back(b2CreateRevoluteJoint(worldId, &jdBody2LAA));

    // Left armB
    const auto leftArmB = DrawerBox::create(
            worldId, cX - gSize * 4, cY + gSize * 2,
            gSize * 2, gSize * 1, 0, true);
    bodies.push_back(leftArmB);

    // Left armA - Left armB
    auto jdLAA2LAB = b2DefaultRevoluteJointDef();
    jdLAA2LAB.base.bodyIdA = leftArmA->getBodyId();
    jdLAA2LAB.base.bodyIdB = leftArmB->getBodyId();
    jdLAA2LAB.base.localFrameA.p = {-leftArmA->getWidthM() / 2, 0.0f};
    jdLAA2LAB.base.localFrameB.p = {leftArmB->getWidthM() / 2, 0.0f};
    jdLAA2LAB.base.collideConnected = false;
    jdLAA2LAB.enableLimit = true;
    jdLAA2LAB.lowerAngle = DEG_TO_RAD * -30;
    jdLAA2LAB.upperAngle = DEG_TO_RAD * 30;
    joints.push_back(b2CreateRevoluteJoint(worldId, &jdLAA2LAB));

    // Right armA
    const auto rightArmA = DrawerBox::create(
            worldId, cX + gSize * 2, cY + gSize * 2,
            gSize * 2, gSize * 1, 0, true);
    bodies.push_back(rightArmA);

    // Body - Right armA
    auto jdBody2RAA = b2DefaultRevoluteJointDef();
    jdBody2RAA.base.bodyIdA = body->getBodyId();
    jdBody2RAA.base.bodyIdB = rightArmA->getBodyId();
    jdBody2RAA.base.localFrameA.p = {body->getWidthM() / 2, body->getHeightM() / 4};
    jdBody2RAA.base.localFrameB.p = {-rightArmA->getWidthM() / 2, 0.0f};
    jdBody2RAA.base.collideConnected = false;
    jdBody2RAA.enableLimit = true;
    jdBody2RAA.lowerAngle = DEG_TO_RAD * -30;
    jdBody2RAA.upperAngle = DEG_TO_RAD * 30;
    joints.push_back(b2CreateRevoluteJoint(worldId, &jdBody2RAA));

    // Right armB
    const auto rightArmB = DrawerBox::create(
            worldId, cX + gSize * 4, cY + gSize * 2,
            gSize * 2, gSize * 1, 0, true);
    bodies.push_back(rightArmB);

    // Right armA - Right armB
    auto jdLAA2RAB = b2DefaultRevoluteJointDef();
    jdLAA2RAB.base.bodyIdA = rightArmA->getBodyId();
    jdLAA2RAB.base.bodyIdB = rightArmB->getBodyId();
    jdLAA2RAB.base.localFrameA.p = {rightArmA->getWidthM() / 2, 0.0f};
    jdLAA2RAB.base.localFrameB.p = {-rightArmB->getWidthM() / 2, 0.0f};
    jdLAA2RAB.base.collideConnected = false;
    jdLAA2RAB.enableLimit = true;
    jdLAA2RAB.lowerAngle = DEG_TO_RAD * -30;
    jdLAA2RAB.upperAngle = DEG_TO_RAD * 30;
    joints.push_back(b2CreateRevoluteJoint(worldId, &jdLAA2RAB));

    // Left legA
    const auto leftLegA = DrawerBox::create(
            worldId, cX - gSize * 1, cY + gSize * 6,
            gSize * 1, gSize * 2, 0, true);
    bodies.push_back(leftLegA);

    // Body - Left legA
    auto jdBody2LLA = b2DefaultRevoluteJointDef();
    jdBody2LLA.base.bodyIdA = body->getBodyId();
    jdBody2LLA.base.bodyIdB = leftLegA->getBodyId();
    jdBody2LLA.base.localFrameA.p = {-body->getWidthM() / 2, -body->getHeightM() / 2};
    jdBody2LLA.base.localFrameB.p = {0.0f, leftLegA->getHeightM() / 2};
    jdBody2LLA.base.collideConnected = false;
    jdBody2LLA.enableLimit = true;
    jdBody2LLA.lowerAngle = DEG_TO_RAD * -30;
    jdBody2LLA.upperAngle = DEG_TO_RAD * 30;
    joints.push_back(b2CreateRevoluteJoint(worldId, &jdBody2LLA));

    const auto leftLegB = DrawerBox::create(
            worldId, cX - gSize * 1, cY + gSize * 8,
            gSize * 1, gSize * 2, 0, true);
    bodies.push_back(leftLegB);

    // Left legA - Left LegB
    auto jdBody2LLB = b2DefaultRevoluteJointDef();
    jdBody2LLB.base.bodyIdA = leftLegA->getBodyId();
    jdBody2LLB.base.bodyIdB = leftLegB->getBodyId();
    jdBody2LLB.base.localFrameA.p = {0.0f, -leftLegA->getHeightM() / 2};
    jdBody2LLB.base.localFrameB.p = {0.0f, leftLegB->getHeightM() / 2};
    jdBody2LLB.base.collideConnected = false;
    jdBody2LLB.enableLimit = true;
    jdBody2LLB.lowerAngle = DEG_TO_RAD * -30;
    jdBody2LLB.upperAngle = DEG_TO_RAD * 30;
    joints.push_back(b2CreateRevoluteJoint(worldId, &jdBody2LLB));

    // Right legA
    const auto rightLegA = DrawerBox::create(
            worldId, cX + gSize * 1, cY + gSize * 6,
            gSize * 1, gSize * 2, 0, true);
    bodies.push_back(rightLegA);

    // Body - Right legA
    auto jdBody2RLA = b2DefaultRevoluteJointDef();
    jdBody2RLA.base.bodyIdA = body->getBodyId();
    jdBody2RLA.base.bodyIdB = rightLegA->getBodyId();
    jdBody2RLA.base.localFrameA.p = {body->getWidthM() / 2, -body->getHeightM() / 2};
    jdBody2RLA.base.localFrameB.p = {0.0f, rightLegA->getHeightM() / 2};
    jdBody2RLA.base.collideConnected = false;
    jdBody2RLA.enableLimit = true;
    jdBody2RLA.lowerAngle = DEG_TO_RAD * -30;
    jdBody2RLA.upperAngle = DEG_TO_RAD * 30;
    joints.push_back(b2CreateRevoluteJoint(worldId, &jdBody2RLA));

    const auto rightLegB = DrawerBox::create(
            worldId, cX + gSize * 1, cY + gSize * 8,
            gSize * 1, gSize * 2, 0, true);
    bodies.push_back(rightLegB);

    // Right legA - Right LegB
    auto jdBody2RLB = b2DefaultRevoluteJointDef();
    jdBody2RLB.base.bodyIdA = rightLegA->getBodyId();
    jdBody2RLB.base.bodyIdB = rightLegB->getBodyId();
    jdBody2RLB.base.localFrameA.p = {0.0f, -rightLegA->getHeightM() / 2};
    jdBody2RLB.base.localFrameB.p = {0.0f, rightLegB->getHeightM() / 2};
    jdBody2RLB.base.collideConnected = false;
    jdBody2RLB.enableLimit = true;
    jdBody2RLB.lowerAngle = DEG_TO_RAD * -30;
    jdBody2RLB.upperAngle = DEG_TO_RAD * 30;
    joints.push_back(b2CreateRevoluteJoint(worldId, &jdBody2RLB));

    return true;
}

void DrawerRagdoll::update(const float delay) {

    // Bodies
    for (const auto body: bodies) body->update(delay);

    // Joints
    for (const auto joint: joints) {
        b2BodyId bodyA = b2Joint_GetBodyA(joint);
        b2BodyId bodyB = b2Joint_GetBodyB(joint);
        auto localA = b2Joint_GetLocalFrameA(joint).p;
        auto localB = b2Joint_GetLocalFrameB(joint).p;
        auto pA = b2Body_GetWorldPoint(bodyA, localA);
        auto pB = b2Body_GetWorldPoint(bodyB, localB);
        const int fX = this->m2p(pA.x);
        const int fY = this->m2p(pA.y);
        const int tX = this->m2p(pB.x);
        const int tY = this->m2p(pB.y);
        UtilCamera::getInstance()->drawLine(fX, -fY, tX, -tY,
                                            color, true);
    }
}