phycylinder.cpp

/********************************************************************************
 *  WorldSim -- library for robot simulations                                   *
 *  Copyright (C) 2008-2011 Gianluca Massera <emmegian@yahoo.it>                *
 *                                                                              *
 *  This program is free software; you can redistribute it and/or modify        *
 *  it under the terms of the GNU General Public License as published by        *
 *  the Free Software Foundation; either version 2 of the License, or           *
 *  (at your option) any later version.                                         *
 *                                                                              *
 *  This program 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 General Public License for more details.                                *
 *                                                                              *
 *  You should have received a copy of the GNU General Public License           *
 *  along with this program; if not, write to the Free Software                 *
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA  *
 ********************************************************************************/

#include "phycylinder.h"
#include "private/phyobjectprivate.h"
#include "private/worldprivate.h"
#include <QtAlgorithms>

namespace farsa {

PhyCylinder::PhyCylinder( real radius, real height, World* w, QString name, const wMatrix& tm ) :
    PhyObject( w, name, tm, false ),
    radiusv(radius),
    heightv(height),
    m_upperBaseColor(),
    m_lowerBaseColor(),
    m_segmentsColor(),
    m_uniformColor(),
    m_graphicalRepresentationNeedsUpdate(),
    m_oldColorv()
{
    w->pushObject( this );
    createPrivateObject();
    changedMatrix();

    m_uniformColor.append(SegmentColor(SimpleInterval(-PI_GRECO, PI_GRECO), colorv));
}

PhyCylinder::~PhyCylinder() {
#ifdef WORLDSIM_USE_NEWTON
    /* nothing to do */
#endif
}

void PhyCylinder::setUpperBaseColor(QColor color)
{
    m_upperBaseColor = color;

    if (colorv.isValid()) {
        m_lowerBaseColor = colorv;
        m_segmentsColor.append(SegmentColor(SimpleInterval(-PI_GRECO, PI_GRECO), colorv));
        colorv = QColor();
    }

    setUpdateAllGraphicalRepresentation();
}

void PhyCylinder::setLowerBaseColor(QColor color)
{
    m_lowerBaseColor = color;

    if (colorv.isValid()) {
        m_upperBaseColor = colorv;
        m_segmentsColor.append(SegmentColor(SimpleInterval(-PI_GRECO, PI_GRECO), colorv));
        colorv = QColor();
    }

    setUpdateAllGraphicalRepresentation();
}

namespace {
    // Utility class used in the function PhyCylinder::setSegmentsColor(). This is
    // used basically to sort section of various color by ascending interval start
    class SimpleIntervalAndColor {
    public:
        SimpleIntervalAndColor() :
            interval(),
            color()
        {
        }

        SimpleIntervalAndColor(const SimpleInterval& i, QColor c) :
            interval(i),
            color(c)
        {
        }

        bool operator<(const SimpleIntervalAndColor& other) const
        {
            return interval < other.interval;
        }

