src/PhysicalObject.cpp

Go to the documentation of this file.

#ifndef DOLOG
//#define DOLOG
#undef  DOLOG
#endif


#include <fstream>
#include <stdexcept>

#include "PhysicalObject.h"


PhysicalObject::PhysicalObject(bool enablePhysics)
    : mTransform(),
        mInverseTransform(),
        mPhysicsOn(enablePhysics),
        mMass(INFINITY),
        mPosition(),
        mScaling(1.0f),
        mRotation(),
        mVelocity(),
        mAngularVelocity(),
        mTime(0.0f)
{
}


PhysicalObject::~PhysicalObject()
{
}


void PhysicalObject::updateProperties(float t)
{/*{{{*/
    LLOG(mTime);
//    dump();    // FIXME DEBUG
    assert(t >= -0.0f && t >= mTime - EPSILON);
    
    float dt = t - mTime;   // delta time
    Vec3f dv(0, 0, 0);      // delta velocity
    Vec3f dw(0, 0, 0);      // delta angular velocity
    LLOG("Updating properties to time : " << t << "  dt = " << dt);
    if (dt < EPSILON)
    {
        LLOG("no need for update");
        return;
    }
//    dump();    // FIXME DEBUG

    // determine if the ball is sliding or rolling
    const Vec3f vw = mAngularVelocity.cross(WORLD_UP * -BALL_RADIUS);
    const Vec3f v_perimeter = vw + mVelocity;
    bool adjustAngularVelocity = false;
    LLOG("v_p   = " << v_perimeter << "  |v_p|   = " << v_perimeter.length());
    LLOG("w x r = " << vw << "  |w x r| = " << vw.length());
    if (mVelocity.length() < PHYS_EPS)
    {
        if (mVelocity.length() > EPSILON)
        {
            LLOG("\n\n\n");
//            dump();    // FIXME DEBUG
            LLOG("\nSTOPPING\n\n\n");
        }
        // ball should stop
        mAngularVelocity    = Vec3f(0, 0, 0);
        mVelocity           = Vec3f(0, 0, 0);
        mTime               = t;

        // update transormation matrices
        resetTransform();
        scale(mScaling, mScaling, mScaling);
        rotate(mRotation.length(), mRotation);
        translate(mPosition);
        return;
    }
    else if (v_perimeter.length() < PHYS_EPS)
    {
        LLOG("ROLLS");
        // ball rolls
        if (mVelocity.length() > EPSILON)
            dv = mVelocity.normal() * -MUE_ROLLING * GRAVITY * dt;
        adjustAngularVelocity = true;
    }
    else
    {
        LLOG("SLIDES");
        // ball slides
        Vec3f f_friction(0, 0, 0);
        if (v_perimeter.length() > EPSILON)
        {
            // delta linear speed
//            LOG("old dv = " << v_perimeter.normal() * -MUE_SLIDING * GRAVITY * dt);
//            dv          = v_perimeter.normal() * -MUE_SLIDING * GRAVITY * dt;
            // friction force
            f_friction  = v_perimeter.normal() * -MUE_SLIDING * GRAVITY * mMass;
        }
        LLOG("F  = " << f_friction << "  |F|  = " << f_friction.length());
        // delta angular speed
        dw = (WORLD_UP * -BALL_RADIUS).cross(f_friction) * 2.5f * dt / (mMass * BALL_RADIUS * BALL_RADIUS);
        if ((WORLD_UP * -BALL_RADIUS).cross(f_friction).length() < PHYS_EPS)
        {
            LLOG("ROTATES");
            // ball rotates along vertical axis
            f_friction = mAngularVelocity.cross(WORLD_UP_MASK.normal()) * -MUE_SLIDING * GRAVITY * mMass;
            dw = (WORLD_UP_MASK.normal() * CONTACT_RADIUS).cross(f_friction) * 2.5f * dt / (mMass * BALL_RADIUS * BALL_RADIUS);
        }
        // energy * 2 / m
        float Ekin      = mVelocity.dot(mVelocity) + 0.4f * BALL_RADIUS * BALL_RADIUS * mAngularVelocity.dot(mAngularVelocity);
        float dErot     = 0.4f * BALL_RADIUS * BALL_RADIUS * (mAngularVelocity + dw).dot(mAngularVelocity + dw);
        float dEfrict   = MUE_SLIDING * GRAVITY * v_perimeter.length() * dt;
        LLOG("Ekin       = " << Ekin);
        LLOG("dErot      = " << dErot);
        LLOG("dEfrict    = " << dEfrict);
        if (v_perimeter.length() > EPSILON)
        {
            float dE = Ekin - dErot - dEfrict;
            if (dE < 0.0f)
                dv = - v_perimeter.normal() * (mVelocity.length() + sqrt(- dE));
            else
                dv = - v_perimeter.normal() * (mVelocity.length() - sqrt(dE));
        }
    }
    LLOG("dv = " << dv << "  |dv| = " << dv.length());
    LLOG("dw = " << dw << "  |dw| = " << dw.length());

    // delta rotation
    Vec3f dr;
    // delta position
    Vec3f ds = dv * 0.5f * dt + mVelocity * dt;
    LLOG("ds = " << ds << "  |ds| = " << ds.length());

    // update transformations now if sliding
    if (!adjustAngularVelocity)
    {
        mRotation += dw * 0.5f * dt + mAngularVelocity * dt;
        LLOG("dr = " << (dw * 0.5f * dt + mAngularVelocity * dt)
                << "  |dr| = " << (dw * 0.5f * dt + mAngularVelocity * dt).length());

        // update transormation matrices
        resetTransform();
        scale(mScaling, mScaling, mScaling);
        rotate(mRotation.length(), mRotation);
        translate(mPosition + WORLD_UP_MASK.scaled(ds));
    }

    // update properties
    mVelocity           = WORLD_UP_MASK.scaled(mVelocity + dv);
    Vec3f oldAV         = mAngularVelocity;
    mAngularVelocity    += dw;
    mTime               = t;

    // test if we should ensure correct rolling
    const Vec3f v_perimeter2 = mAngularVelocity.cross(WORLD_UP * -BALL_RADIUS) + mVelocity;
    if (adjustAngularVelocity || v_perimeter.dot(v_perimeter - v_perimeter2) * v_perimeter2.dot(v_perimeter - v_perimeter2) < 0.0f)
    {
        LLOG("adjusting w : " << mAngularVelocity << "(" << mAngularVelocity.length() << ") -> "
                << (WORLD_UP * BALL_RADIUS).cross(mVelocity) / (BALL_RADIUS * BALL_RADIUS)
                << "(" << (WORLD_UP * BALL_RADIUS).cross(mVelocity).length() / (BALL_RADIUS * BALL_RADIUS) << ")");

        // recalculate angular velocity aligned to velocity
        mAngularVelocity = (WORLD_UP * BALL_RADIUS).cross(mVelocity) / (BALL_RADIUS * BALL_RADIUS);
        dw = mAngularVelocity - oldAV;
    }

    // update transformations again if rolling
    if (adjustAngularVelocity)
    {
        mRotation += dw * 0.5f * dt + oldAV * dt;
        // update transormation matrices
        resetTransform();
        scale(mScaling, mScaling, mScaling);
        rotate(mRotation.length(), mRotation);
        translate(mPosition + WORLD_UP_MASK.scaled(ds));
    }

    LLOG("result:");
//#ifdef DOLOG
//    dump();    // FIXME DEBUG
//#endif
    LLOG("\n");
}/*}}}*/


