vos/metaobjects/property/extrapolatedproperty.cc

Go to the documentation of this file.

/*
    This file is part of the Virtual Object System of
    the Interreality project (http://interreality.org).

    Copyright (C) 2001, 2002 Peter Amstutz

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation; either
    version 2 of the License, or (at your option) any later version.

    This library is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
    Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public
    License along with this library; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307  USA

    Peter Amstutz <tetron@interreality.org>
*/

#include "property.hh"
#include "extrapolatedproperty.hh"

set<ExtrapolatedProperty*> ExtrapolatedProperty::allexps;


ExtrapolatedProperty::ExtrapolatedProperty(MetaObject* superobject)
    : MetaObject(superobject), Property(superobject)
{
    allexps.insert(this);
}

ExtrapolatedProperty::~ExtrapolatedProperty()
{
    allexps.erase(this);
}

void ExtrapolatedProperty::registerExtenders()
{
    static bool done = false;
    if(! done) {
    LocalSite::addLocalObjectExtension("property:property.extrapolated", ExtrapolatedLocalProperty::new_ExtrapolatedLocalProperty);
    LocalSite::addLocalObjectExtension(typeid(ExtrapolatedLocalProperty).name(), ExtrapolatedLocalProperty::new_ExtrapolatedLocalProperty);
    LocalSite::addLocalObjectExtension(typeid(ExtrapolatedProperty).name(), ExtrapolatedLocalProperty::new_ExtrapolatedLocalProperty);
    RemoteSite::addRemoteObjectExtension("property:property.extrapolated", ExtrapolatedRemoteProperty::new_ExtrapolatedRemoteProperty);
    RemoteSite::addRemoteObjectExtension(typeid(ExtrapolatedRemoteProperty).name(), ExtrapolatedRemoteProperty::new_ExtrapolatedRemoteProperty);
    RemoteSite::addRemoteObjectExtension(typeid(ExtrapolatedProperty).name(), ExtrapolatedRemoteProperty::new_ExtrapolatedRemoteProperty);
    done = true;
    }
}

string ExtrapolatedProperty::stringifyVector(const vector<double>& x)
{
    string out;
    char n[64];

    if(x.size() > 0) {
        snprintf(n, sizeof(n), "%f", x[0]);
        out += n;
        for(unsigned int i = 1; i < x.size(); i++) {
            out += " ";
            snprintf(n, sizeof(n), "%f", x[i]);
            out += n;
        }
    }

    return out;
}

void ExtrapolatedProperty::updateAll()
{
    for(AllExpSet::iterator i = allexps.begin(); i != allexps.end(); i++) {
        try {
            (*i)->update();
        } catch(runtime_error e) {
            LOG("extrapolatedproperty", 2, "updateAll caught exception: " << e.what());
        }
    }
}

void ExtrapolatedProperty::calculatePosition(vector<double>& newx, vector<double>& newdx, double& t)
{
    t = now() - lastCheckpoint;

    // todo: add slewing calculations
    // (also need to filter estimated time until next update)

    newx.resize(x.size());
    newdx.resize(x.size());

    for(unsigned int i = 0; i < x.size(); i++) {
        newx[i] = (ddx[i]/2)*(t*t) + dx[i]*t + x[i];
        newdx[i] = ddx[i]*t + dx[i];
    }
}

bool isVectorZero (vector<double> v)
{
    for (unsigned int i = 0; i < v.size(); i++)
        if (v[i] != 0) return false;
    return true;
}

void ExtrapolatedProperty::update()
{
    if (isVectorZero (dx) && isVectorZero (ddx)) return;

    vector<double> newx;
    vector<double> newdx;
    double t;

    calculatePosition(newx, newdx, t);

    /// XXX is this correct to remain sync'd with the normal property
    //interface? rh XXX
    Property::replace(stringifyVector(newx), "text/x-vector-float");

    for(ExtrapolationListenerSet::iterator i = extrapolationListeners.begin();
        i != extrapolationListeners.end();
        i++)
    {
        (*i)->notifyPositionChange(*this, newx, newdx, ddx, t);
    }
}

void ExtrapolatedProperty::addExtrapolationListener(ExtrapolatedPropertyListener* el)
{
    extrapolationListeners.insert(el);
    if(RefCounted* rc = dynamic_cast<RefCounted*>(el)) {
        rc->acquire(); // released by: removeExtrapolationListener(), doExcise() or notifyObjectExcise()
        rc->addExciseListener(this);
    }

    double t = now() - lastCheckpoint;
    el->notifyBaseChange(*this, x, dx, ddx, t);
    vector<double> newx;
    vector<double> newdx;
    calculatePosition(newx, newdx, t);
    el->notifyPositionChange(*this, newx, newdx, ddx, t);
}

