<commit_before>/*
 * Copyright (C) 2013 Daniel Nicoletti <dantti12@gmail.com>
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Library 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
 * Library General Public License for more details.
 *
 * You should have received a copy of the GNU Library General Public License
 * along with this library; see the file COPYING.LIB. If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
 * Boston, MA 02110-1301, USA.
 */

#include "dispatcher_p.h"

#include "common.h"
#include "context.h"
#include "controller.h"
#include "action.h"
#include "request_p.h"
#include "dispatchtypepath.h"

#include <QUrl>
#include <QMetaMethod>
#include <QStringBuilder>
#include <QDebug>

using namespace Cutelyst;

Dispatcher::Dispatcher(QObject *parent) :
    QObject(parent),
    d_ptr(new DispatcherPrivate)
{
    Q_D(Dispatcher);
}

Dispatcher::~Dispatcher()
{
    delete d_ptr;
}

void Dispatcher::setupActions(const QList<Controller*> &controllers)
{
    Q_D(Dispatcher);

    if (d->dispatchers.isEmpty()) {
        registerDispatchType(new DispatchTypePath(this));
    }

    foreach (Controller *controller, controllers) {
        // App controller
//        qDebug() << "Found a controller:" << controller << meta->className();
        const QMetaObject *meta = controller->metaObject();
        controller->setObjectName(meta->className());
        bool instanceUsed = false;
        for (int i = 0; i < meta->methodCount(); ++i) {
            QMetaMethod method = meta->method(i);
            if (method.methodType() == QMetaMethod::Method) {
                //                    qDebug() << Q_FUNC_INFO << method.name() << method.attributes() << method.methodType() << method.methodSignature();
                //                    qDebug() << Q_FUNC_INFO << method.parameterTypes() << method.tag() << method.access();
                bool registered = false;
                Action *action = new Action(method, controller);
                if (action->isValid() && !d->actionHash.contains(action->privateName())) {
                    if (!action->attributes().contains("Private")) {
                        // Register the action with each dispatcher
                        foreach (DispatchType *dispatch, d->dispatchers) {
                            if (dispatch->registerAction(action)) {
                                registered = true;
                            }
                        }
                    } else {
                        // We register private actions
                        registered = true;
                    }
                }

                // The Begin, Auto, End actions are not
                // registered by Dispatchers but we need them
                // as private actions anyway
                if (registered || action->isValid()) {
                    d->actionHash.insert(action->privateName(), action);
                    d->containerHash[action->ns()] << action;
                    instanceUsed = true;
                } else {
                    delete action;
                }
            }
        }

        if (instanceUsed) {
            d->constrollerHash.insert(meta->className(), controller);
        }
    }

    printActions();
}

bool Dispatcher::dispatch(Context *ctx)
{
    if (ctx->action()) {
        return ctx->forward(QLatin1Char('/') % ctx->action()->ns() % QLatin1String("/_DISPATCH"));
    } else {
        QString error;
        const QString &path = ctx->req()->path();
        if (path.isEmpty()) {
            error = QLatin1String("No default action defined");
        } else {
            error = QLatin1String("Unknown resource \"") % path % QLatin1Char('"');
        }
        qCDebug(CUTELYST_DISPATCHER) << error;
        ctx->error(error);
    }
    return false;
}

bool Dispatcher::forward(Context *ctx, const QString &opname, const QStringList &arguments)
{
    Action *action = command2Action(ctx, opname);
    if (action) {
        return action->dispatch(ctx);
    }

    qCCritical(CUTELYST_DISPATCHER) << "Action not found" << action;
    return false;
}

void Dispatcher::prepareAction(Context *ctx)
{
    Q_D(Dispatcher);

    QString path = ctx->req()->path();
    QStringList pathParts = path.split(QLatin1Char('/'));
    QStringList args;

    // Root action
    pathParts.prepend(QLatin1String(""));

    while (!pathParts.isEmpty()) {
        path = pathParts.join(QLatin1Char('/'));
        path.remove(d->initialSlash);

        foreach (DispatchType *type, d->dispatchers) {
            if (type->match(ctx, path)) {
                if (!path.isEmpty()) {
                    qCDebug(CUTELYST_DISPATCHER) << "Path is" << path;
                }

                if (!ctx->args().isEmpty()) {
                    qCDebug(CUTELYST_DISPATCHER) << "Arguments are" << ctx->args().join(QLatin1Char('/'));
                }

                return;
            }
        }

        args.prepend(pathParts.takeLast());
        ctx->req()->d_ptr->args = unexcapedArgs(args);
    }
}

Action *Dispatcher::getAction(const QString &name, const QString &ns) const
{
    Q_D(const Dispatcher);
    if (name.isEmpty()) {
        return 0;
    }

    QString _ns = cleanNamespace(ns);

    return d->actionHash.value(_ns % QLatin1Char('/') % name);
}

ActionList Dispatcher::getActions(const QString &name, const QString &ns) const
{
    Q_D(const Dispatcher);

    ActionList ret;
    if (name.isEmpty()) {
        return ret;
    }

    QString _ns = cleanNamespace(ns);

    ActionList containers = d->getContainers(_ns);
    foreach (Action *action, containers) {
        if (action->name() == name) {
            ret.prepend(action);
        }
    }

    return ret;
}

QHash<QString, Controller *> Dispatcher::controllers() const
{
    Q_D(const Dispatcher);
    return d->constrollerHash;
}

QString Dispatcher::uriForAction(Action *action, const QStringList &captures)
{
    Q_D(const Dispatcher);
    foreach (DispatchType *dispatch, d->dispatchers) {
        QString uri = dispatch->uriForAction(action, captures);
        if (!uri.isNull()) {
            return uri.isEmpty() ? QLatin1String("/") : uri;
        }
    }
    return QString();
}

void Dispatcher::registerDispatchType(DispatchType *dispatchType)
{
    Q_D(Dispatcher);
    d->dispatchers.append(dispatchType);
}

void Dispatcher::printActions()
{
    Q_D(Dispatcher);

    QString buffer;
    QTextStream out(&buffer, QIODevice::WriteOnly);
    bool showInternalActions = false;

    out << "Loaded Private actions:" << endl;
    QString privateTitle("Private");
    QString classTitle("Class");
    QString methodTitle("Method");
    int privateLength = privateTitle.length();
    int classLength = classTitle.length();
    int actionLength = methodTitle.length();
    QMap<QString, Action*>::ConstIterator it = d->actionHash.constBegin();
    while (it != d->actionHash.constEnd()) {
        Action *action = it.value();
        QString path = it.key();
        if (!path.startsWith(QLatin1String("/"))) {
            path.prepend(QLatin1String("/"));
        }
        privateLength = qMax(privateLength, path.length());
        classLength = qMax(classLength, action->className().length());
        actionLength = qMax(actionLength, action->name().length());
        ++it;
    }

    out << "." << QString().fill(QLatin1Char('-'), privateLength).toUtf8().data()
        << "+" << QString().fill(QLatin1Char('-'), classLength).toUtf8().data()
        << "+" << QString().fill(QLatin1Char('-'), actionLength).toUtf8().data()
        << "." << endl;
    out << "|" << privateTitle.leftJustified(privateLength).toUtf8().data()
        << "|" << classTitle.leftJustified(classLength).toUtf8().data()
        << "|" << methodTitle.leftJustified(actionLength).toUtf8().data()
        << "|" << endl;
    out << "." << QString().fill(QLatin1Char('-'), privateLength).toUtf8().data()
        << "+" << QString().fill(QLatin1Char('-'), classLength).toUtf8().data()
        << "+" << QString().fill(QLatin1Char('-'), actionLength).toUtf8().data()
        << "." << endl;

    it = d->actionHash.constBegin();
    while (it != d->actionHash.constEnd()) {
        Action *action = it.value();
        if (showInternalActions || !action->name().startsWith(QLatin1Char('_'))) {
            QString path = it.key();
            if (!path.startsWith(QLatin1String("/"))) {
                path.prepend(QLatin1String("/"));
            }
            out << "|" << path.leftJustified(privateLength).toUtf8().data()
                << "|" << action->className().leftJustified(classLength).toUtf8().data()
                << "|" << action->name().leftJustified(actionLength).toUtf8().data()
                << "|" << endl;
        }
        ++it;
    }

    out << "." << QString().fill(QLatin1Char('-'), privateLength).toUtf8().data()
        << "+" << QString().fill(QLatin1Char('-'), classLength).toUtf8().data()
        << "+" << QString().fill(QLatin1Char('-'), actionLength).toUtf8().data()
        << ".";
    qCDebug(CUTELYST_DISPATCHER) << buffer.toUtf8().data();

    // List all public actions
    foreach (DispatchType *dispatch, d->dispatchers) {
        dispatch->list();
    }
}

Action *Dispatcher::command2Action(Context *ctx, const QString &command, const QStringList &extraParams)
{
    Q_D(Dispatcher);
//    qDebug() << Q_FUNC_INFO << "Command" << command;

    Action *ret = d->actionHash.value(command);
    if (!ret) {
        ret = invokeAsPath(ctx, command, ctx->args());
    }

    return ret;
}

QStringList Dispatcher::unexcapedArgs(const QStringList &args)
{
    QStringList ret;
    foreach (const QString &arg, args) {
        ret << QUrl::fromPercentEncoding(arg.toLocal8Bit());
    }
    return ret;
}

QString Dispatcher::actionRel2Abs(Context *ctx, const QString &path)
{
    QString ret = path;
    if (!ret.startsWith(QLatin1Char('/'))) {
        // TODO at Catalyst it uses
        // c->stack->last()->namespace
        QString ns = ctx->action()->ns();
        ret = ns % QLatin1Char('/') % path;
    }

    if (ret.startsWith(QLatin1Char('/'))) {
        ret.remove(0, 1);
    }

    return ret;
}

Action *Dispatcher::invokeAsPath(Context *ctx, const QString &relativePath, const QStringList &args)
{
    Action *ret = 0;
    QString path = actionRel2Abs(ctx, relativePath);

    QRegularExpression re("^(?:(.*)/)?(\\w+)?$");
    while (!path.isEmpty()) {
        QRegularExpressionMatch match = re.match(path);
        if (match.hasMatch()) {
            path = match.captured(1);
            ret = getAction(match.captured(2), path);
            if (ret) {
                break;
            }
        } else {
            break;
        }
    }

    return ret;
}

QString Dispatcher::cleanNamespace(const QString &ns) const
{
    QStringList ret;
    foreach (const QString &part, ns.split(QLatin1Char('/'))) {
        if (!part.isEmpty()) {
            ret << part;
        }
    }
    return ret.join(QLatin1Char('/'));
}

ActionList DispatcherPrivate::getContainers(const QString &ns) const
{
    ActionList ret;

    QString _ns = ns;
    if (_ns == QLatin1String("/")) {
        _ns = QLatin1String("");
    }

    while (!_ns.isEmpty()) {
        ret << containerHash.value(_ns);
        _ns = _ns.section(QLatin1Char('/'), 0, -2);
    }
    ret << containerHash.value(_ns);

    return ret;
}
<commit_msg>Let the dispatcher work with slots too<commit_after>/*
 * Copyright (C) 2013 Daniel Nicoletti <dantti12@gmail.com>
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Library 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
 * Library General Public License for more details.
 *
 * You should have received a copy of the GNU Library General Public License
 * along with this library; see the file COPYING.LIB. If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
 * Boston, MA 02110-1301, USA.
 */

#include "dispatcher_p.h"

#include "common.h"
#include "context.h"
#include "controller.h"
#include "action.h"
#include "request_p.h"
#include "dispatchtypepath.h"

#include <QUrl>
#include <QMetaMethod>
#include <QStringBuilder>
#include <QDebug>

using namespace Cutelyst;

Dispatcher::Dispatcher(QObject *parent) :
    QObject(parent),
    d_ptr(new DispatcherPrivate)
{
    Q_D(Dispatcher);
}

Dispatcher::~Dispatcher()
{
    delete d_ptr;
}

