---

spinvelocity.cc

Go to the documentation of this file.

#include "spinvelocity.hh"
#include "register.hh"
#include <iostream>
#include "abstractworld.hh"

namespace anoid {
    namespace plugin {

        using namespace simple;
        using namespace config;

        void SpinVelocity::init(Configuration &c) {
            spinvelocity = c.getVector("spinvelocity");

            Ability::init(c);
            Spin::init(c);
            Scale::init(c);
            Mass::init(c);

            if (parentElement) {
                if (parentElement->getShape()) {
                    parentElement->getShape()->getInverseInertiaTensor(
                            inverseInertiaTensor,
                            scale.getX(),
                            scale.getY(),
                            scale.getZ());
                    inverseInertiaTensor[0] /= mass;
                    inverseInertiaTensor[1] /= mass;
                    inverseInertiaTensor[2] /= mass;
                    inverseInertiaTensor[4] /= mass;
                    inverseInertiaTensor[5] /= mass;
                    inverseInertiaTensor[6] /= mass;
                    inverseInertiaTensor[8] /= mass;
                    inverseInertiaTensor[9] /= mass;
                    inverseInertiaTensor[10] /= mass;
                }
            }

            spinvelocity.setX(spinvelocity.getX() / inverseInertiaTensor[0]);
            spinvelocity.setY(spinvelocity.getY() / inverseInertiaTensor[5]);
            spinvelocity.setZ(spinvelocity.getZ() / inverseInertiaTensor[10]);

            registerName("SpinVelocity");
        }

        Vector SpinVelocity::getSpinVelocity() {
            return spinvelocity;
        }

        void SpinVelocity::setSpinVelocity(Vector &s) {
            spinvelocity = s;
        }

        void SpinVelocity::update() {
            Vector tmp;

            copyMatrix(inverseWorldInertiaTensor, rotation);

            matMult(inverseWorldInertiaTensor, inverseInertiaTensor);
            matMult(inverseWorldInertiaTensor, transpose(rotation));
            transpose(rotation);

            tmp = inverseWorldInertiaTensor * spinvelocity;

            double t = getWorld()->getTime();

            matMult(tilde(spin, tmp), rotation);
            spin[0] *= t;
            spin[1] *= t;
            spin[2] *= t;
            spin[4] *= t;
            spin[5] *= t;
            spin[6] *= t;
            spin[8] *= t;
            spin[9] *= t;
            spin[10] *= t;

            Spin::update();
            Scale::update();
            Mass::update();
            Ability::update();
        }

        void SpinVelocity::redraw() {
            Spin::redraw();
            Scale::redraw();
            Mass::redraw();
            Ability::redraw();
        }

        Register(SpinVelocity)

    };
};

---

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