PhysicsSystem.cpp

/***************************************************************************
 *   This file is part of OpenCAL: Open Computer Animation Library         *
 *   I created OpenCAL as my master's thesis Computer Science (multimedia) *
 *   at the tUL university in Diepenbeek, Belgium                          *
 *                                                                         *
 *   Copyright (C) 2003-2004 by Jeroen Dierckx                             *
 *   jeroen.dierckx@student.luc.ac.be                                      *
 *                                                                         *
 ***************************************************************************/

// Includes
#include "PhysicsSystem.h"
#include <OpenCAL/PhysicalObject.h>
#include <OpenCAL/Force.h>
#include <OpenCAL/Constraint.h>
#include <OpenCAL/EulerSolver.h>
#include <OpenCAL/MidpointSolver.h>
#include <OpenCAL/RungeKutta4Solver.h>
using namespace OpenCAL;

using namespace std;


// Macros
/*
#define forAllObjects(it)\
ObjectList::iterator it;\
for(it = m_objects.begin(); it != m_objects.end(); ++it)

#define forAllForces(it)\
ForceList::iterator it;\
for(it = m_forces.begin(); it != m_forces.end(); ++it)

#define forAllConstraints(it)\
ConstraintList::iterator it;\
for(it = m_constraints.begin(); it != m_constraints.end(); ++it)
*/


/******************************
* Constructors and destructor *
******************************/

PhysicsSystem::PhysicsSystem(System *parent)
: System(parent), m_solver(0), m_maxStepSize(0.3f)
{
}

PhysicsSystem::~PhysicsSystem()
{
    if(m_solver)
        delete m_solver;
}


/************************
* Get and set functions *
************************/


/******************
* Force functions *
******************/

void PhysicsSystem::addForce(const Vector3 &force)
{
    unsigned int i;

    for(i = 0; i < m_systems.size(); ++i)
        ((PhysicsSystem *) m_systems[i])->addForce(force);

    PhysicalObject *object;
    for(i = 0; i < m_objects.size(); ++i)
    {
        object = (PhysicalObject *) m_objects[i];
        if(!object->isFixed())
            object->addForce(force);
    }
}

void PhysicsSystem::resetForces()
{
    unsigned int i;

    for(i = 0; i < m_systems.size(); ++i)
        ((PhysicsSystem *) m_systems[i])->resetForces();

    PhysicalObject *object;
    for(i = 0; i < m_objects.size(); ++i)
    {
        object = (PhysicalObject *) m_objects[i];
        if(!object->isFixed())
            object->resetForce();
    }
}


/***********************
* ODE Source functions *
***********************/

void PhysicsSystem::fillState()
{
/*
    unsigned int size = m_objects.size() * 6;
    m_state.clear(size);
*/

    m_state.clear();

    //cout << "Filling from " << m_objects.size() << " objects" << endl;
    for(unsigned int i = 0; i < m_objects.size(); ++i)
    {
        PhysicalObject *object = (PhysicalObject *) m_objects[i];
        object->addToState(&m_state);
        //m_state.concat(((PhysicalObject *) m_objects[i])->getPosition());
        //m_state.concat(((PhysicalObject *) m_objects[i])->getVelocity());
    }
}

void PhysicsSystem::fillDerivative()
{
    //unsigned int size = m_objects.size() * 6;
    //m_derivative.clear(size);

    m_derivative.clear();


    for(unsigned int i = 0; i < m_objects.size(); ++i)
    {
        PhysicalObject *object = (PhysicalObject *) m_objects[i];
        object->addToDerivative(&m_derivative);

        //bool fixed = object->isFixed();
        //m_derivative.concat(fixed ? Vector3::zero : object->getVelocity());
        //m_derivative.concat(fixed ? Vector3::zero : object->getAccelleration());
    }
}

void PhysicsSystem::fromState()
{
    // Go to the beginning of the state vector
    m_state.reset();

    for(unsigned int i = 0; i < m_objects.size(); ++i)
    {
        PhysicalObject *object = (PhysicalObject *) m_objects[i];
        object->fromState(&m_state);
/*
        if(!object->isFixed())
        {
            Vector3 *pos = object->getPositionP();
            Vector3 *vel = object->getVelocityP();

            pos->setX(m_state[i * 6]);
            pos->setY(m_state[i * 6 + 1]);
            pos->setZ(m_state[i * 6 + 2]);

            vel->setX(m_state[i * 6 + 3]);
            vel->setY(m_state[i * 6 + 4]);
            vel->setZ(m_state[i * 6 + 5]);
        }
        // @warning Should this be done here?
        object->setForce(Vector3::zero);
*/
    }
}


/******************
* Other functions *
******************/