void Dispatcher::setupActions(const QList<Controller*> &controllers)
{
    Q_D(Dispatcher);

    if (d->dispatchers.isEmpty()) {
        registerDispatchType(new DispatchTypePath(this));
    }

    foreach (Controller *controller, controllers) {
        // App controller
//        qDebug() << "Found a controller:" << controller << meta->className();
        const QMetaObject *meta = controller->metaObject();
        controller->setObjectName(meta->className());
        bool instanceUsed = false;
        for (int i = 0; i < meta->methodCount(); ++i) {
            QMetaMethod method = meta->method(i);
            if (method.methodType() == QMetaMethod::Method ||
                    method.methodType() == QMetaMethod::Slot) {
                bool registered = false;
                Action *action = new Action(method, controller);
                if (action->isValid() && !d->actionHash.contains(action->privateName())) {
                    if (!action->attributes().contains("Private")) {
                        // Register the action with each dispatcher
                        foreach (DispatchType *dispatch, d->dispatchers) {
                            if (dispatch->registerAction(action)) {
                                registered = true;
                            }
                        }
                    } else {
                        // We register private actions
                        registered = true;
                    }
                }

                // The Begin, Auto, End actions are not
                // registered by Dispatchers but we need them
                // as private actions anyway
                if (registered || action->isValid()) {
                    d->actionHash.insert(action->privateName(), action);
                    d->containerHash[action->ns()] << action;
                    instanceUsed = true;
                } else {
                    delete action;
                }
            }
        }

        if (instanceUsed) {
            d->constrollerHash.insert(meta->className(), controller);
        }
    }

    printActions();
}

bool Dispatcher::dispatch(Context *ctx)
{
    if (ctx->action()) {
        return ctx->forward(QLatin1Char('/') % ctx->action()->ns() % QLatin1String("/_DISPATCH"));
    } else {
        QString error;
        const QString &path = ctx->req()->path();
        if (path.isEmpty()) {
            error = QLatin1String("No default action defined");
        } else {
            error = QLatin1String("Unknown resource \"") % path % QLatin1Char('"');
        }
        qCDebug(CUTELYST_DISPATCHER) << error;
        ctx->error(error);
    }
    return false;
}

bool Dispatcher::forward(Context *ctx, const QString &opname, const QStringList &arguments)
{
    Action *action = command2Action(ctx, opname);
    if (action) {
        return action->dispatch(ctx);
    }

    qCCritical(CUTELYST_DISPATCHER) << "Action not found" << action;
    return false;
}

void Dispatcher::prepareAction(Context *ctx)
{
    Q_D(Dispatcher);

    QString path = ctx->req()->path();
    QStringList pathParts = path.split(QLatin1Char('/'));
    QStringList args;

    // Root action
    pathParts.prepend(QLatin1String(""));

    while (!pathParts.isEmpty()) {
        path = pathParts.join(QLatin1Char('/'));
        path.remove(d->initialSlash);

        foreach (DispatchType *type, d->dispatchers) {
            if (type->match(ctx, path)) {
                if (!path.isEmpty()) {
                    qCDebug(CUTELYST_DISPATCHER) << "Path is" << path;
                }

                if (!ctx->args().isEmpty()) {
                    qCDebug(CUTELYST_DISPATCHER) << "Arguments are" << ctx->args().join(QLatin1Char('/'));
                }

                return;
            }
        }

        args.prepend(pathParts.takeLast());
        ctx->req()->d_ptr->args = unexcapedArgs(args);
    }
}

Action *Dispatcher::getAction(const QString &name, const QString &ns) const
{
    Q_D(const Dispatcher);
    if (name.isEmpty()) {
        return 0;
    }

    QString _ns = cleanNamespace(ns);

    return d->actionHash.value(_ns % QLatin1Char('/') % name);
}

ActionList Dispatcher::getActions(const QString &name, const QString &ns) const
{
    Q_D(const Dispatcher);

    ActionList ret;
    if (name.isEmpty()) {
        return ret;
    }

    QString _ns = cleanNamespace(ns);

    ActionList containers = d->getContainers(_ns);
    foreach (Action *action, containers) {
        if (action->name() == name) {
            ret.prepend(action);
        }
    }

    return ret;
}

QHash<QString, Controller *> Dispatcher::controllers() const
{
    Q_D(const Dispatcher);
    return d->constrollerHash;
}

QString Dispatcher::uriForAction(Action *action, const QStringList &captures)
{
    Q_D(const Dispatcher);
    foreach (DispatchType *dispatch, d->dispatchers) {
        QString uri = dispatch->uriForAction(action, captures);
        if (!uri.isNull()) {
            return uri.isEmpty() ? QLatin1String("/") : uri;
        }
    }
    return QString();
}

void Dispatcher::registerDispatchType(DispatchType *dispatchType)
{
    Q_D(Dispatcher);
    d->dispatchers.append(dispatchType);
}

void Dispatcher::printActions()
{
    Q_D(Dispatcher);

    QString buffer;
    QTextStream out(&buffer, QIODevice::WriteOnly);
    bool showInternalActions = false;

    out << "Loaded Private actions:" << endl;
    QString privateTitle("Private");
    QString classTitle("Class");
    QString methodTitle("Method");
    int privateLength = privateTitle.length();
    int classLength = classTitle.length();
    int actionLength = methodTitle.length();
    QMap<QString, Action*>::ConstIterator it = d->actionHash.constBegin();
    while (it != d->actionHash.constEnd()) {
        Action *action = it.value();
        QString path = it.key();
        if (!path.startsWith(QLatin1String("/"))) {
            path.prepend(QLatin1String("/"));
        }
        privateLength = qMax(privateLength, path.length());
        classLength = qMax(classLength, action->className().length());
        actionLength = qMax(actionLength, action->name().length());
        ++it;
    }

    out << "." << QString().fill(QLatin1Char('-'), privateLength).toUtf8().data()
        << "+" << QString().fill(QLatin1Char('-'), classLength).toUtf8().data()
        << "+" << QString().fill(QLatin1Char('-'), actionLength).toUtf8().data()
        << "." << endl;
    out << "|" << privateTitle.leftJustified(privateLength).toUtf8().data()
        << "|" << classTitle.leftJustified(classLength).toUtf8().data()
        << "|" << methodTitle.leftJustified(actionLength).toUtf8().data()
        << "|" << endl;
    out << "." << QString().fill(QLatin1Char('-'), privateLength).toUtf8().data()
        << "+" << QString().fill(QLatin1Char('-'), classLength).toUtf8().data()
        << "+" << QString().fill(QLatin1Char('-'), actionLength).toUtf8().data()
        << "." << endl;

    it = d->actionHash.constBegin();
    while (it != d->actionHash.constEnd()) {
        Action *action = it.value();
        if (showInternalActions || !action->name().startsWith(QLatin1Char('_'))) {
            QString path = it.key();
            if (!path.startsWith(QLatin1String("/"))) {
                path.prepend(QLatin1String("/"));
            }
            out << "|" << path.leftJustified(privateLength).toUtf8().data()
                << "|" << action->className().leftJustified(classLength).toUtf8().data()
                << "|" << action->name().leftJustified(actionLength).toUtf8().data()
                << "|" << endl;
        }
        ++it;
    }

    out << "." << QString().fill(QLatin1Char('-'), privateLength).toUtf8().data()
        << "+" << QString().fill(QLatin1Char('-'), classLength).toUtf8().data()
        << "+" << QString().fill(QLatin1Char('-'), actionLength).toUtf8().data()
        << ".";
    qCDebug(CUTELYST_DISPATCHER) << buffer.toUtf8().data();

    // List all public actions
    foreach (DispatchType *dispatch, d->dispatchers) {
        dispatch->list();
    }
}

Action *Dispatcher::command2Action(Context *ctx, const QString &command, const QStringList &extraParams)
{
    Q_D(Dispatcher);
//    qDebug() << Q_FUNC_INFO << "Command" << command;

    Action *ret = d->actionHash.value(command);
    if (!ret) {
        ret = invokeAsPath(ctx, command, ctx->args());
    }

    return ret;
}

QStringList Dispatcher::unexcapedArgs(const QStringList &args)
{
    QStringList ret;
    foreach (const QString &arg, args) {
        ret << QUrl::fromPercentEncoding(arg.toLocal8Bit());
    }
    return ret;
}

QString Dispatcher::actionRel2Abs(Context *ctx, const QString &path)
{
    QString ret = path;
    if (!ret.startsWith(QLatin1Char('/'))) {
        // TODO at Catalyst it uses
        // c->stack->last()->namespace
        QString ns = ctx->action()->ns();
        ret = ns % QLatin1Char('/') % path;
    }

    if (ret.startsWith(QLatin1Char('/'))) {
        ret.remove(0, 1);
    }

    return ret;
}

Action *Dispatcher::invokeAsPath(Context *ctx, const QString &relativePath, const QStringList &args)
{
    Action *ret = 0;
    QString path = actionRel2Abs(ctx, relativePath);

    QRegularExpression re("^(?:(.*)/)?(\\w+)?$");
    while (!path.isEmpty()) {
        QRegularExpressionMatch match = re.match(path);
        if (match.hasMatch()) {
            path = match.captured(1);
            ret = getAction(match.captured(2), path);
            if (ret) {
                break;
            }
        } else {
            break;
        }
    }

    return ret;
}

QString Dispatcher::cleanNamespace(const QString &ns) const
{
    QStringList ret;
    foreach (const QString &part, ns.split(QLatin1Char('/'))) {
        if (!part.isEmpty()) {
            ret << part;
        }
    }
    return ret.join(QLatin1Char('/'));
}

ActionList DispatcherPrivate::getContainers(const QString &ns) const
{
    ActionList ret;

    QString _ns = ns;
    if (_ns == QLatin1String("/")) {
        _ns = QLatin1String("");
    }

    while (!_ns.isEmpty()) {
        ret << containerHash.value(_ns);
        _ns = _ns.section(QLatin1Char('/'), 0, -2);
    }
    ret << containerHash.value(_ns);

    return ret;
}
<|endoftext|>


<commit_before>#pragma once

namespace Channel9
{
	// Forward declare stuff to avoid loops in headers.
	union Value;
	class Environment;
	class CallableContext;
	struct String;
	struct Tuple;
	struct RunningContext;
	struct RunnableContext;
	class IStream;
	class Message;

	enum ValueType
	{
		POSITIVE_NUMBER = 0x00,

		BTRUE			= 0x10,
		FLOAT_NUM 		= 0x20,

		// these are types that will eventually
		// be made into full heap types, but for now
		// are allocated on the normal heap so need
		// to be treated as plain old values.
		CALLABLE_CONTEXT= 0x40,

		// Bit pattern of 0000 1000 indicates falsy,
		// all other type patterns are truthy.
		FALSY_MASK 		= 0xCF,
		FALSY_PATTERN 	= 0x80,

		NIL 			= 0x80,
		UNDEF 			= 0x90,
		BFALSE 			= 0xA0,
		// don't use 	= 0xB0,

		// All heap types have E as their high nibble,
		// as that nibble comes from the reference itself
		// and indicates that the full type is in the
		// next nibble of the address
		HEAP_TYPE 		= 0xE0,

		VARIABLE_FRAME  = 0xE2,
		STRING          = 0xE3,
		TUPLE           = 0xE4,
		MESSAGE         = 0xE5,
		RUNNABLE_CONTEXT= 0xE7,
		RUNNING_CONTEXT = 0xE8,

		NEGATIVE_NUMBER = 0xF0
	};
}

#include "trace.hpp"

#if defined(DEBUG)
#	define DO_DEBUG if (true)
#else
#	define DO_DEBUG if (false)
#endif

<commit_msg>Add a ptrdiff_t that isn't screwed up on osx.<commit_after>#pragma once
#include <stddef.h>
#include <stdint.h>

namespace Channel9
{
	// Forward declare stuff to avoid loops in headers.
	union Value;
	class Environment;
	class CallableContext;
	struct String;
	struct Tuple;
	struct RunningContext;
	struct RunnableContext;
	class IStream;
	class Message;

	enum ValueType
	{
		POSITIVE_NUMBER = 0x00,

		BTRUE			= 0x10,
		FLOAT_NUM 		= 0x20,

		// these are types that will eventually
		// be made into full heap types, but for now
		// are allocated on the normal heap so need
		// to be treated as plain old values.
		CALLABLE_CONTEXT= 0x40,

		// Bit pattern of 0000 1000 indicates falsy,
		// all other type patterns are truthy.
		FALSY_MASK 		= 0xCF,
		FALSY_PATTERN 	= 0x80,

		NIL 			= 0x80,
		UNDEF 			= 0x90,
		BFALSE 			= 0xA0,
		// don't use 	= 0xB0,