void ExtrapolatedProperty::removeExtrapolationListener(ExtrapolatedPropertyListener* x)
{
    extrapolationListeners.erase(x);
    if(RefCounted* rc = dynamic_cast<RefCounted*>(x)) {
        rc->removeExciseListener(this);
        rc->release(); // acquride by: addExtrapolationListener
    }
}

void ExtrapolatedProperty::notifyObjectExcise(RefCounted* object)
{
    Property::notifyObjectExcise(object);

    if(ExtrapolatedPropertyListener* pl = dynamic_cast<ExtrapolatedPropertyListener*>(object)) {
        removeExtrapolationListener(pl);
    }
}

void ExtrapolatedProperty::doExcise()
{
    Property::doExcise();

    for(ExtrapolationListenerSet::iterator i = extrapolationListeners.begin();
        i != extrapolationListeners.end(); i++)
    {
        if(RefCounted* rc = dynamic_cast<RefCounted*>(*i)) {
            rc->removeExciseListener(this);
            rc->release(); // acquired by: addExtrapolationListener()
        }
    }
    extrapolationListeners.clear();
}

void ExtrapolatedProperty::stringToVector(const string& prop, vector<double>& x)
{
    unsigned int i = 0;
    double value;

    x.clear();
    while(i < prop.size()) {
        while(i < prop.size() && isspace(prop[i])) i++;
        if(i >= prop.size()) break;
        if(sscanf(prop.c_str() + i, "%lf", &value) != 1) break;
        x.push_back(value);
        while(i < prop.size() && !isspace(prop[i])) i++;
    }
}

const string ExtrapolatedProperty::getType()
{
    return "property:property.extrapolated";
}

double ExtrapolatedProperty::now()
{
#ifdef WIN32
    return getTimer();
#else
    return getRealTime();
#endif
}

void ExtrapolatedProperty::getCurrent(vector<double>& ax,
                                      vector<double>& adx,
                                      vector<double>& addx,
                                      double& t)
{
    calculatePosition(ax, adx, t);
    if(ddx.size() != x.size()) ddx.resize(x.size());
    addx = ddx;

    t = now() - lastCheckpoint;
}

void ExtrapolatedProperty::getBase(vector<double>& ax,
                                   vector<double>& adx,
                                   vector<double>& addx)
{
    ax = x;
    adx = dx;
    addx = ddx;
}

void ExtrapolatedProperty::read(string& target, int start, int length) {
    readRaw(target, start, length);
}

int ExtrapolatedProperty::getLength() {
    return getRawLength();
}


void ExtrapolatedProperty::readRaw(string& target, int start, int length) {
    vector<double> newx;
    vector<double> newdx;
    double t;

    calculatePosition(newx, newdx, t);
    if(length == -1)
        target = stringifyVector(newx).substr(start);
    else
        target = stringifyVector(newx).substr(start, length);

}

int ExtrapolatedProperty::getRawLength() {
    vector<double> newx;
    vector<double> newdx;
    double t;

    calculatePosition(newx, newdx, t);

    return stringifyVector(newx).size();
}



// Local extrapolated property definition

ExtrapolatedLocalProperty::ExtrapolatedLocalProperty(MetaObject* superobject)
    : MetaObject(superobject), Property(superobject), LocalProperty(superobject),
      BufferedLocalProperty(superobject), ExtrapolatedProperty(superobject),
      extrap_base_changed(true)
{
}

ExtrapolatedLocalProperty::~ExtrapolatedLocalProperty()
{
}

MetaObject* ExtrapolatedLocalProperty::new_ExtrapolatedLocalProperty(MetaObject* superobject, const string& type)
{
    return new ExtrapolatedLocalProperty(superobject);
}

void ExtrapolatedLocalProperty::write(int start, const string& newdata)
{
    readRaw(raw_data, 0, -1);
    raw_data.replace(start, newdata.size(), newdata);
    stringToVector(raw_data, x);
    BufferedLocalProperty::replace(raw_data, getDataType());
    dx.clear();
    ddx.clear();
    for(unsigned int i = 0; i < x.size(); i++) {
        dx.push_back(0);
        ddx.push_back(0);
    }
    extrap_base_changed = true;
}

