Box2D基本3_Motorを使って自動車を作る

#include "DrawerCar.h"
#include "DrawerBox.h"
#include "DrawerCircle.h"

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

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

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

bool DrawerCar::init() {

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

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

    // Left wheel
    const auto wheelL = DrawerCircle::create(
            worldId, cX - gSize * 2, cY, gSize, true);
    bodies.push_back(wheelL);

    // Right wheel
    const auto wheelR = DrawerCircle::create(
            worldId, cX + gSize * 2, cY, gSize, true);
    bodies.push_back(wheelR);

    // Body - Left wheel
    auto jdBody2WL = b2DefaultRevoluteJointDef();
    jdBody2WL.base.bodyIdA = body->getBodyId();
    jdBody2WL.base.bodyIdB = wheelL->getBodyId();
    jdBody2WL.base.localFrameA.p = {-body->getWidthM() / 2, -body->getHeightM() / 2};
    jdBody2WL.base.localFrameB.p = {0.0f, 0.0f};
    jdBody2WL.base.collideConnected = false;
    jdBody2WL.enableMotor = true;
    jdBody2WL.maxMotorTorque = 100.0f;
    jdBody2WL.motorSpeed = 0.0f;
    jointL = b2CreateRevoluteJoint(worldId, &jdBody2WL);
    joints.push_back(jointL);

    // Body - Right wheel
    auto jdBody2WR = b2DefaultRevoluteJointDef();
    jdBody2WR.base.bodyIdA = body->getBodyId();
    jdBody2WR.base.bodyIdB = wheelR->getBodyId();
    jdBody2WR.base.localFrameA.p = {body->getWidthM() / 2, -body->getHeightM() / 2};
    jdBody2WR.base.localFrameB.p = {0.0f, 0.0f};
    jdBody2WR.base.collideConnected = false;
    jdBody2WR.enableMotor = true;
    jdBody2WR.maxMotorTorque = 100.0f;
    jdBody2WR.motorSpeed = 0.0f;
    jointR = b2CreateRevoluteJoint(worldId, &jdBody2WR);
    joints.push_back(jointR);

    return true;
}

void DrawerCar::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);
    }
}

void DrawerCar::setMotorSpeed(float spd) {
    LOGD("Main", "setMotorSpeed: %f", spd);

    b2BodyId wheelL = b2Joint_GetBodyB(jointL);
    b2Body_SetAwake(wheelL, true);
    b2RevoluteJoint_SetMotorSpeed(jointL, spd);

    b2BodyId wheelR = b2Joint_GetBodyB(jointR);
    b2Body_SetAwake(wheelR, true);
    b2RevoluteJoint_SetMotorSpeed(jointR, spd);
}