		// All heap types have E as their high nibble,
		// as that nibble comes from the reference itself
		// and indicates that the full type is in the
		// next nibble of the address
		HEAP_TYPE 		= 0xE0,

		VARIABLE_FRAME  = 0xE2,
		STRING          = 0xE3,
		TUPLE           = 0xE4,
		MESSAGE         = 0xE5,
		RUNNABLE_CONTEXT= 0xE7,
		RUNNING_CONTEXT = 0xE8,

		NEGATIVE_NUMBER = 0xF0
	};

	/* On some platforms (*cough* OSX/x64 *cough*), uintptr_t doesn't map
	 * to the same type as any of the uint*_t types (uint64 is long long and
	 * uintptr_t is long), and this makes function overloading just plain not
	 * work. To get around this, we redefine uintptr_t for the channel9
	 * namespace so it matches one of those types.
	 */
	template <size_t ptrsize_t>
	struct ptrsize;
	template <>
	struct ptrsize<4>
	{
		typedef uint32_t ptrtype_t;
	};
	template <>
	struct ptrsize<8>
	{
		typedef uint64_t ptrtype_t;
	};
	typedef ptrsize<sizeof(void*)>::ptrtype_t uintptr_t;
}

#include "trace.hpp"

#if defined(DEBUG)
#	define DO_DEBUG if (true)
#else
#	define DO_DEBUG if (false)
#endif

<|endoftext|>


<commit_before>/****************************************************************************
**
** Copyright (C) 2010 Nokia Corporation and/or its subsidiary(-ies).
** All rights reserved.
** Contact: Nokia Corporation (qt-info@nokia.com)
**
** This file is part of the Qt Mobility Components.
**
** $QT_BEGIN_LICENSE:LGPL$
** No Commercial Usage
** This file contains pre-release code and may not be distributed.
** You may use this file in accordance with the terms and conditions
** contained in the Technology Preview License Agreement accompanying
** this package.
**
** GNU Lesser General Public License Usage
** Alternatively, this file may be used under the terms of the GNU Lesser
** General Public License version 2.1 as published by the Free Software
** Foundation and appearing in the file LICENSE.LGPL included in the
** packaging of this file.  Please review the following information to
** ensure the GNU Lesser General Public License version 2.1 requirements
** will be met: http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
**
** In addition, as a special exception, Nokia gives you certain additional
** rights.  These rights are described in the Nokia Qt LGPL Exception
** version 1.1, included in the file LGPL_EXCEPTION.txt in this package.
**
** If you have questions regarding the use of this file, please contact
** Nokia at qt-info@nokia.com.
**
**
**
**
**
**
**
**
** $QT_END_LICENSE$
**
****************************************************************************/

#include "qbluetoothservicediscoveryagent_p.h"

#include <QUrl>

#include <arpa/inet.h>
#include <netinet/in.h>

QTM_BEGIN_NAMESPACE

QBluetoothServiceDiscoveryAgentPrivate::QBluetoothServiceDiscoveryAgentPrivate(const QBluetoothAddress &address)
    : error(QBluetoothServiceDiscoveryAgent::NoError)
    , state(Inactive)
    , deviceAddress(address)
    , deviceDiscoveryAgent(0)
    , mode(QBluetoothServiceDiscoveryAgent::MinimalDiscovery)
    , m_sdpAgent(NULL)
    , m_filter(NULL)
    , m_attributes(NULL)
{

}

QBluetoothServiceDiscoveryAgentPrivate::~QBluetoothServiceDiscoveryAgentPrivate()
{
    delete m_filter;
    delete m_attributes;
    delete m_sdpAgent;
}

void QBluetoothServiceDiscoveryAgentPrivate::start(const QBluetoothAddress &address)
{
    Q_Q(QBluetoothServiceDiscoveryAgent);
    TRAPD(err, startL(address));
    if (err != KErrNone) {
        error = QBluetoothServiceDiscoveryAgent::UnknownError;
        emit q->error(error);
    }
}

void QBluetoothServiceDiscoveryAgentPrivate::startL(const QBluetoothAddress &address)
{
    initL(address);

    if (uuidFilter.isEmpty()) {
        m_filter->AddL(QBluetoothUuid::PublicBrowseGroup);
    } else {
        foreach (const QBluetoothUuid &uuid, uuidFilter) {
            if (uuid.minimumSize() == 2) {
                TUUID sUuid(uuid.toUInt16());
                m_filter->AddL(sUuid);
            } else if (uuid.minimumSize() == 4) {
                TUUID sUuid(uuid.toUInt32());
                m_filter->AddL(sUuid);
            } else if (uuid.minimumSize() == 16) {
                TUint32 *dataPointer = (TUint32*)uuid.toUInt128().data;
                TUint32 lL = *(dataPointer++);
                TUint32 lH = *(dataPointer++);
                TUint32 hL = *(dataPointer++);
                TUint32 hH = *(dataPointer);
                TUUID sUuid(hH, hL, lH, lL);
                m_filter->AddL(sUuid);
            } else {
                // filter size can be 0 on error cases, searching all services
                m_filter->AddL(QBluetoothUuid::PublicBrowseGroup);
            }
        }
    }
    m_sdpAgent->SetRecordFilterL(*m_filter);
    m_sdpAgent->NextRecordRequestL();
}

void QBluetoothServiceDiscoveryAgentPrivate::stop()
{
    delete m_sdpAgent;
    m_sdpAgent = NULL;
    delete m_filter;
    m_filter = NULL;
    delete m_attributes;
    m_attributes = NULL;
}

void QBluetoothServiceDiscoveryAgentPrivate::initL(const QBluetoothAddress &address)
{
    TBTDevAddr btAddress(address.toUInt64());
    stop();

    //Trapped in Start
    m_sdpAgent = q_check_ptr(CSdpAgent::NewL(*this, btAddress));
    m_filter = q_check_ptr(CSdpSearchPattern::NewL());
    m_attributes = q_check_ptr(CSdpAttrIdMatchList::NewL());
    m_attributes->AddL(KAttrRangeAll);

}

bool QBluetoothServiceDiscoveryAgentPrivate::quickDiscovery(const QBluetoothAddress &address, const QBluetoothDeviceInfo &info)
{
    return false;
}

void QBluetoothServiceDiscoveryAgentPrivate::NextRecordRequestComplete(TInt aError, TSdpServRecordHandle aHandle, TInt aTotalRecordsCount)
{
    Q_Q(QBluetoothServiceDiscoveryAgent);
    if (aError == KErrNone && aTotalRecordsCount > 0 && m_sdpAgent && m_attributes) {
        // request attributes
        TRAPD(err, m_sdpAgent->AttributeRequestL(aHandle, *m_attributes));
        if (err) {
            error = QBluetoothServiceDiscoveryAgent::UnknownError;
            emit q->error(error);
        }
    } else if (aError == KErrEof) {
        _q_serviceDiscoveryFinished();
    } else {
        _q_serviceDiscoveryFinished();
    }
}

void QBluetoothServiceDiscoveryAgentPrivate::AttributeRequestResult(TSdpServRecordHandle, TSdpAttributeID aAttrID, CSdpAttrValue *aAttrValue)
{
    Q_Q(QBluetoothServiceDiscoveryAgent);
    m_currentAttributeId = aAttrID;
    TRAPD(err, aAttrValue->AcceptVisitorL(*this));
    if (m_stack.size() != 1) {
        error = QBluetoothServiceDiscoveryAgent::UnknownError;
        emit q->error(error);
        delete aAttrValue;
        return;
    }

    m_serviceInfo.setAttribute(aAttrID, m_stack.pop());

    if (err != KErrNone) {
        error = QBluetoothServiceDiscoveryAgent::UnknownError;
        emit q->error(error);
    }
    delete aAttrValue;
}

void QBluetoothServiceDiscoveryAgentPrivate::AttributeRequestComplete(TSdpServRecordHandle, TInt aError)
{
    Q_Q(QBluetoothServiceDiscoveryAgent);

    if (aError == KErrNone && m_sdpAgent) {
        m_serviceInfo.setDevice(discoveredDevices.at(0));
        discoveredServices.append(m_serviceInfo);
        m_serviceInfo = QBluetoothServiceInfo();
        TRAPD(err, m_sdpAgent->NextRecordRequestL());
        if (err != KErrNone) {
            error = QBluetoothServiceDiscoveryAgent::UnknownError;
            emit q->error(error);
        }
    } else if (aError != KErrEof) {
        error = QBluetoothServiceDiscoveryAgent::UnknownError;
        emit q->error(error);
    }

    // emit found service.
    if (discoveredServices.last().isValid())
        emit q->serviceDiscovered(discoveredServices.last());
}

void QBluetoothServiceDiscoveryAgentPrivate::VisitAttributeValueL(CSdpAttrValue &aValue, TSdpElementType aType)
{
    QVariant var;
    TUint datasize = aValue.DataSize();

    switch (aType) {
    case ETypeNil:
        break;
    case ETypeUint:
        if (datasize == 8) {
            TUint64 value;
            aValue.Uint64(value);
            var = QVariant::fromValue(value);
        } else
            var = QVariant::fromValue(aValue.Uint());
        break;
    case ETypeInt:
            var = QVariant::fromValue(aValue.Int());
        break;
    case ETypeUUID: {
        TPtrC8 shortForm(aValue.UUID().ShortestForm());
        if (shortForm.Size() == 2) {
            QBluetoothUuid uuid(ntohs(*reinterpret_cast<const quint16 *>(shortForm.Ptr())));
            var = QVariant::fromValue(uuid);
        } else if (shortForm.Size() == 4) {
            QBluetoothUuid uuid(ntohl(*reinterpret_cast<const quint32 *>(shortForm.Ptr())));
            var = QVariant::fromValue(uuid);
        } else if (shortForm.Size() == 16) {
            QBluetoothUuid uuid(*reinterpret_cast<const quint128 *>(shortForm.Ptr()));
            var = QVariant::fromValue(uuid);
        }
        break;
    }
    case ETypeString: {
        TPtrC8 stringBuffer = aValue.Des();
        var = QVariant::fromValue(QString::fromLocal8Bit(reinterpret_cast<const char *>(stringBuffer.Ptr()), stringBuffer.Size()));
        break;
    }
    case ETypeBoolean:
        var = QVariant::fromValue(static_cast<bool>(aValue.Bool()));
        break;
    case ETypeDES:
        m_stack.push(QVariant::fromValue(QBluetoothServiceInfo::Sequence()));
        break;
    case ETypeDEA:
        m_stack.push(QVariant::fromValue(QBluetoothServiceInfo::Alternative()));
        break;
    case ETypeURL: {
        TPtrC8 stringBuffer = aValue.Des();
        var = QVariant::fromValue(QUrl(QString::fromLocal8Bit(reinterpret_cast<const char *>(stringBuffer.Ptr()), stringBuffer.Size())));
        break;
    }
    case ETypeEncoded:
        qWarning() << "Don't know how to handle encoded type.";
        break;
    default:
        qWarning() << "Don't know how to handle type" << aType;
    }

    if (aType != ETypeDES && aType != ETypeDEA) {
        if (m_stack.size() == 0) {
            // single value attribute, just push onto stack
            m_stack.push(var);
        } else if (m_stack.size() >= 1) {
            // sequence or alternate attribute, add non-DES -DEA values to DES or DEA
            if (m_stack.top().canConvert<QBluetoothServiceInfo::Sequence>()) {
                QBluetoothServiceInfo::Sequence *sequence = static_cast<QBluetoothServiceInfo::Sequence *>(m_stack.top().data());
                sequence->append(var);
            } else if (m_stack.top().canConvert<QBluetoothServiceInfo::Alternative>()) {
                QBluetoothServiceInfo::Alternative *alternative = static_cast<QBluetoothServiceInfo::Alternative *>(m_stack.top().data());
                alternative->append(var);
            } else {
                qWarning("Unknown type in the QVariant, should be either a QBluetoothServiceInfo::Sequence or an QBluetoothServiceInfo::Alternative");
            }
        }
    }
}

void QBluetoothServiceDiscoveryAgentPrivate::StartListL(CSdpAttrValueList &)
{

}