void PhysicsSystem::step(float deltaSeconds)
{
    //if(!m_enabled) return;

    /*
        1. Reset forces      \
        2. Apply forces       - Done by the System
        3. Apply constraints /
        4. Integrate system
    */

    unsigned int steps = (unsigned int) (deltaSeconds / m_maxStepSize) + 1;
    deltaSeconds /= steps;

    for(unsigned int i = 0; i < steps; ++i)
    {
        System::step(deltaSeconds);
        if(m_objects.size() > 0)
            m_solver->solve(deltaSeconds, this);
    }
}


/************************
* Phase space functions *
************************/

/*
PhaseSpace PhysicsSystem::getState() const
{
    unsigned int size = m_objects.size() * 6;

    // Reserve the space in the phase space (faster)
    PhaseSpace result(size);

    for(unsigned int i = 0; i < m_objects.size(); ++i)
    {
        result += ((PhysicalObject *) m_objects[i])->getPosition();
        result += ((PhysicalObject *) m_objects[i])->getVelocity();
    }

    return result;
}

PhaseSpace PhysicsSystem::getDerivative() const
{
    unsigned int size = m_objects.size() * 6;

    // Reserve the space in the phase space (faster)
    PhaseSpace result(size);

    for(unsigned int i = 0; i < m_objects.size(); ++i)
    {
        result += ((PhysicalObject *) m_objects[i])->getVelocity();
        result += ((PhysicalObject *) m_objects[i])->getAccelleration();
    }

    return result;
}

void PhysicsSystem::setState(const PhaseSpace &state)
{
    for(unsigned int i = 0; i < m_objects.size(); ++i)
    {
        PhysicalObject *object = (PhysicalObject *) m_objects[i];
        Vector3 *pos = object->getPositionP();
        Vector3 *vel = object->getVelocityP();

        pos->setX(state[i * 6]);
        pos->setY(state[i * 6 + 1]);
        pos->setZ(state[i * 6 + 2]);

        vel->setX(state[i * 6 + 3]);
        vel->setY(state[i * 6 + 4]);
        vel->setZ(state[i * 6 + 5]);

        // @warning Should this be done here?
        object->setForce(Vector3::zero);
    }
}
*/


void PhysicsSystem::initialize()
{
    System::initialize();

    m_solver = new EulerSolver;
    //m_solver = new MidpointSolver;
    //m_solver = new RungeKutta4Solver;

/*
    forAllObjects(object)
        (*object)->initialize();

    forAllForces(force)
        (*force)->initialize();

    forAllConstraints(constraint)
        (*constraint)->initialize();
*/
}

/**********************
* Protected functions *
**********************/

/*
void PhysicsSystem::addObject(Object *object)
{
    if(!dynamic_cast<PhysicalObject *>(object))
    {
        cerr << "Only PhysicalObject objects are allowed in this system. ";
        cerr << "Please delete this object manually, ";
        cerr << "because it will not be managed by the system." << endl;
        return;
    }

    System::addObject(object);
}

void PhysicsSystem::addActor(Actor *actor)
{
    if(!dynamic_cast<Force *>(actor))
    {
        cerr << "Only Force actors are allowed in this system. ";
        cerr << "Please delete this actor manually, ";
        cerr << "because it will not be managed by the system." << endl;
        return;
    }

    System::addActor(actor);
}

void PhysicsSystem::addAgent(Agent *agent)
{
    if(!dynamic_cast<Constraint *>(agent))
    {
        cerr << "Only Constraint agents are allowed in this system. ";
        cerr << "Please delete this agent manually, ";
        cerr << "because it will not be managed by the system." << endl;
        return;
    }

    System::addAgent(agent);
}
*/


#ifdef BLABLA
void PhysicsSystem::deleteObject(PhysicalObject *object)
{
    if(isDestructing()) return;

    forAllObjects(it)
        if(*it == object)
        {
            m_objects.erase(it);
            return;
        }

#ifdef VERBOSE
    Debug::print("Trying to delete an object that isn't in the list!");
#endif // VERBOSE

}

void PhysicsSystem::deleteForce(Force *force)
{
    if(isDestructing()) return;

    forAllForces(it)
        if(*it == force)
        {
            m_forces.erase(it);
            return;
        }

#ifdef VERBOSE
    Debug::print("Trying to delete a force that isn't in the list!");
#endif // VERBOSE

}

void PhysicsSystem::deleteConstraint(Constraint *constraint)
{
    if(isDestructing()) return;

    forAllConstraints(it)
        if(*it == constraint)
        {
            m_constraints.erase(it);
            return;
        }

#ifdef VERBOSE
    Debug::print("Trying to delete a constraint that isn't in the list!");
#endif // VERBOSE

}
#endif // BLABLA

---