void ExtrapolatedLocalProperty::replace(const string& newdata, const string& newtype)
{
    vector<double> newx;
    stringToVector(newdata, newx);
    dx.clear();
    ddx.clear();
    for(unsigned int i = 0; i < x.size(); i++) {
        dx.push_back(0);
        ddx.push_back(0);
    }
    BufferedLocalProperty::replace(newdata, newtype);
    setPosition(newx);
}

void ExtrapolatedLocalProperty::set(const vector<double>& ax,
                                    const vector<double>& adx,
                                    const vector<double>& addx,
                                    double t)
{
    x.resize(ax.size());
    x = ax;
    dx = adx;
    ddx = addx;
    dx.resize(x.size());
    ddx.resize(x.size());

    lastCheckpoint = now() - t;

    extrap_base_changed = true;

    for(ExtrapolationListenerSet::iterator i = extrapolationListeners.begin();
        i != extrapolationListeners.end();
        i++)
    {
        try {
            // only send to local, will send to remote on next flush
            if(! dynamic_cast<ExtrapolatedListenerSiteWrapper*>(*i)) (*i)->notifyBaseChange(*this, x, dx, ddx, t);
        } catch(...) { }
    }
}

void ExtrapolatedLocalProperty::sendMessage(Message* m)
{

    BufferedLocalProperty::sendMessage(m);

    rREF(Vobject&, from, findObjectFromRoot(m->getFrom()),

         if(m->getMethod() == "property:start-listening") {
             try {
                 for(int i = 0; i < m->getNumFields(); i++) {
                     if(m->getField(i).key == "listen"
                        && m->getField(i).value == "property.extrapolated") {
                         vRef<Site> st = m->getSourceSite();

                         vRef<PropertyEvent> event = new PropertyEvent(
                             PropertyEvent::PropertyRead,
                             from, *this,
                             BufferedLocalProperty::read(),
                             getDataType(),
                             true);
                         string message;
                         if(validatePropertyAccess(*event, message)) {
                             ExtrapolatedPropertyListener* pl = new ExtrapolatedListenerSiteWrapper(&st);
                             addExtrapolationListener(pl);
                             st->addExciseListener(this);
                             try {
                                 pl->notifyBaseChange(*this, x, dx, ddx, now() - lastCheckpoint);
                             } catch(...) { }
                         }
                         break;
                     }
                 }
             } catch(Message::NoSuchFieldError) { }
         } else if(m->getMethod() == "property:extrapolated-set") {
             string nx;
             string ndx;
             string nddx;

             try {
                 nx = m->getField("x").value;
             } catch(Message::NoSuchFieldError) { }
             try {
                 ndx = m->getField("dx").value;
             } catch(Message::NoSuchFieldError) { }
             try {
                 nddx = m->getField("ddx").value;
             } catch(Message::NoSuchFieldError) {
             }
             pREF(Message*, reply, new Message(),
                  LocalVobject::initReply(this, reply, m, "property:extrapolated-update");
                  string dt = getDataType();

                  vRef<PropertyEvent> event = new PropertyEvent(
                             PropertyEvent::PropertyReplace,
                             from, *this,
                             nx, dt,
                             BufferedLocalProperty::read(),
                             dt,
                             true);
                  string message;
                  if(validatePropertyAccess(*event, message)) {
                      vector<double> _x;
                      vector<double> _dx;
                      vector<double> _ddx;

                      stringToVector(nx, _x);
                      stringToVector(ndx, _dx);
                      stringToVector(nddx, _ddx);

                      set(_x, _dx, _ddx);

                      nx = stringifyVector(x);
                      ndx = stringifyVector(dx);
                      nddx = stringifyVector(ddx);
                        ndx.resize(nx.size());
                        nddx.resize(nx.size());

                      double t = now() - lastCheckpoint;
                      char st[32];
                      snprintf(st, sizeof(st), "%f", t);

                      reply->insertField(-1, "x", nx);
                      reply->insertField(-1, "dx", ndx);
                      reply->insertField(-1, "ddx", nddx);
                      reply->insertField(-1, "t", st);
                  } else {
                      reply->insertField(-1, "error", message);
                  }
                  from.sendMessage(reply);
                 );
         }
        );
}