void QBluetoothServiceDiscoveryAgentPrivate::EndListL()
{
    if (m_stack.size() > 1) {
        // finished a sequence or alternative add it to the parent sequence or alternative
        QVariant var = m_stack.pop();
        if (m_stack.top().canConvert<QBluetoothServiceInfo::Sequence>()) {
            QBluetoothServiceInfo::Sequence *sequence = static_cast<QBluetoothServiceInfo::Sequence *>(m_stack.top().data());
            sequence->append(var);
        } else if (m_stack.top().canConvert<QBluetoothServiceInfo::Alternative>()) {
            QBluetoothServiceInfo::Alternative *alternative = static_cast<QBluetoothServiceInfo::Alternative *>(m_stack.top().data());
            alternative->append(var);
        } else {
            qWarning("Unknown type in the QVariant, should be either a QBluetoothServiceInfo::Sequence or an QBluetoothServiceInfo::Alternative");
        }
    }
}

QTM_END_NAMESPACE
<commit_msg>Fix convertion of QBluetoothUuid to TUUID<commit_after>/****************************************************************************
**
** Copyright (C) 2010 Nokia Corporation and/or its subsidiary(-ies).
** All rights reserved.
** Contact: Nokia Corporation (qt-info@nokia.com)
**
** This file is part of the Qt Mobility Components.
**
** $QT_BEGIN_LICENSE:LGPL$
** No Commercial Usage
** This file contains pre-release code and may not be distributed.
** You may use this file in accordance with the terms and conditions
** contained in the Technology Preview License Agreement accompanying
** this package.
**
** GNU Lesser General Public License Usage
** Alternatively, this file may be used under the terms of the GNU Lesser
** General Public License version 2.1 as published by the Free Software
** Foundation and appearing in the file LICENSE.LGPL included in the
** packaging of this file.  Please review the following information to
** ensure the GNU Lesser General Public License version 2.1 requirements
** will be met: http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
**
** In addition, as a special exception, Nokia gives you certain additional
** rights.  These rights are described in the Nokia Qt LGPL Exception
** version 1.1, included in the file LGPL_EXCEPTION.txt in this package.
**
** If you have questions regarding the use of this file, please contact
** Nokia at qt-info@nokia.com.
**
**
**
**
**
**
**
**
** $QT_END_LICENSE$
**
****************************************************************************/

#include "qbluetoothservicediscoveryagent_p.h"

#include <QUrl>
#include <QtEndian>

#include <arpa/inet.h>
#include <netinet/in.h>

QTM_BEGIN_NAMESPACE

QBluetoothServiceDiscoveryAgentPrivate::QBluetoothServiceDiscoveryAgentPrivate(const QBluetoothAddress &address)
    : error(QBluetoothServiceDiscoveryAgent::NoError)
    , state(Inactive)
    , deviceAddress(address)
    , deviceDiscoveryAgent(0)
    , mode(QBluetoothServiceDiscoveryAgent::MinimalDiscovery)
    , m_sdpAgent(NULL)
    , m_filter(NULL)
    , m_attributes(NULL)
{

}

QBluetoothServiceDiscoveryAgentPrivate::~QBluetoothServiceDiscoveryAgentPrivate()
{
    delete m_filter;
    delete m_attributes;
    delete m_sdpAgent;
}

void QBluetoothServiceDiscoveryAgentPrivate::start(const QBluetoothAddress &address)
{
    Q_Q(QBluetoothServiceDiscoveryAgent);
    TRAPD(err, startL(address));
    if (err != KErrNone) {
        error = QBluetoothServiceDiscoveryAgent::UnknownError;
        emit q->error(error);
    }
}

void QBluetoothServiceDiscoveryAgentPrivate::startL(const QBluetoothAddress &address)
{
    initL(address);

    if (uuidFilter.isEmpty()) {
        m_filter->AddL(QBluetoothUuid::PublicBrowseGroup);
    } else {
        foreach (const QBluetoothUuid &uuid, uuidFilter) {
            if (uuid.minimumSize() == 2) {
                TUUID sUuid(uuid.toUInt16());
                m_filter->AddL(sUuid);
            } else if (uuid.minimumSize() == 4) {
                TUUID sUuid(uuid.toUInt32());
                m_filter->AddL(sUuid);
            } else if (uuid.minimumSize() == 16) {
                TUint32 *dataPointer = (TUint32*)uuid.toUInt128().data;
                TUint32 hH = qToBigEndian<quint32>(*(dataPointer++));
                TUint32 hL = qToBigEndian<quint32>(*(dataPointer++));
                TUint32 lH = qToBigEndian<quint32>(*(dataPointer++));
                TUint32 lL = qToBigEndian<quint32>(*(dataPointer));
                TUUID sUuid(hH, hL, lH, lL);
                m_filter->AddL(sUuid);
            } else {
                // filter size can be 0 on error cases, searching all services
                m_filter->AddL(QBluetoothUuid::PublicBrowseGroup);
            }
        }
    }
    m_sdpAgent->SetRecordFilterL(*m_filter);
    m_sdpAgent->NextRecordRequestL();
}

void QBluetoothServiceDiscoveryAgentPrivate::stop()
{
    delete m_sdpAgent;
    m_sdpAgent = NULL;
    delete m_filter;
    m_filter = NULL;
    delete m_attributes;
    m_attributes = NULL;
}

void QBluetoothServiceDiscoveryAgentPrivate::initL(const QBluetoothAddress &address)
{
    TBTDevAddr btAddress(address.toUInt64());
    stop();

    //Trapped in Start
    m_sdpAgent = q_check_ptr(CSdpAgent::NewL(*this, btAddress));
    m_filter = q_check_ptr(CSdpSearchPattern::NewL());
    m_attributes = q_check_ptr(CSdpAttrIdMatchList::NewL());
    m_attributes->AddL(KAttrRangeAll);

}

bool QBluetoothServiceDiscoveryAgentPrivate::quickDiscovery(const QBluetoothAddress &address, const QBluetoothDeviceInfo &info)
{
    return false;
}

void QBluetoothServiceDiscoveryAgentPrivate::NextRecordRequestComplete(TInt aError, TSdpServRecordHandle aHandle, TInt aTotalRecordsCount)
{
    Q_Q(QBluetoothServiceDiscoveryAgent);
    if (aError == KErrNone && aTotalRecordsCount > 0 && m_sdpAgent && m_attributes) {
        // request attributes
        TRAPD(err, m_sdpAgent->AttributeRequestL(aHandle, *m_attributes));
        if (err) {
            error = QBluetoothServiceDiscoveryAgent::UnknownError;
            emit q->error(error);
        }
    } else if (aError == KErrEof) {
        _q_serviceDiscoveryFinished();
    } else {
        _q_serviceDiscoveryFinished();
    }
}

void QBluetoothServiceDiscoveryAgentPrivate::AttributeRequestResult(TSdpServRecordHandle, TSdpAttributeID aAttrID, CSdpAttrValue *aAttrValue)
{
    Q_Q(QBluetoothServiceDiscoveryAgent);
    m_currentAttributeId = aAttrID;
    TRAPD(err, aAttrValue->AcceptVisitorL(*this));
    if (m_stack.size() != 1) {
        error = QBluetoothServiceDiscoveryAgent::UnknownError;
        emit q->error(error);
        delete aAttrValue;
        return;
    }

    m_serviceInfo.setAttribute(aAttrID, m_stack.pop());

    if (err != KErrNone) {
        error = QBluetoothServiceDiscoveryAgent::UnknownError;
        emit q->error(error);
    }
    delete aAttrValue;
}

void QBluetoothServiceDiscoveryAgentPrivate::AttributeRequestComplete(TSdpServRecordHandle, TInt aError)
{
    Q_Q(QBluetoothServiceDiscoveryAgent);

    if (aError == KErrNone && m_sdpAgent) {
        m_serviceInfo.setDevice(discoveredDevices.at(0));
        discoveredServices.append(m_serviceInfo);
        m_serviceInfo = QBluetoothServiceInfo();
        TRAPD(err, m_sdpAgent->NextRecordRequestL());
        if (err != KErrNone) {
            error = QBluetoothServiceDiscoveryAgent::UnknownError;
            emit q->error(error);
        }
    } else if (aError != KErrEof) {
        error = QBluetoothServiceDiscoveryAgent::UnknownError;
        emit q->error(error);
    }

    // emit found service.
    if (discoveredServices.last().isValid())
        emit q->serviceDiscovered(discoveredServices.last());
}

void QBluetoothServiceDiscoveryAgentPrivate::VisitAttributeValueL(CSdpAttrValue &aValue, TSdpElementType aType)
{
    QVariant var;
    TUint datasize = aValue.DataSize();

    switch (aType) {
    case ETypeNil:
        break;
    case ETypeUint:
        if (datasize == 8) {
            TUint64 value;
            aValue.Uint64(value);
            var = QVariant::fromValue(value);
        } else
            var = QVariant::fromValue(aValue.Uint());
        break;
    case ETypeInt:
            var = QVariant::fromValue(aValue.Int());
        break;
    case ETypeUUID: {
        TPtrC8 shortForm(aValue.UUID().ShortestForm());
        if (shortForm.Size() == 2) {
            QBluetoothUuid uuid(ntohs(*reinterpret_cast<const quint16 *>(shortForm.Ptr())));
            var = QVariant::fromValue(uuid);
        } else if (shortForm.Size() == 4) {
            QBluetoothUuid uuid(ntohl(*reinterpret_cast<const quint32 *>(shortForm.Ptr())));
            var = QVariant::fromValue(uuid);
        } else if (shortForm.Size() == 16) {
            QBluetoothUuid uuid(*reinterpret_cast<const quint128 *>(shortForm.Ptr()));
            var = QVariant::fromValue(uuid);
        }
        break;
    }
    case ETypeString: {
        TPtrC8 stringBuffer = aValue.Des();
        var = QVariant::fromValue(QString::fromLocal8Bit(reinterpret_cast<const char *>(stringBuffer.Ptr()), stringBuffer.Size()));
        break;
    }
    case ETypeBoolean:
        var = QVariant::fromValue(static_cast<bool>(aValue.Bool()));
        break;
    case ETypeDES:
        m_stack.push(QVariant::fromValue(QBluetoothServiceInfo::Sequence()));
        break;
    case ETypeDEA:
        m_stack.push(QVariant::fromValue(QBluetoothServiceInfo::Alternative()));
        break;
    case ETypeURL: {
        TPtrC8 stringBuffer = aValue.Des();
        var = QVariant::fromValue(QUrl(QString::fromLocal8Bit(reinterpret_cast<const char *>(stringBuffer.Ptr()), stringBuffer.Size())));
        break;
    }
    case ETypeEncoded:
        qWarning() << "Don't know how to handle encoded type.";
        break;
    default:
        qWarning() << "Don't know how to handle type" << aType;
    }

    if (aType != ETypeDES && aType != ETypeDEA) {
        if (m_stack.size() == 0) {
            // single value attribute, just push onto stack
            m_stack.push(var);
        } else if (m_stack.size() >= 1) {
            // sequence or alternate attribute, add non-DES -DEA values to DES or DEA
            if (m_stack.top().canConvert<QBluetoothServiceInfo::Sequence>()) {
                QBluetoothServiceInfo::Sequence *sequence = static_cast<QBluetoothServiceInfo::Sequence *>(m_stack.top().data());
                sequence->append(var);
            } else if (m_stack.top().canConvert<QBluetoothServiceInfo::Alternative>()) {
                QBluetoothServiceInfo::Alternative *alternative = static_cast<QBluetoothServiceInfo::Alternative *>(m_stack.top().data());
                alternative->append(var);
            } else {
                qWarning("Unknown type in the QVariant, should be either a QBluetoothServiceInfo::Sequence or an QBluetoothServiceInfo::Alternative");
            }
        }
    }
}

void QBluetoothServiceDiscoveryAgentPrivate::StartListL(CSdpAttrValueList &)
{

}

void QBluetoothServiceDiscoveryAgentPrivate::EndListL()
{
    if (m_stack.size() > 1) {
        // finished a sequence or alternative add it to the parent sequence or alternative
        QVariant var = m_stack.pop();
        if (m_stack.top().canConvert<QBluetoothServiceInfo::Sequence>()) {
            QBluetoothServiceInfo::Sequence *sequence = static_cast<QBluetoothServiceInfo::Sequence *>(m_stack.top().data());
            sequence->append(var);
        } else if (m_stack.top().canConvert<QBluetoothServiceInfo::Alternative>()) {
            QBluetoothServiceInfo::Alternative *alternative = static_cast<QBluetoothServiceInfo::Alternative *>(m_stack.top().data());
            alternative->append(var);
        } else {
            qWarning("Unknown type in the QVariant, should be either a QBluetoothServiceInfo::Sequence or an QBluetoothServiceInfo::Alternative");
        }
    }
}