void PhysicalObject::writeDump(std::ostream & outs)
{
    if (!mPhysicsOn)
    {
        throw std::logic_error("Physics must be on to write to file");
    }

    int magic = 0x316a626f;
    outs.write(reinterpret_cast<const char *>(&magic), sizeof(int));

    outs.write(reinterpret_cast<const char *>(&mTransform), sizeof(Matrix));
    outs.write(reinterpret_cast<const char *>(&mInverseTransform), sizeof(Matrix));
    outs.write(reinterpret_cast<const char *>(&mMass), sizeof(float));
    outs.write(reinterpret_cast<const char *>(&mPosition), sizeof(Vec3f));
    outs.write(reinterpret_cast<const char *>(&mScaling), sizeof(float));
    outs.write(reinterpret_cast<const char *>(&mRotation), sizeof(Vec3f));
    outs.write(reinterpret_cast<const char *>(&mVelocity), sizeof(Vec3f));
    outs.write(reinterpret_cast<const char *>(&mAngularVelocity), sizeof(Vec3f));
    outs.write(reinterpret_cast<const char *>(&mTime), sizeof(float));
}


void PhysicalObject::readDump(std::istream & ins)
{
    if (!mPhysicsOn)
    {
        throw std::logic_error("Physics must be on to read from file");
    }

    int magic;
    ins.read(reinterpret_cast<char *>(&magic), sizeof(int));
    if (magic != 0x316a626f)
    {
        throw std::logic_error("Could not find object data");
    }
    ins.clear();

    ins.read(reinterpret_cast<char *>(&mTransform), sizeof(Matrix));
    ins.clear();
    ins.read(reinterpret_cast<char *>(&mInverseTransform), sizeof(Matrix));
    ins.clear();
    ins.read(reinterpret_cast<char *>(&mMass), sizeof(float));
    ins.clear();
    ins.read(reinterpret_cast<char *>(&mPosition), sizeof(Vec3f));
    ins.clear();
    ins.read(reinterpret_cast<char *>(&mScaling), sizeof(float));
    ins.clear();
    ins.read(reinterpret_cast<char *>(&mRotation), sizeof(Vec3f));
    ins.clear();
    ins.read(reinterpret_cast<char *>(&mVelocity), sizeof(Vec3f));
    ins.clear();
    ins.read(reinterpret_cast<char *>(&mAngularVelocity), sizeof(Vec3f));
    ins.clear();
    ins.read(reinterpret_cast<char *>(&mTime), sizeof(float));
    ins.clear();
}

---