//---------------------------------------------------------------------------


#pragma hdrstop

#include "objetoDinamico.h"

//---------------------------------------------------------------------------

#pragma package(smart_init)


objetoDinamico::objetoDinamico(cWorld *world, double imassa, bool igrav):
              cMesh(world) {

        massa = imassa;
        matrizInercia.set(1,0,0,
                          0,1,0,
                          0,0,1);

        createAABBCollisionDetector(true,true);                  

}

void objetoDinamico::inicializa() {

        cMatrix3d ini;
        ini.identity();
        forca = torque = aceleracao = aceleAngular = veloAngular =
        giro = velocidade = translacao = cVector3d(0,0,0);
        setPos(translacao);
        setRot(ini);
        computeBoundaryBox(true);
        centroMassa0 = centroMassa = cAdd(getBoundaryMin(),cMul(0.5,cSub(getBoundaryMax(),getBoundaryMin())));
        
}

void objetoDinamico::integraVelocidade(double dt) {

        velocidade = cAdd(velocidade,cMul(dt,aceleracao));
        // amortecimento da velocidade para evitar ganho de energia
        velocidade.sub(cMul(DISSIPATION,velocidade));

}

void objetoDinamico::reposiciona(cVector3d novaposi) {

        centroMassa0=novaposi;
        translacao = cVector3d(0,0,0);

}

void objetoDinamico::integraPosicao(double dt) {

        translacao = cAdd(translacao,cMul(dt,velocidade));
        centroMassa = cAdd(centroMassa0,translacao);

        // atualiza a matriz de transformacao
        setPos(centroMassa);

}

void objetoDinamico::calcAceleracao(double dt, cVector3d forca) {

        aceleracao = cMul(1/massa,forca);
        if(gravidade)
                aceleracao = cAdd(aceleracao,cVector3d(0,-9.8,0));

}

void objetoDinamico::aplicaForca(cVector3d iforca, cVector3d posiforca) {

        // desmembra forca e torque
        cVector3d r;

        // assume que posiforca e' uma posicao valida (no volume do objeto)
        r = cSub(posiforca,centroMassa);


        if((r.length()>0)&&(iforca.length()>0)) {
                // F* = -||r||.(F • ||r||)
                forca = cAdd(forca,cMul(-1*cDot(iforca,cNormalize(r)),cNormalize(r)));

                // T = F × r
                torque = cAdd(torque,cCross(iforca,r));
        }

}

void objetoDinamico::integra(double dt) {

        // integra posicao e velocidade (linear e angular)
        integraPosicao(dt);
        integraVelocidade(dt);
        integraOrientacao(dt);
        integraVeloAngular(dt);

        // recalcula aceleracoes
        calcAceleracao(dt,forca);
        calcAceleAngular(dt,torque);

        // zera as forcas
        torque = forca = cVector3d(0,0,0);

}

void objetoDinamico::integraOrientacao(double dt) {

        cVector3d igiro = cMul(dt,veloAngular);
        giro = cAdd(giro,igiro);

        // atualiza a matriz de transformacao
        if(igiro.length()>0)   {
                //rotate(cNormalize(igiro),igiro.length());
                rotate(cVector3d(1,0,0),igiro.x);
                rotate(cVector3d(0,1,0),igiro.y);
                rotate(cVector3d(0,0,1),igiro.z);
        }

}

void objetoDinamico::integraVeloAngular(double dt) {

        veloAngular = cAdd(veloAngular,cMul(dt,aceleAngular));
        // amortecimento da velocidade angular para evitar ganho de energia
        veloAngular.sub(cMul(DISSIPATION,veloAngular));


}

void objetoDinamico::calcAceleAngular(double dt, cVector3d torque) {

       aceleAngular = cMul(matrizInercia,torque);
}