QTM_END_NAMESPACE
<|endoftext|>


<commit_before>// =============================================================================
// PROJECT CHRONO - http://projectchrono.org
//
// Copyright (c) 2014 projectchrono.org
// All right reserved.
//
// Use of this source code is governed by a BSD-style license that can be found
// in the LICENSE file at the top level of the distribution and at
// http://projectchrono.org/license-chrono.txt.
//
// =============================================================================
// Author: Radu Serban
// =============================================================================
//
// Chrono::Vehicle + ChronoParallel demo program for simulating a HMMWV vehicle
// over rigid or granular material.
//
// The vehicle model uses the utility class ChWheeledVehicleAssembly and is
// based on JSON specification files from the Chrono data directory.
//
// Contact uses the DEM-P (penalty) formulation.
//
// The global reference frame has Z up.
// All units SI.
// =============================================================================

#include <stdio.h>
#include <cmath>
#include <vector>

#include "chrono/core/ChFileutils.h"
#include "chrono/utils/ChUtilsInputOutput.h"

#include "chrono_vehicle/ChVehicleModelData.h"
#include "chrono_vehicle/ChDriver.h"
#include "chrono_vehicle/terrain/DeformableTerrain.h"
#include "chrono_vehicle/terrain/RigidTerrain.h"
#include "chrono_vehicle/wheeled_vehicle/utils/ChWheeledVehicleAssembly.h"
#include "chrono_vehicle/wheeled_vehicle/utils/ChWheeledVehicleIrrApp.h"

#include "chrono_models/vehicle/hmmwv/HMMWV.h"

using namespace chrono;
using namespace chrono::collision;
using namespace chrono::irrlicht;
using namespace chrono::vehicle;
using namespace chrono::vehicle::hmmwv;

using std::cout;
using std::endl;

// =============================================================================
// USER SETTINGS
// =============================================================================

// -----------------------------------------------------------------------------
// Terrain parameters
// -----------------------------------------------------------------------------

// Dimensions
double terrainHeight = 0;
double terrainLength = 16.0;  // size in X direction
double terrainWidth = 8.0;    // size in Y direction

// Divisions (X and Y)
int divLength = 1024;
int divWidth = 512;

// -----------------------------------------------------------------------------
// Vehicle parameters
// -----------------------------------------------------------------------------

// Type of wheel/tire (controls both contact and visualization)
enum WheelType { CYLINDRICAL, LUGGED };
WheelType wheel_type = LUGGED;

// Type of terrain
enum TerrainType { DEFORMABLE_SOIL, RIGID_SOIL };
TerrainType terrain_type = DEFORMABLE_SOIL;

// Type of powertrain model (SHAFTS, SIMPLE)
PowertrainModelType powertrain_model = PowertrainModelType::SHAFTS;

// Drive type (FWD, RWD, or AWD)
DrivelineType drive_type = DrivelineType::AWD;

// Chassis visualization (MESH, PRIMITIVES, NONE)
VisualizationType chassis_vis = VisualizationType::NONE;

// Initial vehicle position and orientation
ChVector<> initLoc(-5, -2, 0.6);
ChQuaternion<> initRot(1, 0, 0, 0);

// Contact material properties
float Y_t = 1.0e6f;
float cr_t = 0.1f;
float mu_t = 0.8f;

// -----------------------------------------------------------------------------
// Simulation parameters
// -----------------------------------------------------------------------------

// Simulation step size
double step_size = 1e-3;

// Time interval between two render frames (1/FPS)
double render_step_size = 1.0 / 100;

// Point on chassis tracked by the camera
ChVector<> trackPoint(0.0, 0.0, 1.75);

// Output directories
const std::string out_dir = "../HMMWV_DEF_SOIL";
const std::string img_dir = out_dir + "/IMG";

// Visualization output
bool img_output = false;

// =============================================================================

class MyDriver : public ChDriver {
  public:
    MyDriver(ChVehicle& vehicle, double delay) : ChDriver(vehicle), m_delay(delay) {}
    ~MyDriver() {}

    virtual void Synchronize(double time) override {
        m_throttle = 0;
        m_steering = 0;
        m_braking = 0;

        double eff_time = time - m_delay;

        // Do not generate any driver inputs for a duration equal to m_delay.
        if (eff_time < 0)
            return;

        if (eff_time > 0.2)
            m_throttle = 0.7;
        else
            m_throttle = 3.5 * eff_time;

        if (eff_time < 2)
            m_steering = 0;
        else
            m_steering = 0.6 * std::sin(CH_C_2PI * (eff_time - 2) / 6);
    }

  private:
    double m_delay;
};

// =============================================================================

void CreateLuggedGeometry(std::shared_ptr<ChBody> wheelBody) {
    std::string lugged_file("hmmwv/lugged_wheel_section.obj");
    geometry::ChTriangleMeshConnected lugged_mesh;
    ChConvexDecompositionHACDv2 lugged_convex;
    utils::LoadConvexMesh(vehicle::GetDataFile(lugged_file), lugged_mesh, lugged_convex);
    int num_hulls = lugged_convex.GetHullCount();

    ChCollisionModel* coll_model = wheelBody->GetCollisionModel();
    coll_model->ClearModel();

    // Assemble the tire contact from 15 segments, properly offset.
    // Each segment is further decomposed in convex hulls.
    for (int iseg = 0; iseg < 15; iseg++) {
        ChQuaternion<> rot = Q_from_AngAxis(iseg * 24 * CH_C_DEG_TO_RAD, VECT_Y);
        for (int ihull = 0; ihull < num_hulls; ihull++) {
            std::vector<ChVector<> > convexhull;
            lugged_convex.GetConvexHullResult(ihull, convexhull);
            coll_model->AddConvexHull(convexhull, VNULL, rot);
        }
    }

    // Add a cylinder to represent the wheel hub.
    coll_model->AddCylinder(0.223, 0.223, 0.126);
    coll_model->BuildModel();

    // Visualization
    geometry::ChTriangleMeshConnected trimesh;
    trimesh.LoadWavefrontMesh(vehicle::GetDataFile("hmmwv/lugged_wheel.obj"), false, false);

    auto trimesh_shape = std::make_shared<ChTriangleMeshShape>();
    trimesh_shape->SetMesh(trimesh);
    trimesh_shape->SetName("lugged_wheel");
    wheelBody->AddAsset(trimesh_shape);

    auto mcolor = std::make_shared<ChColorAsset>(0.3f, 0.3f, 0.3f);
    wheelBody->AddAsset(mcolor);
}

// =============================================================================

int main(int argc, char* argv[]) {
    // --------------------
    // Create HMMWV vehicle
    // --------------------
    HMMWV_Full my_hmmwv;
    my_hmmwv.SetContactMethod(ChMaterialSurfaceBase::DEM);
    my_hmmwv.SetChassisFixed(false);
    my_hmmwv.SetInitPosition(ChCoordsys<>(initLoc, initRot));
    my_hmmwv.SetPowertrainType(powertrain_model);
    my_hmmwv.SetDriveType(drive_type);
    my_hmmwv.SetTireType(TireModelType::RIGID);
    my_hmmwv.Initialize();

    VisualizationType wheel_vis = (wheel_type == CYLINDRICAL) ? VisualizationType::MESH : VisualizationType::NONE;
    my_hmmwv.SetChassisVisualizationType(chassis_vis);
    my_hmmwv.SetWheelVisualizationType(wheel_vis);

    ChSystem* system = my_hmmwv.GetSystem();

    // --------------------------------------------------------
    // Set wheel contact material.
    // If needed, modify wheel contact and visualization models
    // --------------------------------------------------------
    for (int i = 0; i < 4; i++) {
        auto wheelBody = my_hmmwv.GetVehicle().GetWheelBody(i);
        wheelBody->GetMaterialSurfaceDEM()->SetFriction(mu_t);
        wheelBody->GetMaterialSurfaceDEM()->SetYoungModulus(Y_t);
        wheelBody->GetMaterialSurfaceDEM()->SetRestitution(cr_t);

        CreateLuggedGeometry(wheelBody);
    }

    // --------------------
    // Create driver system
    // --------------------
    MyDriver driver(my_hmmwv.GetVehicle(), 0.5);
    driver.Initialize();

    // ------------------
    // Create the terrain
    // ------------------
    ChTerrain* terrain;

    switch (terrain_type) {
        case DEFORMABLE_SOIL: {
            DeformableTerrain* terrainD = new DeformableTerrain(system);
            terrainD->SetPlane(ChCoordsys<>(VNULL, Q_from_AngX(CH_C_PI_2)));
            terrainD->SetSoilParametersSCM(2e6,   // Bekker Kphi
                                           0,     // Bekker Kc
                                           1.1,   // Bekker n exponent
                                           0,     // Mohr cohesive limit (Pa)
                                           30,    // Mohr friction limit (degrees)
                                           0.01,  // Janosi shear coefficient (m)
                                           2e8    // Elastic stiffness (Pa/m), before plastic yeld
                                           );
            ////terrainD->SetBulldozingFlow(true);     // inflate soil at the border of the rut
            ////terrainD->SetBulldozingParameters(40,  // angle of friction for erosion of displaced material at the border
            ////                                1.6);  // displaced material vs. downward pressed material

            ////terrainD->SetTexture(vehicle::GetDataFile("terrain/textures/grass.jpg"), 80, 16);
            ////terrainD->SetPlotType(vehicle::DeformableTerrain::PLOT_PRESSURE_YELD, 0, 30000.2);
            terrainD->SetPlotType(vehicle::DeformableTerrain::PLOT_SINKAGE, 0, 0.1);

            terrainD->Initialize(terrainHeight, terrainLength, terrainWidth, divLength, divWidth);

            terrain = terrainD;

            break;
        }

        case RIGID_SOIL: {
            RigidTerrain* terrainR = new RigidTerrain(system);
            terrainR->SetContactFrictionCoefficient(0.9f);
            terrainR->SetContactRestitutionCoefficient(0.01f);
            terrainR->SetContactMaterialProperties(2e7f, 0.3f);
            terrainR->SetColor(ChColor(0.8f, 0.8f, 0.5f));
            terrainR->SetTexture(vehicle::GetDataFile("terrain/textures/tile4.jpg"), 200, 200);
            terrainR->Initialize(terrainHeight, terrainLength, terrainWidth);

            terrain = terrainR;

            break;
        }
    }

    // Complete system construction
    system->SetupInitial();

    // ---------------------------------------
    // Create the vehicle Irrlicht application
    // ---------------------------------------
    ChWheeledVehicleIrrApp app(&my_hmmwv.GetVehicle(), &my_hmmwv.GetPowertrain(), L"HMMWV Deformable Soil Demo");
    app.SetSkyBox();
    app.AddTypicalLights(irr::core::vector3df(30.f, -30.f, 100.f), irr::core::vector3df(30.f, 50.f, 100.f), 250, 130);
    app.SetChaseCamera(trackPoint, 6.0, 0.5);
    app.SetTimestep(step_size);
    app.AssetBindAll();
    app.AssetUpdateAll();

    // -----------------
    // Initialize output
    // -----------------
    if (ChFileutils::MakeDirectory(out_dir.c_str()) < 0) {
        std::cout << "Error creating directory " << out_dir << std::endl;
        return 1;
    }
    if (img_output) {
        if (ChFileutils::MakeDirectory(img_dir.c_str()) < 0) {
            std::cout << "Error creating directory " << img_dir << std::endl;
            return 1;
        }
    }

    // ---------------
    // Simulation loop
    // ---------------

    // Solver settings.
    ////system->SetSolverType(ChSystem::SOLVER_MINRES);
    system->SetMaxItersSolverSpeed(50);
    system->SetMaxItersSolverStab(50);
    ////system->SetTol(0);
    ////system->SetMaxPenetrationRecoverySpeed(1.5);
    ////system->SetMinBounceSpeed(2.0);
    ////system->SetSolverOverrelaxationParam(0.8);
    ////system->SetSolverSharpnessParam(1.0);

    // Number of simulation steps between two 3D view render frames
    int render_steps = (int)std::ceil(render_step_size / step_size);

    // Initialize simulation frame counter
    int step_number = 0;
    int render_frame = 0;
    ChRealtimeStepTimer realtime_timer;

    while (app.GetDevice()->run()) {
        double time = system->GetChTime();

        // Render scene
        if (step_number % render_steps == 0) {
            app.BeginScene(true, true, irr::video::SColor(255, 140, 161, 192));
            app.DrawAll();
            ChIrrTools::drawColorbar(0, 0.1, "Sinkage", app.GetDevice(), 30);
            app.EndScene();

            if (img_output && step_number >= 0) {
                char filename[100];
                sprintf(filename, "%s/img_%03d.jpg", img_dir.c_str(), render_frame + 1);
                app.WriteImageToFile(filename);
            }

            render_frame++;
        }

        double throttle_input = driver.GetThrottle();
        double steering_input = driver.GetSteering();
        double braking_input = driver.GetBraking();

        // Update modules
        driver.Synchronize(time);
        terrain->Synchronize(time);
        my_hmmwv.Synchronize(time, steering_input, braking_input, throttle_input, *terrain);
        app.Synchronize("", steering_input, throttle_input, braking_input);

        // Advance dynamics
        double step = realtime_timer.SuggestSimulationStep(step_size);
        system->DoStepDynamics(step);
        app.Advance(step);

        // Increment frame number
        step_number++;
    }

    // Cleanup
    delete terrain;

    return 0;
}
<commit_msg>Use adaptive mesh refinement for the HMMWVV + SCM demo<commit_after>// =============================================================================
// PROJECT CHRONO - http://projectchrono.org
//
// Copyright (c) 2014 projectchrono.org
// All right reserved.
//
// Use of this source code is governed by a BSD-style license that can be found
// in the LICENSE file at the top level of the distribution and at
// http://projectchrono.org/license-chrono.txt.
//
// =============================================================================
// Author: Radu Serban
// =============================================================================
//
// Chrono::Vehicle + ChronoParallel demo program for simulating a HMMWV vehicle
// over rigid or granular material.
//
// The vehicle model uses the utility class ChWheeledVehicleAssembly and is
// based on JSON specification files from the Chrono data directory.
//
// Contact uses the DEM-P (penalty) formulation.
//
// The global reference frame has Z up.
// All units SI.
// =============================================================================