void ExtrapolatedLocalProperty::flush()
{
    changed = true;
    BufferedLocalProperty::flush();
    if(extrap_base_changed) {
        double t = now() - lastCheckpoint;
        for(ExtrapolationListenerSet::iterator i = extrapolationListeners.begin();
            i != extrapolationListeners.end();
            i++)
        {
            // send update only if it is NOT local
            try {
                if(dynamic_cast<ExtrapolatedListenerSiteWrapper*>(*i)) (*i)->notifyBaseChange(*this, x, dx, ddx, t);
            } catch(...) { }
        }
        extrap_base_changed = false;
    }
}



// Remote extrapolated property definition

ExtrapolatedRemoteProperty::~ExtrapolatedRemoteProperty()
{
}

ExtrapolatedRemoteProperty::ExtrapolatedRemoteProperty(MetaObject* superobject)
    : MetaObject(superobject), Property(superobject), RemoteProperty(superobject),
      BufferedRemoteProperty(superobject), ExtrapolatedProperty(superobject)
{
}

MetaObject* ExtrapolatedRemoteProperty::new_ExtrapolatedRemoteProperty(MetaObject* superobject, const string& type)
{
    return new ExtrapolatedRemoteProperty(superobject);
}

void ExtrapolatedRemoteProperty::addExtrapolationListener(ExtrapolatedPropertyListener* x)
{
    if(extrapolationListeners.empty()) {
        pREF(Message*, m, new Message(),
             rREF(LocalSite&, ls, RemoteVobject::initFields(this, m, "property:start-listening", false), );
             m->insertField(-1, "listen", "property.extrapolated");

             sendMessage(m);
            );
    }
    ExtrapolatedProperty::addExtrapolationListener(x);
}

void ExtrapolatedRemoteProperty::removeExtrapolationListener(ExtrapolatedPropertyListener* x)
{
    ExtrapolatedProperty::removeExtrapolationListener(x);
    if(extrapolationListeners.empty()) {
        pREF(Message*, m, new Message(),
             rREF(LocalSite&, ls, RemoteVobject::initFields(this, m, "property:stop-listening", false), );
             m->insertField(-1, "listen", "property.extrapolated");

             sendMessage(m);
            );
    }
}

void ExtrapolatedRemoteProperty::set(const vector<double>& x, const vector<double>& dx, const vector<double>& ddx, double t)
{
    pREF(Message*, m, new Message(),
         rREF(LocalSite&, ls, RemoteVobject::initFields(this, m, "property:extrapolated-set", false), );

         char st[32];
         snprintf(st, sizeof(st), "%f", t);

         m->insertField(-1, "x", stringifyVector(x));
         m->insertField(-1, "dx", stringifyVector(dx));
         m->insertField(-1, "ddx", stringifyVector(ddx));
         m->insertField(-1, "t", st);

         sendMessage(m);
        );
}

void ExtrapolatedRemoteProperty::sendUpdateMessage(Message* m)
{
    BufferedRemoteProperty::sendMessage(m);

    if(m->getMethod() == "property:extrapolated-update") {
        LOG("extrapolatedproperty", 3, "got extrap: " << m->getLoggableString());
        try {
            const Message::Field& _st = m->getField("t");
            const Message::Field& _sx = m->getField("x");
            const Message::Field& _sdx = m->getField("dx");
            const Message::Field& _sddx = m->getField("ddx");

            double _t;

            stringToVector(_sx.value, x);
            stringToVector(_sdx.value, dx);
            stringToVector(_sddx.value, ddx);

            sscanf(_st.value.c_str(), "%lf", &_t);

            lastCheckpoint = now() - _t;

            for(ExtrapolationListenerSet::iterator i = extrapolationListeners.begin();
                i != extrapolationListeners.end();
                i++)
            {
                try {
                    (*i)->notifyBaseChange(*this, x, dx, ddx, _t);
                } catch(...) { }
            }
        } catch(Message::NoSuchFieldError) {
        }
    }
}

// ExtrapolatedListenerSiteWrapper implementation

ExtrapolatedListenerSiteWrapper::ExtrapolatedListenerSiteWrapper(Site* s)
    : site(s)
{
}

ExtrapolatedListenerSiteWrapper::~ExtrapolatedListenerSiteWrapper()
{
}

