---

dynamics.cc

Go to the documentation of this file.

#include "dynamics.hh"
#include "register.hh"
#include "element.hh"
#include <iostream>
#include "abstractworld.hh"
#include "collisionmessage.hh"

namespace anoid {
    namespace plugin {

        using namespace simple;
        using namespace base;
        using namespace config;

        void Dynamics::update() {
            Vector Ft(0.0,0.0,0.0), Tt(0.0,0.0,0.0);
            for (vector<Force *>::iterator i = forces.begin(); i != forces.end(); ++i) {
                Ft += (*i)->getForce();
                Tt += ((*i)->getApplicationPoint() % (*i)->getForce());
            }

            if (mass != 0.0)
                acceleration = (1.0 / mass) * Ft;
            else
                acceleration = O;

            spinvelocity += getWorld()->getTime() * Tt;

            Acceleration::update();
            SpinVelocity::update();
            Ability::update();
        }

        void Dynamics::redraw() {
            Acceleration::redraw();
            SpinVelocity::redraw();
            Ability::update();
        }

        void Dynamics::init(Configuration &c) {
            if (c.hasElement("restitution"))
                constantOfRestitution = c.getDouble("restitution");
            else
                constantOfRestitution = 1.0;

            Ability::init(c);
            Acceleration::init(c);
            SpinVelocity::init(c);

            registerName("Dynamics");
        }

        Object *Dynamics::add(Object *o) {
            Force *f = dynamic_cast<Force *>(o);
            if (f) {
                forces.push_back(f);
            }

            return Ability::add(o);
        }

        void Dynamics::receive(Message *m) {
            CollisionMessage *c = dynamic_cast<CollisionMessage *>(m);

            if (c) {
                double j = -1.0 - constantOfRestitution;
                double den = 0.0;
                Vector *n = c->getNormal();
                Element *e = c->getOther();
                Mass *omass = dynamic_cast<Mass *>(e->lookupAbility("Mass"));
                SpinVelocity *ospinvelocity = dynamic_cast<SpinVelocity *>(e->lookupAbility("SpinVelocity"));
                Velocity *ovelocity = dynamic_cast<Velocity *>(e->lookupAbility("Velocity"));

                Vector p1 = Vector(*c->getP1());
                Vector p2 = Vector(*c->getP2());
                p1.scale(scale);
                Scale *oscale = dynamic_cast<Scale *>(e->lookupAbility("Scale"));
                if (oscale) {
                    p2.scale(oscale->getScale());
                }

                double m = 0.0;
                if (omass) {
                    m = omass->getMass();
                }

                Vector v;
                if (ovelocity)
                    v = ovelocity->getVelocity();

                if (mass != 0.0)
                    den += 1.0 / mass;
                if (m != 0.0)
                    den += 1.0 / m;
                den *= (*n * *n);

                Vector tmp;
                if (mass != 0.0)
                    tmp = (inverseInertiaTensor * (p1 % *n)) % p1;
                if ((m != 0.0) && (ospinvelocity)) {
                    tmp += (ospinvelocity->getInverseWorldInertiaTensor() * (p2 % *n)) % p2;
                }

                den += (tmp * *n);

                tmp = velocity + ((inverseInertiaTensor * spinvelocity) % p1);
                if (ospinvelocity && ovelocity)
                    tmp += (O - ovelocity->getVelocity() - ((*ospinvelocity->getInverseInertiaTensor() * ospinvelocity->getSpinVelocity()) % p2));

                j *= (tmp * *n);
                j /= den;

                if (mass != 0.0) {
                    velocity = velocity + ((j / mass) * *n);
                    spinvelocity += (p1 % (j * *n));
                    //          velocity = O;
                    spinvelocity = O;
                }

                position = oldPosition;
                copyMatrix(rotation, oldRotation);
            } else {
                Acceleration::receive(m);
                SpinVelocity::receive(m);
            }

        }

        Register(Dynamics)

    };
};

---

Anoid NG © Michael Westergaard, Martin Stig Stissing, Ronni Michael Laursen, and Kristian Bisgaard Lassen