#include <stdio.h>
#include <cmath>
#include <vector>

#include "chrono/core/ChFileutils.h"
#include "chrono/utils/ChUtilsInputOutput.h"

#include "chrono_vehicle/ChVehicleModelData.h"
#include "chrono_vehicle/ChDriver.h"
#include "chrono_vehicle/terrain/DeformableTerrain.h"
#include "chrono_vehicle/terrain/RigidTerrain.h"
#include "chrono_vehicle/wheeled_vehicle/utils/ChWheeledVehicleAssembly.h"
#include "chrono_vehicle/wheeled_vehicle/utils/ChWheeledVehicleIrrApp.h"

#include "chrono_models/vehicle/hmmwv/HMMWV.h"

using namespace chrono;
using namespace chrono::collision;
using namespace chrono::irrlicht;
using namespace chrono::vehicle;
using namespace chrono::vehicle::hmmwv;

using std::cout;
using std::endl;

// =============================================================================
// USER SETTINGS
// =============================================================================

// -----------------------------------------------------------------------------
// Terrain parameters
// -----------------------------------------------------------------------------

// Dimensions
double terrainHeight = 0;
double terrainLength = 16.0;  // size in X direction
double terrainWidth = 8.0;    // size in Y direction

// Divisions (X and Y)
int divLength = 128;//1024;
int divWidth = 64;//512;

// -----------------------------------------------------------------------------
// Vehicle parameters
// -----------------------------------------------------------------------------

// Type of wheel/tire (controls both contact and visualization)
enum WheelType { CYLINDRICAL, LUGGED };
WheelType wheel_type = LUGGED;

// Type of terrain
enum TerrainType { DEFORMABLE_SOIL, RIGID_SOIL };
TerrainType terrain_type = DEFORMABLE_SOIL;

// Type of powertrain model (SHAFTS, SIMPLE)
PowertrainModelType powertrain_model = PowertrainModelType::SHAFTS;

// Drive type (FWD, RWD, or AWD)
DrivelineType drive_type = DrivelineType::AWD;

// Chassis visualization (MESH, PRIMITIVES, NONE)
VisualizationType chassis_vis = VisualizationType::NONE;

// Initial vehicle position and orientation
ChVector<> initLoc(-5, -2, 0.6);
ChQuaternion<> initRot(1, 0, 0, 0);

// Contact material properties
float Y_t = 1.0e6f;
float cr_t = 0.1f;
float mu_t = 0.8f;

// -----------------------------------------------------------------------------
// Simulation parameters
// -----------------------------------------------------------------------------

// Simulation step size
double step_size = 1e-3;

// Time interval between two render frames (1/FPS)
double render_step_size = 1.0 / 100;

// Point on chassis tracked by the camera
ChVector<> trackPoint(0.0, 0.0, 1.75);

// Output directories
const std::string out_dir = "../HMMWV_DEF_SOIL";
const std::string img_dir = out_dir + "/IMG";

// Visualization output
bool img_output = false;

// =============================================================================

class MyDriver : public ChDriver {
  public:
    MyDriver(ChVehicle& vehicle, double delay) : ChDriver(vehicle), m_delay(delay) {}
    ~MyDriver() {}

    virtual void Synchronize(double time) override {
        m_throttle = 0;
        m_steering = 0;
        m_braking = 0;

        double eff_time = time - m_delay;

        // Do not generate any driver inputs for a duration equal to m_delay.
        if (eff_time < 0)
            return;

        if (eff_time > 0.2)
            m_throttle = 0.7;
        else
            m_throttle = 3.5 * eff_time;

        if (eff_time < 2)
            m_steering = 0;
        else
            m_steering = 0.6 * std::sin(CH_C_2PI * (eff_time - 2) / 6);
    }

  private:
    double m_delay;
};

// =============================================================================

void CreateLuggedGeometry(std::shared_ptr<ChBody> wheelBody) {
    std::string lugged_file("hmmwv/lugged_wheel_section.obj");
    geometry::ChTriangleMeshConnected lugged_mesh;
    ChConvexDecompositionHACDv2 lugged_convex;
    utils::LoadConvexMesh(vehicle::GetDataFile(lugged_file), lugged_mesh, lugged_convex);
    int num_hulls = lugged_convex.GetHullCount();

    ChCollisionModel* coll_model = wheelBody->GetCollisionModel();
    coll_model->ClearModel();

    // Assemble the tire contact from 15 segments, properly offset.
    // Each segment is further decomposed in convex hulls.
    for (int iseg = 0; iseg < 15; iseg++) {
        ChQuaternion<> rot = Q_from_AngAxis(iseg * 24 * CH_C_DEG_TO_RAD, VECT_Y);
        for (int ihull = 0; ihull < num_hulls; ihull++) {
            std::vector<ChVector<> > convexhull;
            lugged_convex.GetConvexHullResult(ihull, convexhull);
            coll_model->AddConvexHull(convexhull, VNULL, rot);
        }
    }

    // Add a cylinder to represent the wheel hub.
    coll_model->AddCylinder(0.223, 0.223, 0.126);
    coll_model->BuildModel();

    // Visualization
    geometry::ChTriangleMeshConnected trimesh;
    trimesh.LoadWavefrontMesh(vehicle::GetDataFile("hmmwv/lugged_wheel.obj"), false, false);

    auto trimesh_shape = std::make_shared<ChTriangleMeshShape>();
    trimesh_shape->SetMesh(trimesh);
    trimesh_shape->SetName("lugged_wheel");
    wheelBody->AddAsset(trimesh_shape);

    auto mcolor = std::make_shared<ChColorAsset>(0.3f, 0.3f, 0.3f);
    wheelBody->AddAsset(mcolor);
}

// =============================================================================

int main(int argc, char* argv[]) {
    // --------------------
    // Create HMMWV vehicle
    // --------------------
    HMMWV_Full my_hmmwv;
    my_hmmwv.SetContactMethod(ChMaterialSurfaceBase::DEM);
    my_hmmwv.SetChassisFixed(false);
    my_hmmwv.SetInitPosition(ChCoordsys<>(initLoc, initRot));
    my_hmmwv.SetPowertrainType(powertrain_model);
    my_hmmwv.SetDriveType(drive_type);
    my_hmmwv.SetTireType(TireModelType::RIGID);
    my_hmmwv.Initialize();

    VisualizationType wheel_vis = (wheel_type == CYLINDRICAL) ? VisualizationType::MESH : VisualizationType::NONE;
    my_hmmwv.SetChassisVisualizationType(chassis_vis);
    my_hmmwv.SetWheelVisualizationType(wheel_vis);

    ChSystem* system = my_hmmwv.GetSystem();

    // --------------------------------------------------------
    // Set wheel contact material.
    // If needed, modify wheel contact and visualization models
    // --------------------------------------------------------
    for (int i = 0; i < 4; i++) {
        auto wheelBody = my_hmmwv.GetVehicle().GetWheelBody(i);
        wheelBody->GetMaterialSurfaceDEM()->SetFriction(mu_t);
        wheelBody->GetMaterialSurfaceDEM()->SetYoungModulus(Y_t);
        wheelBody->GetMaterialSurfaceDEM()->SetRestitution(cr_t);

        CreateLuggedGeometry(wheelBody);
    }

    // --------------------
    // Create driver system
    // --------------------
    MyDriver driver(my_hmmwv.GetVehicle(), 0.5);
    driver.Initialize();

    // ------------------
    // Create the terrain
    // ------------------
    ChTerrain* terrain;

    switch (terrain_type) {
        case DEFORMABLE_SOIL: {
            DeformableTerrain* terrainD = new DeformableTerrain(system);
            terrainD->SetPlane(ChCoordsys<>(VNULL, Q_from_AngX(CH_C_PI_2)));
            terrainD->SetSoilParametersSCM(2e6,   // Bekker Kphi
                                           0,     // Bekker Kc
                                           1.1,   // Bekker n exponent
                                           0,     // Mohr cohesive limit (Pa)
                                           30,    // Mohr friction limit (degrees)
                                           0.01,  // Janosi shear coefficient (m)
                                           2e8    // Elastic stiffness (Pa/m), before plastic yeld
                                           );
            /*
            terrainD->SetBulldozingFlow(true);    // inflate soil at the border of the rut
            terrainD->SetBulldozingParameters(55, // angle of friction for erosion of displaced material at the border of the rut
                                            0.8, // displaced material vs downward pressed material.
                                            5,   // number of erosion refinements per timestep
                                            10); // number of concentric vertex selections subject to erosion
            */
            // Turn on the automatic level of detail refinement, so a coarse terrain mesh
            // is automatically improved by adding more points under the wheel contact patch:
            terrainD->SetAutomaticRefinement(true);
            terrainD->SetAutomaticRefinementResolution(0.04);

            ////terrainD->SetTexture(vehicle::GetDataFile("terrain/textures/grass.jpg"), 80, 16);
            ////terrainD->SetPlotType(vehicle::DeformableTerrain::PLOT_PRESSURE_YELD, 0, 30000.2);
            terrainD->SetPlotType(vehicle::DeformableTerrain::PLOT_SINKAGE, 0, 0.1);

            terrainD->Initialize(terrainHeight, terrainLength, terrainWidth, divLength, divWidth);

            terrain = terrainD;

            break;
        }

        case RIGID_SOIL: {
            RigidTerrain* terrainR = new RigidTerrain(system);
            terrainR->SetContactFrictionCoefficient(0.9f);
            terrainR->SetContactRestitutionCoefficient(0.01f);
            terrainR->SetContactMaterialProperties(2e7f, 0.3f);
            terrainR->SetColor(ChColor(0.8f, 0.8f, 0.5f));
            terrainR->SetTexture(vehicle::GetDataFile("terrain/textures/tile4.jpg"), 200, 200);
            terrainR->Initialize(terrainHeight, terrainLength, terrainWidth);

            terrain = terrainR;

            break;
        }
    }

    // Complete system construction
    system->SetupInitial();

    // ---------------------------------------
    // Create the vehicle Irrlicht application
    // ---------------------------------------
    ChWheeledVehicleIrrApp app(&my_hmmwv.GetVehicle(), &my_hmmwv.GetPowertrain(), L"HMMWV Deformable Soil Demo");
    app.SetSkyBox();
    app.AddTypicalLights(irr::core::vector3df(30.f, -30.f, 100.f), irr::core::vector3df(30.f, 50.f, 100.f), 250, 130);
    app.SetChaseCamera(trackPoint, 6.0, 0.5);
    app.SetTimestep(step_size);
    app.AssetBindAll();
    app.AssetUpdateAll();

    // -----------------
    // Initialize output
    // -----------------
    if (ChFileutils::MakeDirectory(out_dir.c_str()) < 0) {
        std::cout << "Error creating directory " << out_dir << std::endl;
        return 1;
    }
    if (img_output) {
        if (ChFileutils::MakeDirectory(img_dir.c_str()) < 0) {
            std::cout << "Error creating directory " << img_dir << std::endl;
            return 1;
        }
    }

    // ---------------
    // Simulation loop
    // ---------------

    // Solver settings.
    ////system->SetSolverType(ChSystem::SOLVER_MINRES);
    system->SetMaxItersSolverSpeed(50);
    system->SetMaxItersSolverStab(50);
    ////system->SetTol(0);
    ////system->SetMaxPenetrationRecoverySpeed(1.5);
    ////system->SetMinBounceSpeed(2.0);
    ////system->SetSolverOverrelaxationParam(0.8);
    ////system->SetSolverSharpnessParam(1.0);

    // Number of simulation steps between two 3D view render frames
    int render_steps = (int)std::ceil(render_step_size / step_size);

    // Initialize simulation frame counter
    int step_number = 0;
    int render_frame = 0;
    ChRealtimeStepTimer realtime_timer;

    while (app.GetDevice()->run()) {
        double time = system->GetChTime();

        // Render scene
        if (step_number % render_steps == 0) {
            app.BeginScene(true, true, irr::video::SColor(255, 140, 161, 192));
            app.DrawAll();
            ChIrrTools::drawColorbar(0, 0.1, "Sinkage", app.GetDevice(), 30);
            app.EndScene();

            if (img_output && step_number >= 0) {
                char filename[100];
                sprintf(filename, "%s/img_%03d.jpg", img_dir.c_str(), render_frame + 1);
                app.WriteImageToFile(filename);
            }

            render_frame++;
        }

        double throttle_input = driver.GetThrottle();
        double steering_input = driver.GetSteering();
        double braking_input = driver.GetBraking();

        // Update modules
        driver.Synchronize(time);
        terrain->Synchronize(time);
        my_hmmwv.Synchronize(time, steering_input, braking_input, throttle_input, *terrain);
        app.Synchronize("", steering_input, throttle_input, braking_input);

        // Advance dynamics
        double step = realtime_timer.SuggestSimulationStep(step_size);
        system->DoStepDynamics(step);
        app.Advance(step);

        // Increment frame number
        step_number++;
    }

    // Cleanup
    delete terrain;

    return 0;
}
<|endoftext|>