void ExtrapolatedListenerSiteWrapper::notifyBaseChange(ExtrapolatedProperty& ep,
                                                           const vector<double>& position,
                                                           const vector<double>& velocity,
                                                           const vector<double>& acceleration,
                                                           double t)
{
    string nx = ExtrapolatedProperty::stringifyVector(position);
    string ndx = ExtrapolatedProperty::stringifyVector(velocity);
    string nddx = ExtrapolatedProperty::stringifyVector(acceleration);

    char st[32];
    snprintf(st, sizeof(st), "%f", t);

    vRef<Message> update = new Message();
    update->setType("update");
    update->setFrom(ep.getURL().getString());
    update->setTo(site->getURL().getString());
    update->setMethod("property:extrapolated-update");

    update->insertField(-1, "x", nx);
    update->insertField(-1, "dx", ndx);
    update->insertField(-1, "ddx", nddx);
    update->insertField(-1, "t", st);

    site->sendMessage(&update);
}

void ExtrapolatedListenerSiteWrapper::notifyPositionChange(ExtrapolatedProperty& ep,
                                                           const vector<double>& position,
                                                           const vector<double>& velocity,
                                                           const vector<double>& acceleration,
                                                           double t)
{
}

/* various convenience methods */


void ExtrapolatedProperty::initialize() {
    initialize(1, 0.0);
}

void ExtrapolatedProperty::initialize(unsigned int size, double val) {
    vector<double> x;
    vector<double> dx;
    vector<double> ddx;
    x.resize(size);
    dx.resize(size);
    ddx.resize(size);
    for(unsigned int i = 0; i < size; i++)
        x[i] = dx[i] = ddx[i] = val;
    set(x, dx, ddx, 0);
}

void ExtrapolatedProperty::setVelocity(const vector<double>& newv) {
    vector <double>x;
    vector <double>v;
    vector <double>a;
    double t;
    getCurrent(x, v, a, t);
    x.resize(newv.size());
    a.resize(newv.size());
    set(x, newv, a, 0);
}

void ExtrapolatedProperty::setAcceleration(const vector<double>& newa) {
    vector <double>x;
    vector <double>v;
    vector <double>a;
    double t;
    getCurrent(x, v, a, t);
    x.resize(newa.size());
    v.resize(newa.size());
    set(x, v, newa, 0);

}

void ExtrapolatedProperty::setPosition(const vector<double>& newx) {
    vector <double>x;
    vector <double>v;
    vector <double>a;
    double t;
    getCurrent(x, v, a, t);
    v.resize(newx.size());
    a.resize(newx.size());
    set(newx, v, a, 0);
}

void ExtrapolatedProperty::setVelocity(const double newv) {
    vector <double>x;
    vector <double>v;
    vector <double>a;
    double t;
    getCurrent(x, v, a, t);
    v[0] = newv;
    set(x, v, a, 0);
}

void ExtrapolatedProperty::setAcceleration(const double newa) {
    vector <double>x;
    vector <double>v;
    vector <double>a;
    double t;
    getCurrent(x, v, a, t);
    a[0] = newa;
    set(x, v, a, 0);
}

void ExtrapolatedProperty::setPosition(const double newx) {
    vector <double>x;
    vector <double>v;
    vector <double>a;
    double t;
    getCurrent(x, v, a, t);
    x[0] = newx;
    set(x, v, a, 0);
}



void ExtrapolatedProperty::getCurrentPos(vector<double>& x) {
    vector <double>v;
    vector <double>a;
    double t;
    getCurrent(x, v, a, t);
}



void ExtrapolatedProperty::getVelocity(double& val) {
    vector <double>x;
    vector <double>v;
    vector <double>a;
    double t;
    getCurrent(x, v, a, t);
    val = v[0];
}

void ExtrapolatedProperty::getVelocity(vector<double>& v) {
    vector <double>x;
    vector <double>a;
    double t;
    getCurrent(x, v, a, t);
}



void ExtrapolatedProperty::getAcceleration(double& acc) {
    vector <double>x;
    vector <double>v;
    vector <double>a;
    double t;
    getCurrent(x, v, a, t);
    acc = a[0];
}

void ExtrapolatedProperty::getAcceleration(vector<double>& a) {
    vector <double>x;
    vector <double>v;
    double t;
    getCurrent(x, v, a, t);
}

void ExtrapolatedRemoteProperty::readRaw(string& target, int start, int length) {
    if(extrapolationListeners.empty())
        RemoteProperty::readRaw(target, start, length);
    else
        ExtrapolatedProperty::readRaw(target, start, length);
}


int ExtrapolatedRemoteProperty::getRawLength() {
    if(extrapolationListeners.empty())
        return RemoteProperty::getRawLength();
    else
        return ExtrapolatedProperty::getRawLength();
}

---