        SimpleInterval interval;
        QColor color;
    };
}

void PhyCylinder::setSegmentsColor(QColor base, const QList<SegmentColor>& segmentsColor)
{
    if (segmentsColor.isEmpty()) {
        m_segmentsColor.clear();
        m_segmentsColor << SegmentColor(SimpleInterval(-PI_GRECO, PI_GRECO), base);

        return;
    }

    // The part of the cylinder not in segments of the list (which at the end will have the base color)
    Intervals remainingArc(SimpleInterval(-PI_GRECO, PI_GRECO));

    // Adding the first interval. We use a temporary list because the final one will have a different structure
    QList<SegmentColor> tmpSegments;
    const Intervals intervalToAdd = segmentsColor[0].intervals & remainingArc;
    remainingArc -= segmentsColor[0].intervals;
    tmpSegments << SegmentColor(intervalToAdd, segmentsColor[0].color);
    for (QList<SegmentColor>::const_iterator it = segmentsColor.begin(); it != segmentsColor.end(); it++) {
        const Intervals intervalToAdd = it->intervals & remainingArc;
        remainingArc -= it->intervals;
        tmpSegments << SegmentColor(intervalToAdd, it->color);
    }
    // If the remainingArc is not empty, adding the final SegmentColor
    if (!remainingArc.isEmpty()) {
        tmpSegments << SegmentColor(remainingArc, base);
    }

    // Now we fill m_segmentsColor so that it respects the format described in segmentsColor(). First we create
    // a vector of SimpleIntervalAndColor, then sort it and finally use it to populate m_segmentsColor. This procedure
    // works becayse tmpSegments is made up of  intervals that cover the whole circle and without any overlapping
    // Creating a vector of SimpleIntervalAndColor
    QList<SimpleIntervalAndColor> simpleIntervals;
    for (QList<SegmentColor>::const_iterator segmentIt = tmpSegments.constBegin(); segmentIt != tmpSegments.constEnd(); ++segmentIt) {
        for (Intervals::const_iterator intervalIt = segmentIt->intervals.constBegin(); intervalIt != segmentIt->intervals.constEnd(); ++intervalIt) {
            simpleIntervals << SimpleIntervalAndColor(*intervalIt, segmentIt->color);
        }
    }
    // Sorting the vector we just generated
    qSort(simpleIntervals);
    // Now filling m_segmentsColor
    m_segmentsColor.clear();
    for (QList<SimpleIntervalAndColor>::const_iterator it = simpleIntervals.begin(); it != simpleIntervals.end(); ++it) {
        m_segmentsColor << SegmentColor(it->interval, it->color);
    }

    if (colorv.isValid()) {
        m_upperBaseColor = colorv;
        m_lowerBaseColor = colorv;
        colorv = QColor();
    }

    setUpdateAllGraphicalRepresentation();
}

bool PhyCylinder::graphicalRepresentationNeedsUpdate(void* renderer)
{
    // Our current color is not ours if we inherit color from parent! We need to get the color of our parent in that case
    WObject* obj = this;
    while (obj->useColorTextureOfOwner() && (obj->owner() != NULL)) {
        WObject *const owner = dynamic_cast<WObject *>(obj->owner());
        if (owner != NULL) {
            obj = owner;
        } else {
            break;
        }
    }
    QColor colorToTest = obj->color();

    // Checking if color has changed
    if (m_oldColorv != colorToTest) {
        setUpdateAllGraphicalRepresentation();
    }

    if (!m_graphicalRepresentationNeedsUpdate.contains(renderer)) {
        m_graphicalRepresentationNeedsUpdate[renderer] = true;
    }

    const bool ret = m_graphicalRepresentationNeedsUpdate[renderer];

    m_graphicalRepresentationNeedsUpdate[renderer] = false;
    m_oldColorv = colorToTest;

    return ret;
}

void PhyCylinder::setUpdateAllGraphicalRepresentation()
{
    for (QMap<void*, bool>::iterator it = m_graphicalRepresentationNeedsUpdate.begin(); it != m_graphicalRepresentationNeedsUpdate.end(); ++it) {
        it.value() = true;
    }
}

void PhyCylinder::createPrivateObject() {
#ifdef WORLDSIM_USE_NEWTON
    NewtonCollision* c = NewtonCreateCylinder( worldpriv->world, radiusv, heightv, 1, 0 );
    wMatrix initialTransformationMatrix = wMatrix::identity(); // The transformation matrix is set in other places
    priv->body = NewtonCreateBody( worldpriv->world, c, &initialTransformationMatrix[0][0] );
    priv->collision = c;
    NewtonBodySetAutoSleep( priv->body, 0 );
    setMass( 1 );
    NewtonBodySetUserData( priv->body, this );
    NewtonBodySetLinearDamping( priv->body, 0.0 );
    wVector zero = wVector(0,0,0,0);
    NewtonBodySetAngularDamping( priv->body, &zero[0] );
    NewtonBodySetAutoSleep( priv->body, 0 );
    NewtonBodySetFreezeState( priv->body, 0 );
    // Sets the signal-wrappers callback
    NewtonBodySetTransformCallback( priv->body, (PhyObjectPrivate::setTransformHandler) );
    NewtonBodySetForceAndTorqueCallback( priv->body, (PhyObjectPrivate::applyForceAndTorqueHandler) );
#endif
}

} // end namespace farsa