<commit_before>// $Id: Quaternion_test.C,v 1.6 2000/05/26 19:25:04 amoll Exp $
#include <BALL/CONCEPT/classTest.h>

///////////////////////////
#include <BALL/MATHS/matrix44.h>
#include <BALL/MATHS/vector3.h>
#include <BALL/MATHS/quaternion.h>
///////////////////////////

START_TEST(class_name, "$Id: Quaternion_test.C,v 1.6 2000/05/26 19:25:04 amoll Exp $")

/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////

using namespace BALL;

Vector3 const v = Vector3(1.0, 2.0, 3.0);
String filename;
using std::ofstream;
using std::ios;
PRECISION(1E-4)

CHECK(TQuaternion::BALL_CREATE(TQuaternion<T>))
	Quaternion q(v, 4.0);
	Quaternion* q_ptr = (Quaternion*)q.create(false, true);
	TEST_REAL_EQUAL(q_ptr->i, 0)
	TEST_REAL_EQUAL(q_ptr->j, 0)
	TEST_REAL_EQUAL(q_ptr->k, 0)
	TEST_REAL_EQUAL(q_ptr->angle, 1.0)
	delete q_ptr;
	q_ptr = (Quaternion*)q.create();
	TEST_REAL_EQUAL(q_ptr->i, 0.24302)
	TEST_REAL_EQUAL(q_ptr->j, 0.48604)
	TEST_REAL_EQUAL(q_ptr->k, 0.72906)
	TEST_REAL_EQUAL(q_ptr->angle, -0.416147)
	delete q_ptr;
RESULT

CHECK(TQuaternion();)
  Quaternion* p;
	p = new Quaternion();
	TEST_NOT_EQUAL(p, 0)
RESULT

CHECK(~TQuaternion();)
  Quaternion* p;
	p = new Quaternion();
	delete p;
RESULT

Quaternion q, q1, q2;
float i = 0.24302, j = 2 * i, k = 3 * i, angle = -0.416147;

CHECK(TQuaternion(const TVector3<T>& axis, const T &angle))
	q = Quaternion(v, 4.0);
	TEST_REAL_EQUAL(q.i, i)
	TEST_REAL_EQUAL(q.j, j)
	TEST_REAL_EQUAL(q.k, k)
	TEST_REAL_EQUAL(q.angle, angle)
RESULT

CHECK(TQuaternion(const TQuaternion& q, bool deep = true))
	q = Quaternion(v, 4.0);
	q1 = Quaternion(q);
	TEST_REAL_EQUAL(q1.i, i)
	TEST_REAL_EQUAL(q1.j, j)
	TEST_REAL_EQUAL(q1.k, k)
	TEST_REAL_EQUAL(q1.angle, angle)
RESULT

CHECK(TQuaternion(const T& x, const T& y, const T& z, const T& angle))
	q1 = Quaternion(1.0, 2.0, 3.0, 4.0);
	TEST_REAL_EQUAL(q1.i, i)
	TEST_REAL_EQUAL(q1.j, j)
	TEST_REAL_EQUAL(q1.k, k)
	TEST_REAL_EQUAL(q1.angle, angle)
RESULT

CHECK(TQuaternion::getAngle() const )
	TEST_REAL_EQUAL(q.getAngle(), (2.0 * atan2(sqrt(i * i + j * j + k * k), angle)) )
RESULT

CHECK(TQuaternion::getAxis())
	Vector3 v2 = Vector3(i, j, k), v3;
	v2.normalize();
	v3 = q.getAxis();
	TEST_EQUAL(v3, v2)
	v3 = Vector3();
	q1 = Quaternion(v3, 4);
	TEST_EXCEPTION(Exception::DivisionByZero, q1.getAxis())
RESULT

CHECK(TQuaternion::getRotationMatrix(TMatrix4x4<T>& m) const )
	Matrix4x4	m, m2;
	m.set((1.0 - 2.0 * (j * j + k * k)),
			 (2.0 * (i * j - k * angle)),
			 (2.0 * (k * i + j * angle)),
			 0,
			 (2.0 * (i * j + k * angle)),
			 (1.0 - 2.0 * (k * k + i * i)),
			 (2.0 * (j * k - i * angle)),
			 0,
			 (2.0 * (k * i - j * angle)),
			 (2.0 * (j * k + i * angle)),
			 (1.0 - 2.0 * (j * j + i * i)),
			 0,
			 0, 0, 0, 1);
	q.getRotationMatrix(m2);
	TEST_REAL_EQUAL(m2.m11, m.m11);
	TEST_REAL_EQUAL(m2.m12, m.m12);
	TEST_REAL_EQUAL(m2.m13, m.m13);
	TEST_REAL_EQUAL(m2.m14, m.m14);
	TEST_REAL_EQUAL(m2.m21, m.m21);
	TEST_REAL_EQUAL(m2.m22, m.m22);
	TEST_REAL_EQUAL(m2.m23, m.m23);
	TEST_REAL_EQUAL(m2.m24, m.m24);
	TEST_REAL_EQUAL(m2.m31, m.m31);
	TEST_REAL_EQUAL(m2.m32, m.m32);
	TEST_REAL_EQUAL(m2.m33, m.m33);
	TEST_REAL_EQUAL(m2.m34, m.m34);
	TEST_REAL_EQUAL(m2.m41, m.m41);
	TEST_REAL_EQUAL(m2.m42, m.m42);
	TEST_REAL_EQUAL(m2.m43, m.m43);
	TEST_REAL_EQUAL(m2.m44, m.m44);
RESULT

CHECK(TQuaternion::TQuaternion operator - () const )
	float tmp = 1 / ::sqrt(angle * angle + i * i + j * j + k * k);
	q2 = -q;
	q1 = Quaternion(-i * tmp, -j * tmp, -k * tmp,	angle * tmp);
	TEST_REAL_EQUAL(q2.i, q1.i)
	TEST_REAL_EQUAL(q2.j, q1.j)
	TEST_REAL_EQUAL(q2.k, q1.k)
	TEST_REAL_EQUAL(q2.angle, q1.angle)
	q1 = Quaternion();
	q1.angle = 0;
	q2 = Quaternion();
	TEST_EQUAL(-q1, q2);
RESULT

CHECK(TQuaternion::getInverse() const )
	q1 = -Quaternion(q);
	q2 = q.getInverse();
	TEST_EQUAL(q2, q1)
RESULT

CHECK(TQuaternion::getConjugate() const )
	q1 = q.getConjugate();
	TEST_REAL_EQUAL(q1.i, -i)
	TEST_REAL_EQUAL(q1.j, -j)
	TEST_REAL_EQUAL(q1.k, -k)
	TEST_REAL_EQUAL(q1.angle, angle)
RESULT

CHECK(TQuaternion::bool operator == (const TQuaternion& q) const )
	q1 = Quaternion(q);
	TEST_EQUAL(q1 == q, true)
	q1 = Quaternion(v, 4.1);
	TEST_EQUAL(q1 == q, false)
RESULT

CHECK(TQuaternion::TQuaternion& operator += (const TQuaternion& q))
  q1 = Quaternion();
	q1 += q;
	TEST_REAL_EQUAL(q1.i, i)
	TEST_REAL_EQUAL(q1.j, j)
	TEST_REAL_EQUAL(q1.k, k)
	TEST_REAL_EQUAL(q1.angle, angle)
RESULT

CHECK(TQuaternion::TQuaternion& operator -= (const TQuaternion& q))
	q1 = q2 = Quaternion();
  q1 += - q;
	q2 -= q;
	TEST_REAL_EQUAL(q1.i, q2.i)
	TEST_REAL_EQUAL(q1.j, q2.j)
	TEST_REAL_EQUAL(q1.k, q2.k)
	TEST_REAL_EQUAL(q1.angle, q2.angle)
RESULT

CHECK(TQuaternion::bool operator != (const TQuaternion& q) const )
	q1 = Quaternion(q);
	TEST_EQUAL(q1 != q, false)
	q1 = Quaternion(v, 4.1);
	TEST_EQUAL(q1 != q, true)
RESULT

CHECK(TQuaternion::set(const TVector3<T>& axis, const T& angle))
	q1 = Quaternion();
	q1.set(v, 4.0);
	TEST_EQUAL(q1, q)
RESULT

CHECK(TQuaternion::set(const TQuaternion<T>& q, bool /* deep */))
	q1 = Quaternion();
	q1.set(q);
	TEST_EQUAL(q1, q)
RESULT

CHECK(TQuaternion::set(const T& x, const T& y, const T& z, const T& phi))
	q1.set(1.0, 2.0, 3.0, 4.0);
 TEST_EQUAL(q1 == q, true)
RESULT

CHECK(TQuaternion::get(TQuaternion<T>& q, bool deep))
	q.get(q1);
	TEST_EQUAL(q1, q)
RESULT

CHECK(TQuaternion::operator = (const TQuaternion<T>& q))
	q1 = Quaternion();
	q1 = q;
	TEST_EQUAL(q1 == q, true)
RESULT

CHECK(TQuaternion::setIdentity())
	q1 = Quaternion();
  q2.setIdentity();
  TEST_EQUAL(q2 == q1, true)
RESULT

CHECK(TQuaternion operator + (const TQuaternion<T>& a, const TQuaternion<T>& b))
	q1 = Quaternion(q);
	q2 = q + q1;
	q1 += q;
  TEST_EQUAL(q2, q1)
RESULT

CHECK(TQuaternion operator - (const TQuaternion<T>& a, const TQuaternion<T>& b))
	q1 = Quaternion(q);
	q2 = q - q1;
	q1 -= q;
  TEST_EQUAL(q2, q1)
RESULT

NEW_TMP_FILE(filename)

CHECK(std::istream& operator >>(std::istream& s, TQuaternion<T>& q))
	std::ifstream instr("data/Quaternion_test2.txt");
	q1 = Quaternion();
	instr >> q1;
	instr.close();
	TEST_REAL_EQUAL(q1.i, i)
	TEST_REAL_EQUAL(q1.j, j)
	TEST_REAL_EQUAL(q1.k, k)
	TEST_REAL_EQUAL(q1.angle, angle)
RESULT

CHECK(std::ostream& operator << (std::ostream& s, const TQuaternion<T>& q))
	std::ofstream outstr(filename.c_str(), std::ios::out);
	outstr << q;
	outstr.close();
	TEST_FILE(filename.c_str(), "data/Quaternion_test3.txt", true)
RESULT

CHECK(TQuaternion::dump(std::ostream& s, Size depth) const )
  String filename;
	NEW_TMP_FILE(filename)
	std::ofstream outfile(filename.c_str(), ios::out);
	q.dump(outfile);
	outfile.close();
	TEST_FILE(filename.c_str(), "data/Quaternion_test.txt", true)
RESULT

/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////
END_TEST
<commit_msg>improved test:setIdentity<commit_after>// $Id: Quaternion_test.C,v 1.7 2000/06/27 23:29:00 amoll Exp $
#include <BALL/CONCEPT/classTest.h>

///////////////////////////
#include <BALL/MATHS/matrix44.h>
#include <BALL/MATHS/vector3.h>
#include <BALL/MATHS/quaternion.h>
///////////////////////////

START_TEST(class_name, "$Id: Quaternion_test.C,v 1.7 2000/06/27 23:29:00 amoll Exp $")

/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////

using namespace BALL;

Vector3 const v = Vector3(1.0, 2.0, 3.0);
String filename;
using std::ofstream;
using std::ios;
PRECISION(1E-4)

CHECK(TQuaternion::BALL_CREATE(TQuaternion<T>))
	Quaternion q(v, 4.0);
	Quaternion* q_ptr = (Quaternion*)q.create(false, true);
	TEST_REAL_EQUAL(q_ptr->i, 0)
	TEST_REAL_EQUAL(q_ptr->j, 0)
	TEST_REAL_EQUAL(q_ptr->k, 0)
	TEST_REAL_EQUAL(q_ptr->angle, 1.0)
	delete q_ptr;
	q_ptr = (Quaternion*)q.create();
	TEST_REAL_EQUAL(q_ptr->i, 0.24302)
	TEST_REAL_EQUAL(q_ptr->j, 0.48604)
	TEST_REAL_EQUAL(q_ptr->k, 0.72906)
	TEST_REAL_EQUAL(q_ptr->angle, -0.416147)
	delete q_ptr;
RESULT

CHECK(TQuaternion();)
  Quaternion* p;
	p = new Quaternion();
	TEST_NOT_EQUAL(p, 0)
RESULT

CHECK(~TQuaternion();)
  Quaternion* p;
	p = new Quaternion();
	delete p;
RESULT

Quaternion q, q1, q2;
float i = 0.24302, j = 2 * i, k = 3 * i, angle = -0.416147;

CHECK(TQuaternion(const TVector3<T>& axis, const T &angle))
	q = Quaternion(v, 4.0);
	TEST_REAL_EQUAL(q.i, i)
	TEST_REAL_EQUAL(q.j, j)
	TEST_REAL_EQUAL(q.k, k)
	TEST_REAL_EQUAL(q.angle, angle)
RESULT

CHECK(TQuaternion(const TQuaternion& q, bool deep = true))
	q = Quaternion(v, 4.0);
	q1 = Quaternion(q);
	TEST_REAL_EQUAL(q1.i, i)
	TEST_REAL_EQUAL(q1.j, j)
	TEST_REAL_EQUAL(q1.k, k)
	TEST_REAL_EQUAL(q1.angle, angle)
RESULT

CHECK(TQuaternion(const T& x, const T& y, const T& z, const T& angle))
	q1 = Quaternion(1.0, 2.0, 3.0, 4.0);
	TEST_REAL_EQUAL(q1.i, i)
	TEST_REAL_EQUAL(q1.j, j)
	TEST_REAL_EQUAL(q1.k, k)
	TEST_REAL_EQUAL(q1.angle, angle)
RESULT

CHECK(TQuaternion::getAngle() const )
	TEST_REAL_EQUAL(q.getAngle(), (2.0 * atan2(sqrt(i * i + j * j + k * k), angle)) )
RESULT

CHECK(TQuaternion::getAxis())
	Vector3 v2 = Vector3(i, j, k), v3;
	v2.normalize();
	v3 = q.getAxis();
	TEST_EQUAL(v3, v2)
	v3 = Vector3();
	q1 = Quaternion(v3, 4);
	TEST_EXCEPTION(Exception::DivisionByZero, q1.getAxis())
RESULT

CHECK(TQuaternion::getRotationMatrix(TMatrix4x4<T>& m) const )
	Matrix4x4	m, m2;
	m.set((1.0 - 2.0 * (j * j + k * k)),
			 (2.0 * (i * j - k * angle)),
			 (2.0 * (k * i + j * angle)),
			 0,
			 (2.0 * (i * j + k * angle)),
			 (1.0 - 2.0 * (k * k + i * i)),
			 (2.0 * (j * k - i * angle)),
			 0,
			 (2.0 * (k * i - j * angle)),
			 (2.0 * (j * k + i * angle)),
			 (1.0 - 2.0 * (j * j + i * i)),
			 0,
			 0, 0, 0, 1);
	q.getRotationMatrix(m2);
	TEST_REAL_EQUAL(m2.m11, m.m11);
	TEST_REAL_EQUAL(m2.m12, m.m12);
	TEST_REAL_EQUAL(m2.m13, m.m13);
	TEST_REAL_EQUAL(m2.m14, m.m14);
	TEST_REAL_EQUAL(m2.m21, m.m21);
	TEST_REAL_EQUAL(m2.m22, m.m22);
	TEST_REAL_EQUAL(m2.m23, m.m23);
	TEST_REAL_EQUAL(m2.m24, m.m24);
	TEST_REAL_EQUAL(m2.m31, m.m31);
	TEST_REAL_EQUAL(m2.m32, m.m32);
	TEST_REAL_EQUAL(m2.m33, m.m33);
	TEST_REAL_EQUAL(m2.m34, m.m34);
	TEST_REAL_EQUAL(m2.m41, m.m41);
	TEST_REAL_EQUAL(m2.m42, m.m42);
	TEST_REAL_EQUAL(m2.m43, m.m43);
	TEST_REAL_EQUAL(m2.m44, m.m44);
RESULT

CHECK(TQuaternion::TQuaternion operator - () const )
	float tmp = 1 / ::sqrt(angle * angle + i * i + j * j + k * k);
	q2 = -q;
	q1 = Quaternion(-i * tmp, -j * tmp, -k * tmp,	angle * tmp);
	TEST_REAL_EQUAL(q2.i, q1.i)
	TEST_REAL_EQUAL(q2.j, q1.j)
	TEST_REAL_EQUAL(q2.k, q1.k)
	TEST_REAL_EQUAL(q2.angle, q1.angle)
	q1 = Quaternion();
	q1.angle = 0;
	q2 = Quaternion();
	TEST_EQUAL(-q1, q2);
RESULT

CHECK(TQuaternion::getInverse() const )
	q1 = -Quaternion(q);
	q2 = q.getInverse();
	TEST_EQUAL(q2, q1)
RESULT

CHECK(TQuaternion::getConjugate() const )
	q1 = q.getConjugate();
	TEST_REAL_EQUAL(q1.i, -i)
	TEST_REAL_EQUAL(q1.j, -j)
	TEST_REAL_EQUAL(q1.k, -k)
	TEST_REAL_EQUAL(q1.angle, angle)
RESULT

CHECK(TQuaternion::bool operator == (const TQuaternion& q) const )
	q1 = Quaternion(q);
	TEST_EQUAL(q1 == q, true)
	q1 = Quaternion(v, 4.1);
	TEST_EQUAL(q1 == q, false)
RESULT

CHECK(TQuaternion::TQuaternion& operator += (const TQuaternion& q))
  q1 = Quaternion();
	q1 += q;
	TEST_REAL_EQUAL(q1.i, i)
	TEST_REAL_EQUAL(q1.j, j)
	TEST_REAL_EQUAL(q1.k, k)
	TEST_REAL_EQUAL(q1.angle, angle)
RESULT

CHECK(TQuaternion::TQuaternion& operator -= (const TQuaternion& q))
	q1 = q2 = Quaternion();
  q1 += - q;
	q2 -= q;
	TEST_REAL_EQUAL(q1.i, q2.i)
	TEST_REAL_EQUAL(q1.j, q2.j)
	TEST_REAL_EQUAL(q1.k, q2.k)
	TEST_REAL_EQUAL(q1.angle, q2.angle)
RESULT

CHECK(TQuaternion::bool operator != (const TQuaternion& q) const )
	q1 = Quaternion(q);
	TEST_EQUAL(q1 != q, false)
	q1 = Quaternion(v, 4.1);
	TEST_EQUAL(q1 != q, true)
RESULT

CHECK(TQuaternion::set(const TVector3<T>& axis, const T& angle))
	q1 = Quaternion();
	q1.set(v, 4.0);
	TEST_EQUAL(q1, q)
RESULT

CHECK(TQuaternion::set(const TQuaternion<T>& q, bool /* deep */))
	q1 = Quaternion();
	q1.set(q);
	TEST_EQUAL(q1, q)
RESULT

CHECK(TQuaternion::set(const T& x, const T& y, const T& z, const T& phi))
	q1.set(1.0, 2.0, 3.0, 4.0);
 TEST_EQUAL(q1 == q, true)
RESULT

CHECK(TQuaternion::get(TQuaternion<T>& q, bool deep))
	q.get(q1);
	TEST_EQUAL(q1, q)
RESULT

CHECK(TQuaternion::operator = (const TQuaternion<T>& q))
	q1 = Quaternion();
	q1 = q;
	TEST_EQUAL(q1 == q, true)
RESULT

CHECK(TQuaternion::setIdentity())
  q1.setIdentity();
  TEST_EQUAL(q1.i, 0)
  TEST_EQUAL(q1.j, 0)
  TEST_EQUAL(q1.k, 0)
  TEST_EQUAL(q1.angle, 1)
RESULT

CHECK(TQuaternion operator + (const TQuaternion<T>& a, const TQuaternion<T>& b))
	q1 = Quaternion(q);
	q2 = q + q1;
	q1 += q;
  TEST_EQUAL(q2, q1)
RESULT

CHECK(TQuaternion operator - (const TQuaternion<T>& a, const TQuaternion<T>& b))
	q1 = Quaternion(q);
	q2 = q - q1;
	q1 -= q;
  TEST_EQUAL(q2, q1)
RESULT

NEW_TMP_FILE(filename)

CHECK(std::istream& operator >>(std::istream& s, TQuaternion<T>& q))
	std::ifstream instr("data/Quaternion_test2.txt");
	q1 = Quaternion();
	instr >> q1;
	instr.close();
	TEST_REAL_EQUAL(q1.i, i)
	TEST_REAL_EQUAL(q1.j, j)
	TEST_REAL_EQUAL(q1.k, k)
	TEST_REAL_EQUAL(q1.angle, angle)
RESULT

CHECK(std::ostream& operator << (std::ostream& s, const TQuaternion<T>& q))
	std::ofstream outstr(filename.c_str(), std::ios::out);
	outstr << q;
	outstr.close();
	TEST_FILE(filename.c_str(), "data/Quaternion_test3.txt", true)
RESULT

CHECK(TQuaternion::dump(std::ostream& s, Size depth) const )
  String filename;
	NEW_TMP_FILE(filename)
	std::ofstream outfile(filename.c_str(), ios::out);
	q.dump(outfile);
	outfile.close();
	TEST_FILE(filename.c_str(), "data/Quaternion_test.txt", true)
RESULT

/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////
END_TEST
<|endoftext|>