<commit_before>/*-------------------------------------------------------------------------
* Vulkan Conformance Tests
* ------------------------
*
* Copyright (c) 2018 Advanced Micro Devices, Inc.
* Copyright (c) 2018 The Khronos Group Inc.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
*      http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*//*!
* \file
* \brief VK_KHR_driver_properties tests
*//*--------------------------------------------------------------------*/

#include "vktApiDriverPropertiesTests.hpp"
#include "vktTestGroupUtil.hpp"
#include "vktTestCaseUtil.hpp"
#include "vkQueryUtil.hpp"
#include "vkTypeUtil.hpp"

using namespace vk;

namespace vkt
{
namespace api
{
namespace
{

static const deUint32 knownDriverIds[] =
{
	// Specified in the Vulkan registry (vk.xml)
	1,	// author = "Advanced Micro Devices, Inc."   comment = "AMD proprietary driver"
	2,	// author = "Advanced Micro Devices, Inc."   comment = "AMD open-source driver"
	3,	// author = "Mesa open source project"       comment = "Mesa RADV driver"
	4,	// author = "NVIDIA Corporation"             comment = "NVIDIA proprietary driver"
	5,	// author = "Intel Corporation"              comment = "Intel proprietary Windows driver"
	6,	// author = "Intel Corporation"              comment = "Intel open-source Mesa driver"
	7,	// author = "Imagination Technologies"       comment = "Imagination proprietary driver"
	8,	// author = "Qualcomm Technologies, Inc."    comment = "Qualcomm proprietary driver"
	9,	// author = "Arm Limited"                    comment = "Arm proprietary driver"
};

static const VkConformanceVersionKHR knownConformanceVersions[] =
{
	makeConformanceVersionKHR(1, 1, 2, 1),
	makeConformanceVersionKHR(1, 1, 2, 0),
	makeConformanceVersionKHR(1, 1, 1, 3),
	makeConformanceVersionKHR(1, 1, 1, 2),
	makeConformanceVersionKHR(1, 1, 1, 1),
	makeConformanceVersionKHR(1, 1, 1, 0),
	makeConformanceVersionKHR(1, 1, 0, 3),
	makeConformanceVersionKHR(1, 0, 2, 6),
	makeConformanceVersionKHR(1, 0, 2, 5),
	makeConformanceVersionKHR(1, 0, 2, 4),
	makeConformanceVersionKHR(1, 0, 2, 3),
	makeConformanceVersionKHR(1, 0, 2, 2),
	makeConformanceVersionKHR(1, 0, 2, 1),
	makeConformanceVersionKHR(1, 0, 2, 0),
};

DE_INLINE bool isNullTerminated(const char* str, const deUint32 maxSize)
{
	return deStrnlen(str, maxSize) < maxSize;
}

DE_INLINE bool operator==(const VkConformanceVersionKHR& a, const VkConformanceVersionKHR& b)
{
	return ((a.major == b.major)		&&
			(a.minor == b.minor)		&&
			(a.subminor == b.subminor)	&&
			(a.patch == b.patch));
}

tcu::TestStatus testQueryProperties (Context& context)
{
	// Check extension support

	if (!isDeviceExtensionSupported(context.getUsedApiVersion(), context.getDeviceExtensions(), "VK_KHR_driver_properties"))
		TCU_THROW(NotSupportedError, "Unsupported extension: VK_KHR_driver_properties");

	// Query the driver properties

	const VkPhysicalDevice				physDevice			= context.getPhysicalDevice();
	const int							memsetPattern		= 0xaa;
	VkPhysicalDeviceProperties2			deviceProperties2;
	VkPhysicalDeviceDriverPropertiesKHR	deviceDriverProperties;

	deMemset(&deviceDriverProperties, memsetPattern, sizeof(deviceDriverProperties));
	deviceDriverProperties.sType = VK_STRUCTURE_TYPE_PHYSICAL_DEVICE_DRIVER_PROPERTIES_KHR;
	deviceDriverProperties.pNext = DE_NULL;

	deMemset(&deviceProperties2, memsetPattern, sizeof(deviceProperties2));
	deviceProperties2.sType = VK_STRUCTURE_TYPE_PHYSICAL_DEVICE_PROPERTIES_2;
	deviceProperties2.pNext = &deviceDriverProperties;

	context.getInstanceInterface().getPhysicalDeviceProperties2(physDevice, &deviceProperties2);

	// Verify the returned values

	bool match = false;

	for (const deUint32* pDriverId = knownDriverIds; (pDriverId != DE_ARRAY_END(knownDriverIds)) && !match; ++pDriverId)
	{
		if (deviceDriverProperties.driverID == *pDriverId)
		{
			match = true;

			if (!isNullTerminated(deviceDriverProperties.driverName, VK_MAX_DRIVER_NAME_SIZE_KHR))
				TCU_FAIL("Driver name is not a null-terminated string");

			if (deviceDriverProperties.driverName[0] == 0)
				TCU_FAIL("Driver name is empty");

			if (!isNullTerminated(deviceDriverProperties.driverInfo, VK_MAX_DRIVER_INFO_SIZE_KHR))
				TCU_FAIL("Driver info is not a null-terminated string");

			bool conformanceVersionMatch = false;

			for (const VkConformanceVersionKHR* pConformanceVersion  = knownConformanceVersions;
												pConformanceVersion != DE_ARRAY_END(knownConformanceVersions);
											  ++pConformanceVersion)
			{
				if (deviceDriverProperties.conformanceVersion == *pConformanceVersion)
				{
					conformanceVersionMatch = true;
					break;
				}
			}

			if (!conformanceVersionMatch)
				TCU_FAIL("Wrong driver conformance version");
		}
	}

	if (!match)
		TCU_FAIL("Driver ID did not match any known driver");

	return tcu::TestStatus::pass("Pass");
}

void createTestCases (tcu::TestCaseGroup* group)
{
	addFunctionCase(group, "properties", "Query VkPhysicalDeviceDriverPropertiesKHR and check its values", testQueryProperties);
}

} // anonymous

tcu::TestCaseGroup*	createDriverPropertiesTests(tcu::TestContext& testCtx)
{
	return createTestGroup(testCtx, "driver_properties", "VK_KHR_driver_properties tests", createTestCases);
}

} // api
} // vkt
<commit_msg>Add 1.1.2.2 to known conformance versions<commit_after>/*-------------------------------------------------------------------------
* Vulkan Conformance Tests
* ------------------------
*
* Copyright (c) 2018 Advanced Micro Devices, Inc.
* Copyright (c) 2018 The Khronos Group Inc.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
*      http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*//*!
* \file
* \brief VK_KHR_driver_properties tests
*//*--------------------------------------------------------------------*/

#include "vktApiDriverPropertiesTests.hpp"
#include "vktTestGroupUtil.hpp"
#include "vktTestCaseUtil.hpp"
#include "vkQueryUtil.hpp"
#include "vkTypeUtil.hpp"

using namespace vk;

namespace vkt
{
namespace api
{
namespace
{

static const deUint32 knownDriverIds[] =
{
	// Specified in the Vulkan registry (vk.xml)
	1,	// author = "Advanced Micro Devices, Inc."   comment = "AMD proprietary driver"
	2,	// author = "Advanced Micro Devices, Inc."   comment = "AMD open-source driver"
	3,	// author = "Mesa open source project"       comment = "Mesa RADV driver"
	4,	// author = "NVIDIA Corporation"             comment = "NVIDIA proprietary driver"
	5,	// author = "Intel Corporation"              comment = "Intel proprietary Windows driver"
	6,	// author = "Intel Corporation"              comment = "Intel open-source Mesa driver"
	7,	// author = "Imagination Technologies"       comment = "Imagination proprietary driver"
	8,	// author = "Qualcomm Technologies, Inc."    comment = "Qualcomm proprietary driver"
	9,	// author = "Arm Limited"                    comment = "Arm proprietary driver"
};

static const VkConformanceVersionKHR knownConformanceVersions[] =
{
	makeConformanceVersionKHR(1, 1, 2, 2),
	makeConformanceVersionKHR(1, 1, 2, 1),
	makeConformanceVersionKHR(1, 1, 2, 0),
	makeConformanceVersionKHR(1, 1, 1, 3),
	makeConformanceVersionKHR(1, 1, 1, 2),
	makeConformanceVersionKHR(1, 1, 1, 1),
	makeConformanceVersionKHR(1, 1, 1, 0),
	makeConformanceVersionKHR(1, 1, 0, 3),
	makeConformanceVersionKHR(1, 0, 2, 6),
	makeConformanceVersionKHR(1, 0, 2, 5),
	makeConformanceVersionKHR(1, 0, 2, 4),
	makeConformanceVersionKHR(1, 0, 2, 3),
	makeConformanceVersionKHR(1, 0, 2, 2),
	makeConformanceVersionKHR(1, 0, 2, 1),
	makeConformanceVersionKHR(1, 0, 2, 0),
};

DE_INLINE bool isNullTerminated(const char* str, const deUint32 maxSize)
{
	return deStrnlen(str, maxSize) < maxSize;
}

DE_INLINE bool operator==(const VkConformanceVersionKHR& a, const VkConformanceVersionKHR& b)
{
	return ((a.major == b.major)		&&
			(a.minor == b.minor)		&&
			(a.subminor == b.subminor)	&&
			(a.patch == b.patch));
}

tcu::TestStatus testQueryProperties (Context& context)
{
	// Check extension support

	if (!isDeviceExtensionSupported(context.getUsedApiVersion(), context.getDeviceExtensions(), "VK_KHR_driver_properties"))
		TCU_THROW(NotSupportedError, "Unsupported extension: VK_KHR_driver_properties");

	// Query the driver properties

	const VkPhysicalDevice				physDevice			= context.getPhysicalDevice();
	const int							memsetPattern		= 0xaa;
	VkPhysicalDeviceProperties2			deviceProperties2;
	VkPhysicalDeviceDriverPropertiesKHR	deviceDriverProperties;

	deMemset(&deviceDriverProperties, memsetPattern, sizeof(deviceDriverProperties));
	deviceDriverProperties.sType = VK_STRUCTURE_TYPE_PHYSICAL_DEVICE_DRIVER_PROPERTIES_KHR;
	deviceDriverProperties.pNext = DE_NULL;

	deMemset(&deviceProperties2, memsetPattern, sizeof(deviceProperties2));
	deviceProperties2.sType = VK_STRUCTURE_TYPE_PHYSICAL_DEVICE_PROPERTIES_2;
	deviceProperties2.pNext = &deviceDriverProperties;

	context.getInstanceInterface().getPhysicalDeviceProperties2(physDevice, &deviceProperties2);

	// Verify the returned values

	bool match = false;

	for (const deUint32* pDriverId = knownDriverIds; (pDriverId != DE_ARRAY_END(knownDriverIds)) && !match; ++pDriverId)
	{
		if (deviceDriverProperties.driverID == *pDriverId)
		{
			match = true;

			if (!isNullTerminated(deviceDriverProperties.driverName, VK_MAX_DRIVER_NAME_SIZE_KHR))
				TCU_FAIL("Driver name is not a null-terminated string");

			if (deviceDriverProperties.driverName[0] == 0)
				TCU_FAIL("Driver name is empty");

			if (!isNullTerminated(deviceDriverProperties.driverInfo, VK_MAX_DRIVER_INFO_SIZE_KHR))
				TCU_FAIL("Driver info is not a null-terminated string");

			bool conformanceVersionMatch = false;

			for (const VkConformanceVersionKHR* pConformanceVersion  = knownConformanceVersions;
												pConformanceVersion != DE_ARRAY_END(knownConformanceVersions);
											  ++pConformanceVersion)
			{
				if (deviceDriverProperties.conformanceVersion == *pConformanceVersion)
				{
					conformanceVersionMatch = true;
					break;
				}
			}

			if (!conformanceVersionMatch)
				TCU_FAIL("Wrong driver conformance version");
		}
	}

	if (!match)
		TCU_FAIL("Driver ID did not match any known driver");

	return tcu::TestStatus::pass("Pass");
}

void createTestCases (tcu::TestCaseGroup* group)
{
	addFunctionCase(group, "properties", "Query VkPhysicalDeviceDriverPropertiesKHR and check its values", testQueryProperties);
}

} // anonymous

tcu::TestCaseGroup*	createDriverPropertiesTests(tcu::TestContext& testCtx)
{
	return createTestGroup(testCtx, "driver_properties", "VK_KHR_driver_properties tests", createTestCases);
}

} // api
} // vkt
<|endoftext|>


<commit_before>/*
 * This file is part of TelepathyQt4
 *
 * Copyright (C) 2008 Collabora Ltd. <http://www.collabora.co.uk/>
 * Copyright (C) 2008 Nokia Corporation
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */

#include "cli-channel.h"

#include "_gen/cli-channel-body.hpp"
#include "_gen/cli-channel.moc.hpp"
#include "cli-channel.moc.hpp"

#include <QQueue>

#include "cli-dbus.h"
#include "constants.h"
#include "debug-internal.hpp"

namespace Telepathy
{
namespace Client
{

struct Channel::Private
{
    // Public object
    Channel& parent;

    // Optional interface proxies
    DBus::PropertiesInterface* properties;

    // Introspection
    Readiness readiness;
    QStringList interfaces;
    QQueue<void (Private::*)()> introspectQueue;

    // Introspected properties
    QString channelType;
    uint targetHandleType;
    uint targetHandle;

    Private(Channel& parent)
        : parent(parent)
    {
        debug() << "Creating new Channel";

        properties = 0;
        readiness = ReadinessJustCreated;

        debug() << "Connecting to Channel::Closed()";
        parent.connect(&parent,
                       SIGNAL(Closed()),
                       SLOT(onClosed()));

        introspectQueue.enqueue(&Private::introspectMain);
        continueIntrospection();
    }

    void introspectMain()
    {
        if (!properties) {
            properties = parent.propertiesInterface();
            Q_ASSERT(properties != 0);
        }

        debug() << "Calling Properties::GetAll(Channel)";
        QDBusPendingCallWatcher* watcher =
            new QDBusPendingCallWatcher(
                    properties->GetAll(TELEPATHY_INTERFACE_CHANNEL), &parent);
        parent.connect(watcher,
                       SIGNAL(finished(QDBusPendingCallWatcher*)),
                       SLOT(gotMainProperties(QDBusPendingCallWatcher*)));
    }

    void introspectMainFallbackChannelType()
    {
        debug() << "Calling Channel::GetChannelType()";
        QDBusPendingCallWatcher* watcher =
            new QDBusPendingCallWatcher(parent.GetChannelType(), &parent);
        parent.connect(watcher,
                       SIGNAL(finished(QDBusPendingCallWatcher*)),
                       SLOT(gotChannelType(QDBusPendingCallWatcher*)));
    }

    void introspectMainFallbackHandle()
    {
        debug() << "Calling Channel::GetHandle()";
        QDBusPendingCallWatcher* watcher =
            new QDBusPendingCallWatcher(parent.GetHandle(), &parent);
        parent.connect(watcher,
                       SIGNAL(finished(QDBusPendingCallWatcher*)),
                       SLOT(gotHandle(QDBusPendingCallWatcher*)));
    }

    void introspectMainFallbackInterfaces()
    {
        debug() << "Calling Channel::GetInterfaces()";
        QDBusPendingCallWatcher* watcher =
            new QDBusPendingCallWatcher(parent.GetInterfaces(), &parent);
        parent.connect(watcher,
                       SIGNAL(finished(QDBusPendingCallWatcher*)),
                       SLOT(gotInterfaces(QDBusPendingCallWatcher*)));
    }

    void continueIntrospection()
    {
        if (introspectQueue.isEmpty()) {
            if (readiness < ReadinessFull)
                changeReadiness(ReadinessFull);
        } else {
            (this->*introspectQueue.dequeue())();
        }
    }

    void nowHaveInterfaces()
    {
        debug() << "Channel has" << interfaces.size() << "optional interfaces:" << interfaces;

        for (QStringList::const_iterator i = interfaces.begin();
                                         i != interfaces.end();
                                         ++i) {
            // Enqueue introspection of any optional interfaces here
        }

        continueIntrospection();
    }

    void changeReadiness(Readiness newReadiness)
    {
        Q_ASSERT(newReadiness != readiness);

        switch (readiness) {
            case ReadinessJustCreated:
                break;
            case ReadinessFull:
                Q_ASSERT(newReadiness == ReadinessDead);
                break;
            case ReadinessDead:
            default:
                Q_ASSERT(false);
                break;
        }

        debug() << "Channel readiness changed from" << readiness << "to" << newReadiness;

        if (newReadiness == ReadinessFull) {
            debug() << "Channel fully ready";
            debug() << " Channel type" << channelType;
            debug() << " Target handle" << targetHandle;
            debug() << " Target handle type" << targetHandleType;
        } else {
            debug() << "R.I.P. Channel.";
        }

        readiness = newReadiness;
        emit parent.readinessChanged(newReadiness);
    }
};

Channel::Channel(const QString& serviceName,
                 const QString& objectPath,
                 QObject* parent)
    : ChannelInterface(serviceName, objectPath, parent),
      mPriv(new Private(*this))
{
}

Channel::Channel(const QDBusConnection& connection,
                 const QString& serviceName,
                 const QString& objectPath,
                 QObject* parent)
    : ChannelInterface(connection, serviceName, objectPath, parent),
      mPriv(new Private(*this))
{
}

Channel::~Channel()
{
    delete mPriv;
}

Channel::Readiness Channel::readiness() const
{
    return mPriv->readiness;
}

QStringList Channel::interfaces() const
{
    return mPriv->interfaces;
}

QString Channel::channelType() const
{
    return mPriv->channelType;
}

uint Channel::targetHandleType() const
{
    return mPriv->targetHandleType;
}

uint Channel::targetHandle() const
{
    return mPriv->targetHandle;
}

void Channel::gotMainProperties(QDBusPendingCallWatcher* watcher)
{
    QDBusPendingReply<QVariantMap> reply = *watcher;
    QVariantMap props;

    if (!reply.isError())
        props = reply.value();

    QList<bool> conditions;

    conditions << (props.size() >= 4);
    conditions << (props.contains("ChannelType") && !qdbus_cast<QString>(props["ChannelType"]).isEmpty());
    conditions << props.contains("Interfaces");
    conditions << props.contains("TargetHandle");
    conditions << props.contains("TargetHandleType");

    if (conditions.contains(false)) {
        if (reply.isError())
            warning().nospace() << "Properties::GetAll(Channel) failed with " << reply.error().name() << ": " << reply.error().message();
        else
            warning() << "Reply to Properties::GetAll(Channel) didn't contain the expected properties";

        warning() << "Assuming a pre-0.17.7-spec service, falling back to serial inspection";

        mPriv->introspectQueue.enqueue(&Private::introspectMainFallbackChannelType);
        mPriv->introspectQueue.enqueue(&Private::introspectMainFallbackHandle);
        mPriv->introspectQueue.enqueue(&Private::introspectMainFallbackInterfaces);

        mPriv->continueIntrospection();
        return;
    }

    debug() << "Got reply to Properties::GetAll(Channel)";
    mPriv->channelType = qdbus_cast<QString>(props["ChannelType"]);
    mPriv->interfaces = qdbus_cast<QStringList>(props["Interfaces"]);
    mPriv->targetHandle = qdbus_cast<uint>(props["TargetHandle"]);
    mPriv->targetHandleType = qdbus_cast<uint>(props["TargetHandleType"]);
    mPriv->nowHaveInterfaces();
}

void Channel::gotChannelType(QDBusPendingCallWatcher* watcher)
{
    QDBusPendingReply<QString> reply = *watcher;

    if (reply.isError()) {
        warning().nospace() << "Channel::GetChannelType() failed with " << reply.error().name() << ": " << reply.error().message() << ", Channel officially dead";
        if (mPriv->readiness != ReadinessDead)
            mPriv->changeReadiness(ReadinessDead);
        return;
    }

    debug() << "Got reply to fallback Channel::GetChannelType()";
    mPriv->channelType = reply.value();
    mPriv->continueIntrospection();
}

void Channel::gotHandle(QDBusPendingCallWatcher* watcher)
{
    QDBusPendingReply<uint, uint> reply = *watcher;

    if (reply.isError()) {
        warning().nospace() << "Channel::GetHandle() failed with " << reply.error().name() << ": " << reply.error().message() << ", Channel officially dead";
        if (mPriv->readiness != ReadinessDead)
            mPriv->changeReadiness(ReadinessDead);
        return;
    }

    debug() << "Got reply to fallback Channel::GetHandle()";
    mPriv->targetHandleType = reply.argumentAt<0>();
    mPriv->targetHandle = reply.argumentAt<1>();
    mPriv->continueIntrospection();
}

void Channel::gotInterfaces(QDBusPendingCallWatcher* watcher)
{
    QDBusPendingReply<QStringList> reply = *watcher;

    if (reply.isError()) {
        warning().nospace() << "Channel::GetInterfaces() failed with " << reply.error().name() << ": " << reply.error().message() << ", Channel officially dead";
        if (mPriv->readiness != ReadinessDead)
            mPriv->changeReadiness(ReadinessDead);
        return;
    }

    debug() << "Got reply to fallback Channel::GetInterfaces()";
    mPriv->interfaces = reply.value();
    mPriv->nowHaveInterfaces();
}

void Channel::onClosed()
{
    debug() << "Got Channel::Closed";

    if (mPriv->readiness != ReadinessDead)
        mPriv->changeReadiness(ReadinessDead);
}

}
}
<commit_msg>Will-review #1: Change the property checking to be a) more clear b) better scalable to new properties<commit_after>/*
 * This file is part of TelepathyQt4
 *
 * Copyright (C) 2008 Collabora Ltd. <http://www.collabora.co.uk/>
 * Copyright (C) 2008 Nokia Corporation
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */

#include "cli-channel.h"

#include "_gen/cli-channel-body.hpp"
#include "_gen/cli-channel.moc.hpp"
#include "cli-channel.moc.hpp"

#include <QQueue>

#include "cli-dbus.h"
#include "constants.h"
#include "debug-internal.hpp"

namespace Telepathy
{
namespace Client
{

struct Channel::Private
{
    // Public object
    Channel& parent;

    // Optional interface proxies
    DBus::PropertiesInterface* properties;

    // Introspection
    Readiness readiness;
    QStringList interfaces;
    QQueue<void (Private::*)()> introspectQueue;

    // Introspected properties
    QString channelType;
    uint targetHandleType;
    uint targetHandle;

    Private(Channel& parent)
        : parent(parent)
    {
        debug() << "Creating new Channel";

        properties = 0;
        readiness = ReadinessJustCreated;

        debug() << "Connecting to Channel::Closed()";
        parent.connect(&parent,
                       SIGNAL(Closed()),
                       SLOT(onClosed()));

        introspectQueue.enqueue(&Private::introspectMain);
        continueIntrospection();
    }

    void introspectMain()
    {
        if (!properties) {
            properties = parent.propertiesInterface();
            Q_ASSERT(properties != 0);
        }

        debug() << "Calling Properties::GetAll(Channel)";
        QDBusPendingCallWatcher* watcher =
            new QDBusPendingCallWatcher(
                    properties->GetAll(TELEPATHY_INTERFACE_CHANNEL), &parent);
        parent.connect(watcher,
                       SIGNAL(finished(QDBusPendingCallWatcher*)),
                       SLOT(gotMainProperties(QDBusPendingCallWatcher*)));
    }

    void introspectMainFallbackChannelType()
    {
        debug() << "Calling Channel::GetChannelType()";
        QDBusPendingCallWatcher* watcher =
            new QDBusPendingCallWatcher(parent.GetChannelType(), &parent);
        parent.connect(watcher,
                       SIGNAL(finished(QDBusPendingCallWatcher*)),
                       SLOT(gotChannelType(QDBusPendingCallWatcher*)));
    }

    void introspectMainFallbackHandle()
    {
        debug() << "Calling Channel::GetHandle()";
        QDBusPendingCallWatcher* watcher =
            new QDBusPendingCallWatcher(parent.GetHandle(), &parent);
        parent.connect(watcher,
                       SIGNAL(finished(QDBusPendingCallWatcher*)),
                       SLOT(gotHandle(QDBusPendingCallWatcher*)));
    }

    void introspectMainFallbackInterfaces()
    {
        debug() << "Calling Channel::GetInterfaces()";
        QDBusPendingCallWatcher* watcher =
            new QDBusPendingCallWatcher(parent.GetInterfaces(), &parent);
        parent.connect(watcher,
                       SIGNAL(finished(QDBusPendingCallWatcher*)),
                       SLOT(gotInterfaces(QDBusPendingCallWatcher*)));
    }

    void continueIntrospection()
    {
        if (introspectQueue.isEmpty()) {
            if (readiness < ReadinessFull)
                changeReadiness(ReadinessFull);
        } else {
            (this->*introspectQueue.dequeue())();
        }
    }

    void extract01777MainProps(const QVariantMap& props)
    {
        bool haveProps = props.size() >= 4
                      && props.contains("ChannelType") && !qdbus_cast<QString>(props["ChannelType"]).isEmpty()
                      && props.contains("Interfaces")
                      && props.contains("TargetHandle")
                      && props.contains("TargetHandleType");

        if (!haveProps) {
            warning() << "No properties expected from a post-0.17.7 spec service in reply to Properties::GetAll(Channel), falling back to serial inspection";

            introspectQueue.enqueue(&Private::introspectMainFallbackChannelType);
            introspectQueue.enqueue(&Private::introspectMainFallbackHandle);
            introspectQueue.enqueue(&Private::introspectMainFallbackInterfaces);
        } else {
            debug() << " Found properties specified in 0.17.7";

            channelType = qdbus_cast<QString>(props["ChannelType"]);
            interfaces = qdbus_cast<QStringList>(props["Interfaces"]);
            targetHandle = qdbus_cast<uint>(props["TargetHandle"]);
            targetHandleType = qdbus_cast<uint>(props["TargetHandleType"]);

            nowHaveInterfaces();
        }
    }

    void nowHaveInterfaces()
    {
        debug() << "Channel has" << interfaces.size() << "optional interfaces:" << interfaces;

        for (QStringList::const_iterator i = interfaces.begin();
                                         i != interfaces.end();
                                         ++i) {
            // Enqueue introspection of any optional interfaces here
        }
    }

    void changeReadiness(Readiness newReadiness)
    {
        Q_ASSERT(newReadiness != readiness);

        switch (readiness) {
            case ReadinessJustCreated:
                break;
            case ReadinessFull:
                Q_ASSERT(newReadiness == ReadinessDead);
                break;
            case ReadinessDead:
            default:
                Q_ASSERT(false);
                break;
        }

        debug() << "Channel readiness changed from" << readiness << "to" << newReadiness;

        if (newReadiness == ReadinessFull) {
            debug() << "Channel fully ready";
            debug() << " Channel type" << channelType;
            debug() << " Target handle" << targetHandle;
            debug() << " Target handle type" << targetHandleType;
        } else {
            debug() << "R.I.P. Channel.";
        }

        readiness = newReadiness;
        emit parent.readinessChanged(newReadiness);
    }
};

Channel::Channel(const QString& serviceName,
                 const QString& objectPath,
                 QObject* parent)
    : ChannelInterface(serviceName, objectPath, parent),
      mPriv(new Private(*this))
{
}

Channel::Channel(const QDBusConnection& connection,
                 const QString& serviceName,
                 const QString& objectPath,
                 QObject* parent)
    : ChannelInterface(connection, serviceName, objectPath, parent),
      mPriv(new Private(*this))
{
}

Channel::~Channel()
{
    delete mPriv;
}

Channel::Readiness Channel::readiness() const
{
    return mPriv->readiness;
}

QStringList Channel::interfaces() const
{
    return mPriv->interfaces;
}

QString Channel::channelType() const
{
    return mPriv->channelType;
}

uint Channel::targetHandleType() const
{
    return mPriv->targetHandleType;
}

uint Channel::targetHandle() const
{
    return mPriv->targetHandle;
}

void Channel::gotMainProperties(QDBusPendingCallWatcher* watcher)
{
    QDBusPendingReply<QVariantMap> reply = *watcher;
    QVariantMap props;

    if (!reply.isError()) {
        debug() << "Got reply to Properties::GetAll(Channel)";
        props = reply.value();
    } else {
        warning().nospace() << "Properties::GetAll(Channel) failed with " << reply.error().name() << ": " << reply.error().message();
    }

    mPriv->extract01777MainProps(props);
    // Add extraction (and possible fallbacks) in similar functions, called from here

    mPriv->continueIntrospection();
}

void Channel::gotChannelType(QDBusPendingCallWatcher* watcher)
{
    QDBusPendingReply<QString> reply = *watcher;

    if (reply.isError()) {
        warning().nospace() << "Channel::GetChannelType() failed with " << reply.error().name() << ": " << reply.error().message() << ", Channel officially dead";
        if (mPriv->readiness != ReadinessDead)
            mPriv->changeReadiness(ReadinessDead);
        return;
    }

    debug() << "Got reply to fallback Channel::GetChannelType()";
    mPriv->channelType = reply.value();
    mPriv->continueIntrospection();
}

void Channel::gotHandle(QDBusPendingCallWatcher* watcher)
{
    QDBusPendingReply<uint, uint> reply = *watcher;

    if (reply.isError()) {
        warning().nospace() << "Channel::GetHandle() failed with " << reply.error().name() << ": " << reply.error().message() << ", Channel officially dead";
        if (mPriv->readiness != ReadinessDead)
            mPriv->changeReadiness(ReadinessDead);
        return;
    }

    debug() << "Got reply to fallback Channel::GetHandle()";
    mPriv->targetHandleType = reply.argumentAt<0>();
    mPriv->targetHandle = reply.argumentAt<1>();
    mPriv->continueIntrospection();
}

void Channel::gotInterfaces(QDBusPendingCallWatcher* watcher)
{
    QDBusPendingReply<QStringList> reply = *watcher;

    if (reply.isError()) {
        warning().nospace() << "Channel::GetInterfaces() failed with " << reply.error().name() << ": " << reply.error().message() << ", Channel officially dead";
        if (mPriv->readiness != ReadinessDead)
            mPriv->changeReadiness(ReadinessDead);
        return;
    }

    debug() << "Got reply to fallback Channel::GetInterfaces()";
    mPriv->interfaces = reply.value();
    mPriv->nowHaveInterfaces();
    mPriv->continueIntrospection();
}

void Channel::onClosed()
{
    debug() << "Got Channel::Closed";

    if (mPriv->readiness != ReadinessDead)
        mPriv->changeReadiness(ReadinessDead);
}

}
}
<|endoftext|>


<commit_before>#include <cassert>
#include <iostream>

#include <util/PlatformUtils.hpp>
#include <util/Mutexes.hpp>

#include <PlatformSupport/DOMStringHelper.hpp>
#include <PlatformSupport/XalanFileOutputStream.hpp>
#include <PlatformSupport/XalanOutputStreamPrintWriter.hpp>

#include <XalanSourceTree/XalanSourceTreeDOMSupport.hpp>
#include <XalanSourceTree/XalanSourceTreeParserLiaison.hpp>

#include <XPath/XObjectFactoryDefault.hpp>
#include <XPath/XPathFactoryDefault.hpp>

#include <XSLT/StylesheetConstructionContextDefault.hpp>
#include <XSLT/StylesheetExecutionContextDefault.hpp>
#include <XSLT/StylesheetRoot.hpp>
#include <XSLT/XSLTEngineImpl.hpp>
#include <XSLT/XSLTInit.hpp>
#include <XSLT/XSLTInputSource.hpp>
#include <XSLT/XSLTProcessorEnvSupportDefault.hpp>
#include <XSLT/XSLTResultTarget.hpp>

#if defined(WIN32)
//This is here for the threads.
#include <process.h>

#define WIN32_LEAN_AND_MEAN
#include <windows.h>
#elif defined(XALAN_POSIX2_AVAILABLE)
#include <pthread.h>
#include <unistd.h>
#else
#error Unsupported platform!
#endif

#if !defined(XALAN_NO_NAMESPACES)
	using std::cerr;
	using std::cout;
	using std::endl;
#endif

// This is here for memory leak testing.
#if defined(_DEBUG)
#include <crtdbg.h>
#endif

// Used to hold compiled stylesheet
StylesheetRoot* glbStylesheetRoot;

class SynchronizedCounter
{
public:

	SynchronizedCounter();

	~SynchronizedCounter();

	void
	increment();

	void
	decrement();

	unsigned long
	getCounter() const;

private:

	mutable XMLMutex	m_mutex;

	unsigned long		m_counter;
};

SynchronizedCounter::SynchronizedCounter() :
	m_mutex(),
	m_counter(0)
{
}

SynchronizedCounter::~SynchronizedCounter()
{
}

void
SynchronizedCounter::increment()
{
	XMLMutexLock	theLock(&m_mutex);

	if (m_counter < ULONG_MAX)
	{
		++m_counter;
	}
}

void
SynchronizedCounter::decrement()
{
	XMLMutexLock	theLock(&m_mutex);

	if (m_counter > 0)
	{
		--m_counter;
	}
}

unsigned long
SynchronizedCounter::getCounter() const
{
	return m_counter;
}

struct
ThreadInfo
{
	ThreadInfo(
			unsigned int			theThreadNumber,
			SynchronizedCounter*	theCounter) :
		m_threadNumber(theThreadNumber),
		m_counter(theCounter)
	{
	}

	unsigned int			m_threadNumber;

	SynchronizedCounter*	m_counter;
};

#if defined(WIN32)

extern "C" void theThreadRoutine(void* param);

void
#elif defined(XALAN_POSIX2_AVAILABLE)

extern "C" void* theThreadRoutine(void* param);

void*
#else
#error Unsupported platform!
#endif
theThreadRoutine(void*		param)
{
// This routine uses compiled stylesheet (glbStylesheetRoot), which is set using the
// theProcessor.setStylesheetRoot method. The transform is done using the theProcessor's
// process() method.

	const ThreadInfo*	theInfo = reinterpret_cast<const ThreadInfo*>(param);

	assert(theInfo != 0);

	theInfo->m_counter->increment();

	try
	{
		// Create the support objects that are necessary for running the processor...
		XalanSourceTreeDOMSupport		theDOMSupport;
		XalanSourceTreeParserLiaison	theParserLiaison(theDOMSupport);

		theDOMSupport.setParserLiaison(&theParserLiaison);
		// The default is that documents are not thread-safe.  Set this to
		// true so they are.
		//theParserLiaison.setThreadSafe(true);

		XSLTProcessorEnvSupportDefault	theXSLTProcessorEnvSupport;
		XObjectFactoryDefault			theXObjectFactory;
		XPathFactoryDefault				theXPathFactory;

		// Create a processor...and output start message.
		XSLTEngineImpl	theProcessor(
						theParserLiaison,
						theXSLTProcessorEnvSupport,
						theDOMSupport,
						theXObjectFactory,
						theXPathFactory);

		// Connect the processor to the support object...
		theXSLTProcessorEnvSupport.setProcessor(&theProcessor);

		// The execution context uses the same factory support objects as
		// the processor, since those objects have the same lifetime as
		// other objects created as a result of the execution.
		StylesheetExecutionContextDefault	ssExecutionContext(
							theProcessor,
							theXSLTProcessorEnvSupport,
							theDOMSupport,
							theXObjectFactory);

		// Our input files.  The assumption is that the executable will be run
		// from same directory as the input files.

		// Generate the input and output file names.
		const XalanDOMString	theXMLfile("birds.xml");

		const XalanDOMString	theOutputFile(
				XalanDOMString("birds") +
				UnsignedLongToDOMString(theInfo->m_threadNumber) +
				XalanDOMString(".out"));

		//Generate the XML input and output objects.
		XSLTInputSource		theInputSource(c_wstr(theXMLfile));
		XSLTResultTarget	theResultTarget(theOutputFile);

		// Set the stylesheet to be the compiled stylesheet. Then do the transform.
		// Report both the start of the transform and end of the thread.
		theProcessor.setStylesheetRoot(glbStylesheetRoot);
		theProcessor.process(theInputSource,theResultTarget,ssExecutionContext);
	}
	catch(...)
	{
	}

	// Decrement the counter because we're done...
	theInfo->m_counter->decrement();

#if defined(XALAN_POSIX2_AVAILABLE)
	return 0;
#endif
}

inline void
doSleep(unsigned int	theMilliseconds)
{
#if defined(WIN32)
	Sleep(theMilliseconds);
#elif defined(XALAN_POSIX2_AVAILABLE)
	usleep(theMilliseconds * 10);
#else
#error Unsupported platform!
#endif
}

void
doThreads(long	theThreadCount)
{
	cout << endl << "Starting " << theThreadCount << " threads." << endl;

	typedef std::vector<ThreadInfo>		ThreadInfoVectorType;

	ThreadInfoVectorType	theThreadInfo;

	theThreadInfo.reserve(theThreadCount);

	try
	{
		cout << endl << "Clock before starting threads: " << clock() << endl;

		SynchronizedCounter		theCounter;

		for (long i = 0; i < theThreadCount; i++)
		{
			theThreadInfo.push_back(ThreadInfoVectorType::value_type(i, &theCounter));

#if defined(WIN32)

			const unsigned long		theThreadID =
					_beginthread(theThreadRoutine, 4096, reinterpret_cast<LPVOID>(&theThreadInfo.back()));

			if (theThreadID == unsigned(-1))
			{
				cerr << endl << "Unable to create thread number " << i + 1 << "." << endl;
			}

#elif defined(XALAN_POSIX2_AVAILABLE)

			pthread_t	theThread;

			const int	theResult = pthread_create(&theThread, 0, theThreadRoutine, (void*)&theThreadInfo.back());

			if (theResult != 0)
			{
				cerr << endl << "Unable to create thread number " << i + 1 << "." << endl;
			}
			else
			{
				pthread_detach(theThread);
			}
#else
#error Unsupported platform!
#endif
		}

		clock_t		theClock = 0;

		if (theThreadInfo.size() == 0)
		{
			cerr << endl << "No threads were created!" << endl;
		}
		else
		{
			unsigned int	theCheckCount = 0;

			do
			{
				doSleep(2000);

				// Check a couple of times, just in case, since
				// getCounter() is not synchronized...
				if (theCounter.getCounter() == 0)
				{
					if (theCheckCount == 0)
					{
						theClock = clock();
					}

					++theCheckCount;
				}
			}
			while(theCheckCount < 2);
		}

		cout << endl << "Clock after threads: " << theClock << endl;
	}
	catch(...)
	{
		cerr << "Exception caught!!!"
			 << endl
			<< endl;
	}
}

int
main(
			int				argc,
			const char*		argv[])
{
#if !defined(NDEBUG) && defined(_MSC_VER)
	_CrtSetDbgFlag(_CrtSetDbgFlag(_CRTDBG_REPORT_FLAG) | _CRTDBG_LEAK_CHECK_DF);

	_CrtSetReportMode(_CRT_WARN, _CRTDBG_MODE_FILE);
	_CrtSetReportFile(_CRT_WARN, _CRTDBG_FILE_STDERR);
#endif

	if (argc > 2)
	{
		cerr << "Usage: ThreadTest"
			 << endl
			 << endl;
	}
	else
	{
		int	threadCount = 60;

		if (argc == 2)
		{
			threadCount = atoi(argv[1]);
		}

		try
		{
		  // Call the static initializers...
		  XMLPlatformUtils::Initialize();
          {
			XSLTInit	theInit;

			// Create the necessary stuff of compile the stylesheet.
			XercesDOMSupport				ssDOMSupport;
			XercesParserLiaison				ssParserLiaison(ssDOMSupport);
			XSLTProcessorEnvSupportDefault	ssXSLTProcessorEnvSupport;
			XObjectFactoryDefault			ssXObjectFactory;
			XPathFactoryDefault				ssXPathFactory;

			// Create a processor to compile the stylesheet...
			XSLTEngineImpl	ssProcessor(
					ssParserLiaison,
					ssXSLTProcessorEnvSupport,
					ssDOMSupport,
					ssXObjectFactory,
					ssXPathFactory);

			// Create separate factory support objects so the stylesheet's
			// factory-created XObject and XPath instances are independent
			// from processor's.
			XPathFactoryDefault		ssStylesheetXPathFactory;

			// Create a stylesheet construction context, using the
			// stylesheet's factory support objects.
			StylesheetConstructionContextDefault	ssConstructionContext(
													ssProcessor,
													ssXSLTProcessorEnvSupport,
													ssStylesheetXPathFactory);

			const XalanDOMString  theXSLFileName("birds.xsl");
			const XalanDOMString  theXMLFileName("birds.xml");

			// Our stylesheet input source...
			XSLTInputSource		ssStylesheetSourceXSL(c_wstr(theXSLFileName));
			XSLTInputSource		ssStylesheetSourceXML(c_wstr(theXMLFileName));

			// Ask the processor to create a StylesheetRoot for the specified
			// input XSL.  This is the compiled stylesheet.  We don't have to
			// delete it, since it is owned by the StylesheetConstructionContext
			// instance.

			glbStylesheetRoot = ssProcessor.processStylesheet(ssStylesheetSourceXSL,
													   ssConstructionContext);
			assert(glbStylesheetRoot != 0);

			doThreads(threadCount);
		  }

		XMLPlatformUtils::Terminate();

		}

		catch(...)
		{
			cerr << "Exception caught!!!"
				 << endl
				 << endl;
		}

	}

	return 0;
}
<commit_msg>Updates for HP.<commit_after>// Base header file.  Must be first.
#include <Include/PlatformDefinitions.hpp>

#include <cassert>

#if defined(XALAN_OLD_STREAM_HEADERS)
#include <iostream.h>
#else
#include <iostream>
#endif

#include <util/PlatformUtils.hpp>
#include <util/Mutexes.hpp>

#include <PlatformSupport/DOMStringHelper.hpp>
#include <PlatformSupport/XalanFileOutputStream.hpp>
#include <PlatformSupport/XalanOutputStreamPrintWriter.hpp>

#include <XalanSourceTree/XalanSourceTreeDOMSupport.hpp>
#include <XalanSourceTree/XalanSourceTreeParserLiaison.hpp>

#include <XPath/XObjectFactoryDefault.hpp>
#include <XPath/XPathFactoryDefault.hpp>

#include <XSLT/StylesheetConstructionContextDefault.hpp>
#include <XSLT/StylesheetExecutionContextDefault.hpp>
#include <XSLT/StylesheetRoot.hpp>
#include <XSLT/XSLTEngineImpl.hpp>
#include <XSLT/XSLTInit.hpp>
#include <XSLT/XSLTInputSource.hpp>
#include <XSLT/XSLTProcessorEnvSupportDefault.hpp>
#include <XSLT/XSLTResultTarget.hpp>

#if defined(WIN32)
//This is here for the threads.
#include <process.h>

#define WIN32_LEAN_AND_MEAN
#include <windows.h>
#elif defined(XALAN_POSIX2_AVAILABLE)
#include <pthread.h>
#include <unistd.h>
#else
#error Unsupported platform!
#endif

#if !defined(XALAN_NO_NAMESPACES)
	using std::cerr;
	using std::cout;
	using std::endl;
	using std::vector;
#endif

// This is here for memory leak testing.
#if defined(_DEBUG)
#include <crtdbg.h>
#endif

// Used to hold compiled stylesheet
StylesheetRoot* glbStylesheetRoot;

class SynchronizedCounter
{
public:

	SynchronizedCounter();

	~SynchronizedCounter();

	void
	increment();

	void
	decrement();

	unsigned long
	getCounter() const;

private:

	mutable XMLMutex	m_mutex;

	unsigned long		m_counter;
};

SynchronizedCounter::SynchronizedCounter() :
	m_mutex(),
	m_counter(0)
{
}

SynchronizedCounter::~SynchronizedCounter()
{
}

void
SynchronizedCounter::increment()
{
	XMLMutexLock	theLock(&m_mutex);

	if (m_counter < ULONG_MAX)
	{
		++m_counter;
	}
}

void
SynchronizedCounter::decrement()
{
	XMLMutexLock	theLock(&m_mutex);

	if (m_counter > 0)
	{
		--m_counter;
	}
}

unsigned long
SynchronizedCounter::getCounter() const
{
	return m_counter;
}

struct
ThreadInfo
{
	ThreadInfo(
			unsigned int			theThreadNumber,
			SynchronizedCounter*	theCounter) :
		m_threadNumber(theThreadNumber),
		m_counter(theCounter)
	{
	}

	unsigned int			m_threadNumber;

	SynchronizedCounter*	m_counter;
};

#if defined(WIN32)

extern "C" void theThreadRoutine(void* param);

void
#elif defined(XALAN_POSIX2_AVAILABLE)

extern "C" void* theThreadRoutine(void* param);

void*
#else
#error Unsupported platform!
#endif
theThreadRoutine(void*		param)
{
// This routine uses compiled stylesheet (glbStylesheetRoot), which is set using the
// theProcessor.setStylesheetRoot method. The transform is done using the theProcessor's
// process() method.

	const ThreadInfo*	theInfo = reinterpret_cast<const ThreadInfo*>(param);

	assert(theInfo != 0);

	theInfo->m_counter->increment();

	try
	{
		// Create the support objects that are necessary for running the processor...
		XalanSourceTreeDOMSupport		theDOMSupport;
		XalanSourceTreeParserLiaison	theParserLiaison(theDOMSupport);

		theDOMSupport.setParserLiaison(&theParserLiaison);
		// The default is that documents are not thread-safe.  Set this to
		// true so they are.
		//theParserLiaison.setThreadSafe(true);

		XSLTProcessorEnvSupportDefault	theXSLTProcessorEnvSupport;
		XObjectFactoryDefault			theXObjectFactory;
		XPathFactoryDefault				theXPathFactory;

		// Create a processor...and output start message.
		XSLTEngineImpl	theProcessor(
						theParserLiaison,
						theXSLTProcessorEnvSupport,
						theDOMSupport,
						theXObjectFactory,
						theXPathFactory);

		// Connect the processor to the support object...
		theXSLTProcessorEnvSupport.setProcessor(&theProcessor);

		// The execution context uses the same factory support objects as
		// the processor, since those objects have the same lifetime as
		// other objects created as a result of the execution.
		StylesheetExecutionContextDefault	ssExecutionContext(
							theProcessor,
							theXSLTProcessorEnvSupport,
							theDOMSupport,
							theXObjectFactory);

		// Our input files.  The assumption is that the executable will be run
		// from same directory as the input files.

		// Generate the input and output file names.
		const XalanDOMString	theXMLfile("birds.xml");

		const XalanDOMString	theOutputFile(
				XalanDOMString("birds") +
				UnsignedLongToDOMString(theInfo->m_threadNumber) +
				XalanDOMString(".out"));

		//Generate the XML input and output objects.
		XSLTInputSource		theInputSource(c_wstr(theXMLfile));
		XSLTResultTarget	theResultTarget(theOutputFile);

		// Set the stylesheet to be the compiled stylesheet. Then do the transform.
		// Report both the start of the transform and end of the thread.
		theProcessor.setStylesheetRoot(glbStylesheetRoot);
		theProcessor.process(theInputSource,theResultTarget,ssExecutionContext);
	}
	catch(...)
	{
	}

	// Decrement the counter because we're done...
	theInfo->m_counter->decrement();

#if defined(XALAN_POSIX2_AVAILABLE)
	return 0;
#endif
}

inline void
doSleep(unsigned int	theMilliseconds)
{
#if defined(WIN32)
	Sleep(theMilliseconds);
#elif defined(XALAN_POSIX2_AVAILABLE)
	usleep(theMilliseconds * 10);
#else
#error Unsupported platform!
#endif
}

void
doThreads(long	theThreadCount)
{
	cout << endl << "Starting " << theThreadCount << " threads." << endl;

	typedef vector<ThreadInfo>		ThreadInfoVectorType;

	ThreadInfoVectorType	theThreadInfo;

	theThreadInfo.reserve(theThreadCount);

	try
	{
		cout << endl << "Clock before starting threads: " << clock() << endl;

		SynchronizedCounter		theCounter;

		for (long i = 0; i < theThreadCount; i++)
		{
			theThreadInfo.push_back(ThreadInfoVectorType::value_type(i, &theCounter));

#if defined(WIN32)

			const unsigned long		theThreadID =
					_beginthread(theThreadRoutine, 4096, reinterpret_cast<LPVOID>(&theThreadInfo.back()));

			if (theThreadID == unsigned(-1))
			{
				cerr << endl << "Unable to create thread number " << i + 1 << "." << endl;
			}

#elif defined(XALAN_POSIX2_AVAILABLE)

			pthread_t	theThread;

			const int	theResult = pthread_create(&theThread, 0, theThreadRoutine, (void*)&theThreadInfo.back());

			if (theResult != 0)
			{
				cerr << endl << "Unable to create thread number " << i + 1 << "." << endl;
			}
			else
			{
				pthread_detach(theThread);
			}
#else
#error Unsupported platform!
#endif
		}

		clock_t		theClock = 0;

		if (theThreadInfo.size() == 0)
		{
			cerr << endl << "No threads were created!" << endl;
		}
		else
		{
			unsigned int	theCheckCount = 0;

			do
			{
				doSleep(2000);

				// Check a couple of times, just in case, since
				// getCounter() is not synchronized...
				if (theCounter.getCounter() == 0)
				{
					if (theCheckCount == 0)
					{
						theClock = clock();
					}

					++theCheckCount;
				}
			}
			while(theCheckCount < 2);
		}

		cout << endl << "Clock after threads: " << theClock << endl;
	}
	catch(...)
	{
		cerr << "Exception caught!!!"
			 << endl
			<< endl;
	}
}

int
main(
			int				argc,
			const char*		argv[])
{
#if !defined(NDEBUG) && defined(_MSC_VER)
	_CrtSetDbgFlag(_CrtSetDbgFlag(_CRTDBG_REPORT_FLAG) | _CRTDBG_LEAK_CHECK_DF);

	_CrtSetReportMode(_CRT_WARN, _CRTDBG_MODE_FILE);
	_CrtSetReportFile(_CRT_WARN, _CRTDBG_FILE_STDERR);
#endif

	if (argc > 2)
	{
		cerr << "Usage: ThreadTest"
			 << endl
			 << endl;
	}
	else
	{
		int	threadCount = 60;

		if (argc == 2)
		{
			threadCount = atoi(argv[1]);
		}

		try
		{
		  // Call the static initializers...
		  XMLPlatformUtils::Initialize();
          {
			XSLTInit	theInit;

			// Create the necessary stuff of compile the stylesheet.
			XercesDOMSupport				ssDOMSupport;
			XercesParserLiaison				ssParserLiaison(ssDOMSupport);
			XSLTProcessorEnvSupportDefault	ssXSLTProcessorEnvSupport;
			XObjectFactoryDefault			ssXObjectFactory;
			XPathFactoryDefault				ssXPathFactory;

			// Create a processor to compile the stylesheet...
			XSLTEngineImpl	ssProcessor(
					ssParserLiaison,
					ssXSLTProcessorEnvSupport,
					ssDOMSupport,
					ssXObjectFactory,
					ssXPathFactory);

			// Create separate factory support objects so the stylesheet's
			// factory-created XObject and XPath instances are independent
			// from processor's.
			XPathFactoryDefault		ssStylesheetXPathFactory;

			// Create a stylesheet construction context, using the
			// stylesheet's factory support objects.
			StylesheetConstructionContextDefault	ssConstructionContext(
													ssProcessor,
													ssXSLTProcessorEnvSupport,
													ssStylesheetXPathFactory);

			const XalanDOMString  theXSLFileName("birds.xsl");
			const XalanDOMString  theXMLFileName("birds.xml");

			// Our stylesheet input source...
			XSLTInputSource		ssStylesheetSourceXSL(c_wstr(theXSLFileName));
			XSLTInputSource		ssStylesheetSourceXML(c_wstr(theXMLFileName));

			// Ask the processor to create a StylesheetRoot for the specified
			// input XSL.  This is the compiled stylesheet.  We don't have to
			// delete it, since it is owned by the StylesheetConstructionContext
			// instance.

			glbStylesheetRoot = ssProcessor.processStylesheet(ssStylesheetSourceXSL,
													   ssConstructionContext);
			assert(glbStylesheetRoot != 0);

			doThreads(threadCount);
		  }

		XMLPlatformUtils::Terminate();

		}

		catch(...)
		{
			cerr << "Exception caught!!!"
				 << endl
				 << endl;
		}

	}

	return 0;
}
<|endoftext|>


<commit_before>#include <UnitTest/stdafx.h>
#include <UnitTest/UnitTest.h>
#include <core/smartview/block/block.h>
#include <core/smartview/block/blockBytes.h>
#include <core/smartview/block/blockStringA.h>
#include <core/smartview/block/blockStringW.h>
#include <core/smartview/block/blockT.h>

namespace blocktest
{
	TEST_CLASS(blocktest)
	{
	public:
		// Without this, clang gets weird
		static const bool dummy_var = true;

		TEST_CLASS_INITIALIZE(initialize) { unittest::init(); }
		TEST_METHOD(Test_block)
		{
			auto block1 = std::make_shared<smartview::block>(L"test");
			auto block2 = std::make_shared<smartview::block>(L"block2");
			auto block3 = std::make_shared<smartview::block>(L"");
			Assert::AreEqual(block1->isHeader(), true);
			block1->setSize(5);
			Assert::AreEqual(block1->isHeader(), false);
			block1->setOffset(6);
			Assert::AreEqual(block1->isHeader(), false);
			block1->setSource(7);

			Assert::AreEqual(block1->getText(), std::wstring(L"test"));
			Assert::AreEqual(block1->getChildren().empty(), true);
			Assert::AreEqual(block1->toString(), std::wstring(L"test"));
			Assert::AreEqual(block1->getSize(), size_t(5));
			Assert::AreEqual(block1->getOffset(), size_t(6));
			Assert::AreEqual(block1->getSource(), ULONG(7));

			block1->setText(L"the %1!ws!", L"other");
			Assert::AreEqual(block1->getText(), std::wstring(L"the other"));
			block1->addHeader(L" this %1!ws!", L"that");
			Assert::AreEqual(block1->toString(), std::wstring(L"the other this that"));

			block1->setText(L"hello");
			Assert::AreEqual(block1->getText(), std::wstring(L"hello"));

			block1->setText(L"hello %1!ws!", L"world");
			Assert::AreEqual(block1->getText(), std::wstring(L"hello world"));

			block1->terminateBlock();
			block1->terminateBlock();
			block1->addChild(block2);
			Assert::AreEqual(block1->toString(), std::wstring(L"hello world this that\r\nblock2"));

			block1->addBlankLine();
			block1->addLabeledChild(L"Label: ", block2);
			Assert::AreEqual(block1->toString(), std::wstring(L"hello world this that\r\nblock2\r\n\r\nLabel: block2\r\n"));

			Assert::AreEqual(block3->hasData(), false);
			block3->addChild(block1);
			Assert::AreEqual(block3->hasData(), true);
		}
	};
} // namespace blocktest<commit_msg>more coverage<commit_after>#include <UnitTest/stdafx.h>
#include <UnitTest/UnitTest.h>
#include <core/smartview/block/block.h>
#include <core/smartview/block/blockBytes.h>
#include <core/smartview/block/blockStringA.h>
#include <core/smartview/block/blockStringW.h>
#include <core/smartview/block/blockT.h>

namespace blocktest
{
	TEST_CLASS(blocktest)
	{
	public:
		// Without this, clang gets weird
		static const bool dummy_var = true;

		TEST_CLASS_INITIALIZE(initialize) { unittest::init(); }
		TEST_METHOD(Test_block)
		{
			auto block1 = std::make_shared<smartview::block>(L"test");
			auto block2 = std::make_shared<smartview::block>(L"block2");
			auto block3 = std::make_shared<smartview::block>(L"");
			Assert::AreEqual(block1->isHeader(), true);
			block1->setSize(5);
			Assert::AreEqual(block1->isHeader(), false);
			block1->setOffset(6);
			Assert::AreEqual(block1->isHeader(), false);
			block1->setSource(7);

			Assert::AreEqual(block1->getText(), std::wstring(L"test"));
			Assert::AreEqual(block1->getChildren().empty(), true);
			Assert::AreEqual(block1->toString(), std::wstring(L"test"));
			Assert::AreEqual(block1->getSize(), size_t(5));
			Assert::AreEqual(block1->getOffset(), size_t(6));
			Assert::AreEqual(block1->getSource(), ULONG(7));

			block1->setText(L"the %1!ws!", L"other");
			Assert::AreEqual(block1->getText(), std::wstring(L"the other"));
			block1->addHeader(L" this %1!ws!", L"that");
			Assert::AreEqual(block1->toString(), std::wstring(L"the other this that"));

			block1->setText(L"hello");
			Assert::AreEqual(block1->getText(), std::wstring(L"hello"));

			block1->setText(L"hello %1!ws!", L"world");
			Assert::AreEqual(block1->getText(), std::wstring(L"hello world"));

			block1->terminateBlock();
			block1->terminateBlock();
			block1->addChild(block2);
			Assert::AreEqual(block1->toString(), std::wstring(L"hello world this that\r\nblock2"));

			block1->addBlankLine();
			block1->addLabeledChild(L"Label: ", block2);
			Assert::AreEqual(
				block1->toString(), std::wstring(L"hello world this that\r\nblock2\r\n\r\nLabel: block2\r\n"));

			Assert::AreEqual(block3->hasData(), false);
			block3->addChild(block1);
			Assert::AreEqual(block3->hasData(), true);
		}

		TEST_METHOD(Test_blockStringA)
		{
			auto block1 = std::make_shared<smartview::blockStringA>();
			Assert::AreEqual(block1->isSet(), false);
			auto block2 = std::make_shared<smartview::blockStringA>(std::string("test"), 4, 5);
			Assert::AreEqual(block2->toWstring(), std::wstring(L"test"));
			Assert::AreEqual(block2->length(), size_t(4));
		}

		TEST_METHOD(Test_blockStringW)
		{
			auto block1 = std::make_shared<smartview::blockStringW>();
			Assert::AreEqual(block1->isSet(), false);
			auto block2 = std::make_shared<smartview::blockStringW>(std::wstring(L"test"), 4, 5);
			Assert::AreEqual(block2->toString(), std::wstring(L"test"));
			Assert::AreEqual(block2->length(), size_t(4));
		}
	};
} // namespace blocktest<|endoftext|>


<commit_before>// Copyright (c) 2017, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-core.
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core 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 Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core. If not, see <http://www.gnu.org/licenses/>.

#include <hpp/core/explicit-relative-transformation.hh>

#include <pinocchio/spatial/explog.hpp>
#include <pinocchio/spatial/skew.hpp>

#include <hpp/pinocchio/device.hh>

#include <../src/times-frame-function.hh>

namespace hpp {
  namespace core {
    namespace {
      typedef JointJacobian_t::ConstNRowsBlockXpr<3>::Type ConstHalfJacobian_t;
      inline ConstHalfJacobian_t omega(const JointJacobian_t& j) { return j.bottomRows<3>(); }
      inline ConstHalfJacobian_t trans(const JointJacobian_t& j) { return j.topRows<3>(); }

      void inputVariable (JointConstPtr_t joint, std::vector<bool>& conf, std::vector<bool>& vel)
      {
        while (joint && joint->index() != 0) {
          for (size_type i = 0; i < joint->configSize(); ++i)
            conf[joint->rankInConfiguration() + i] = !conf[joint->rankInConfiguration() + i];
          for (size_type i = 0; i < joint->numberDof(); ++i)
            vel[joint->rankInVelocity() + i] = !vel[joint->rankInVelocity() + i];
          hppDout (info, "Adding joint " << joint->name ()
                   << " as input variable.");
          joint = joint->parentJoint();
        }
      }

      BlockIndex::segments_t vectorOfBoolToIntervals (std::vector<bool>& v)
      {
        BlockIndex::segments_t ret;
        for (std::size_t i = 0; i < v.size(); ++i)
          if (v[i]) ret.push_back(BlockIndex::segment_t (i, 1));
        BlockIndex::shrink (ret);
        return ret;
      }

      BlockIndex::segments_t jointConfInterval (JointConstPtr_t j) {
        return BlockIndex::segments_t(1, BlockIndex::segment_t
                                      (j->rankInConfiguration(),
                                       j->configSize()));
      }
      BlockIndex::segments_t jointVelInterval (JointConstPtr_t j) {
        return BlockIndex::segments_t(1, BlockIndex::segment_t
                                      (j->rankInVelocity(), j->numberDof()));
      }
    }

    ExplicitRelativeTransformationPtr_t ExplicitRelativeTransformation::create
      (const std::string& name      , const DevicePtr_t& robot,
       const JointConstPtr_t& joint1, const JointConstPtr_t& joint2,
       const Transform3f& frame1    , const Transform3f& frame2)
      {
        std::vector<bool> conf (robot->configSize(), false);
        std::vector<bool> vel  (robot->numberDof(), false);
        inputVariable (joint1, conf, vel);
        inputVariable (joint2->parentJoint(), conf, vel);

        ExplicitRelativeTransformation* ptr =
          new ExplicitRelativeTransformation (name, robot, joint1, joint2,
              frame1, frame2,
              vectorOfBoolToIntervals(conf),
              jointConfInterval(joint2),
              vectorOfBoolToIntervals(vel),
              jointVelInterval(joint2));
        ExplicitRelativeTransformationPtr_t shPtr (ptr);
        ptr->init (shPtr);
        return shPtr;
      }

    ExplicitRelativeTransformation::ExplicitRelativeTransformation (
        const std::string& name      , const DevicePtr_t& robot,
        const JointConstPtr_t& joint1, const JointConstPtr_t& joint2,
        const Transform3f& frame1    , const Transform3f& frame2,
        const segments_t inConf , const segments_t outConf,
        const segments_t inVel  , const segments_t outVel ,
        std::vector <bool> /*mask*/)
      : DifferentiableFunction (
          BlockIndex::cardinal(inConf),  BlockIndex::cardinal(inVel),
          pinocchio::LiegroupSpace::SE3 (), name),
      robot_ (robot),
      parentJoint_ (joint2->parentJoint ()),
      joint1_ (joint1), joint2_ (joint2),
      inConf_ (inConf),   inVel_  (inVel),
      outConf_ (outConf), outVel_ (outVel),
      F1inJ1_invF2inJ2_ (frame1)
    {
      if (!frame2.isIdentity()) {
        g_    = DifferentiableFunctionPtr_t (new TimesFrameFunction
            (frame2, "Output function " + name));
        ginv_ = DifferentiableFunctionPtr_t (new TimesFrameFunction
            (frame2.inverse(), "Inverse output function " + name));
      }
    }

    void ExplicitRelativeTransformation::forwardKinematics (vectorIn_t arg) const
    {
      qsmall_ = inConf_.rview(robot_->currentConfiguration());
      if (qsmall_ != arg) {
        q_ = robot_->currentConfiguration();
        inConf_.lview(q_) = arg;
        robot_->currentConfiguration(q_);
      }
      robot_->computeForwardKinematics ();
    }

    void ExplicitRelativeTransformation::impl_compute
    (LiegroupElement& result, vectorIn_t argument) const
    {
      forwardKinematics (argument);

      // J1 * M1/J1 = J2 * M2/J2
      // J2 = J1 * M1/J1 * M2/J2^{-1}
      // J2 = J2_{parent} * T
      // T = J2_{parent}^{-1} * J2
      // T = J2_{parent}^{-1} * J1 * F1/J1 * F2/J2^{-1}
      freeflyerPose_ =
        joint1_->currentTransformation () * F1inJ1_invF2inJ2_;

      if (parentJoint_)
        freeflyerPose_ = parentJoint_->currentTransformation ().actInv(freeflyerPose_);

      freeflyerPose_ =
        joint2_->positionInParentFrame ().actInv (freeflyerPose_);

      typedef Transform3f::Quaternion_t Q_t;
      result.vector ().head<3>() = freeflyerPose_.translation();
      result.vector ().tail<4>() = Q_t(freeflyerPose_.rotation()).coeffs();
    }

    void ExplicitRelativeTransformation::impl_jacobian (matrixOut_t jacobian, vectorIn_t arg) const
    {
      LiegroupElement result (outputSpace ());
      impl_compute (result, arg);
      Configuration_t q (robot_->currentConfiguration ());
      outConf_.lview (q) = result.vector ();
      robot_->currentConfiguration (q);
      robot_->computeForwardKinematics ();

      const JointJacobian_t& J1 (joint1_->jacobian());
      // const JointJacobian_t& J2_parent (parentJoint_->jacobian());

      const matrix3_t& R1 (joint1_->currentTransformation().rotation());
      const matrix3_t& R2 (joint2_->currentTransformation().rotation());
      const matrix3_t& R2_inParentFrame (joint2_->positionInParentFrame().
                                         rotation());

      const vector3_t& t1 (joint1_->currentTransformation().translation());

      cross1_ = se3::skew((R1 * F1inJ1_invF2inJ2_.translation()).eval());
      if (parentJoint_) {
        const vector3_t& t2_parent (parentJoint_       ->currentTransformation().translation());
        cross2_ = se3::skew((t2_parent - t1).eval());

        J2_parent_minus_J1_.noalias() = parentJoint_->jacobian() - J1;
      } else {
        cross2_ = - se3::skew(t1);
        // J2_parent_minus_J1_ = - J1;
      }

      matrix3_t R2t_R1 (R2.transpose() * R1);
      // Express velocity of J1 * M1/J1 * M2/J2^{-1} in J2_{parent}.
      if (parentJoint_) {
        const matrix3_t&       R2_parent (parentJoint_->currentTransformation().rotation());
        const JointJacobian_t& J2_parent (parentJoint_->jacobian());

        tmpJac_.noalias() = (R2_inParentFrame.transpose() * R2_parent.transpose()) *
          ( cross1_ * (omega(J2_parent_minus_J1_))
            - cross2_ * omega(J2_parent)
            - trans(J2_parent_minus_J1_));
      } else {
        tmpJac_.noalias()  = (- R2.transpose() * cross1_ * R1) * omega(J1);
        tmpJac_.noalias() += R2t_R1 * trans(J1);
      }

      jacobian.topRows<3>() = inVel_.rview(tmpJac_);
      // jacobian.topRows<3>().setZero();

      if (parentJoint_) {
        const matrix3_t&       R2_parent (parentJoint_->currentTransformation().rotation());
        const JointJacobian_t& J2_parent (parentJoint_->jacobian());

        // J = p2RT2 * 0RTp2 * [ p2
        tmpJac_.noalias() = ( R2.transpose() * R2_parent ) * omega(J2_parent)
          - (R2.transpose() * R1) * omega(J1);
      } else {
        tmpJac_.noalias() = R2t_R1 * omega(J1);
      }

      jacobian.bottomRows<3>() = inVel_.rview(tmpJac_);
    }

    constraints::ExplicitPtr_t ExplicitRelativeTransformation::createNumericalConstraint ()
    {
      if (g_)
        return constraints::Explicit::create (
            robot_->configSpace (),
            weak_.lock(),
            g_, ginv_,
            inConf_.indices(),
            inVel_.indices(),
            outConf_.indices(),
            outVel_.indices());
      else
        return constraints::Explicit::create (
            robot_->configSpace (),
            weak_.lock(),
            inConf_.indices(),
            inVel_.indices(),
            outConf_.indices(),
            outVel_.indices());
    }
  } // namespace core
} // namespace hpp
<commit_msg>Fix ExplicitRelativeTransformation::createNumericalConstraint<commit_after>// Copyright (c) 2017, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-core.
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core 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 Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core. If not, see <http://www.gnu.org/licenses/>.

#include <hpp/core/explicit-relative-transformation.hh>

#include <pinocchio/spatial/explog.hpp>
#include <pinocchio/spatial/skew.hpp>

#include <hpp/pinocchio/device.hh>

#include <../src/times-frame-function.hh>

namespace hpp {
  namespace core {
    namespace {
      typedef JointJacobian_t::ConstNRowsBlockXpr<3>::Type ConstHalfJacobian_t;
      inline ConstHalfJacobian_t omega(const JointJacobian_t& j) { return j.bottomRows<3>(); }
      inline ConstHalfJacobian_t trans(const JointJacobian_t& j) { return j.topRows<3>(); }

      void inputVariable (JointConstPtr_t joint, std::vector<bool>& conf, std::vector<bool>& vel)
      {
        while (joint && joint->index() != 0) {
          for (size_type i = 0; i < joint->configSize(); ++i)
            conf[joint->rankInConfiguration() + i] = !conf[joint->rankInConfiguration() + i];
          for (size_type i = 0; i < joint->numberDof(); ++i)
            vel[joint->rankInVelocity() + i] = !vel[joint->rankInVelocity() + i];
          hppDout (info, "Adding joint " << joint->name ()
                   << " as input variable.");
          joint = joint->parentJoint();
        }
      }

      BlockIndex::segments_t vectorOfBoolToIntervals (std::vector<bool>& v)
      {
        BlockIndex::segments_t ret;
        for (std::size_t i = 0; i < v.size(); ++i)
          if (v[i]) ret.push_back(BlockIndex::segment_t (i, 1));
        BlockIndex::shrink (ret);
        return ret;
      }

      BlockIndex::segments_t jointConfInterval (JointConstPtr_t j) {
        return BlockIndex::segments_t(1, BlockIndex::segment_t
                                      (j->rankInConfiguration(),
                                       j->configSize()));
      }
      BlockIndex::segments_t jointVelInterval (JointConstPtr_t j) {
        return BlockIndex::segments_t(1, BlockIndex::segment_t
                                      (j->rankInVelocity(), j->numberDof()));
      }
    }

    ExplicitRelativeTransformationPtr_t ExplicitRelativeTransformation::create
      (const std::string& name      , const DevicePtr_t& robot,
       const JointConstPtr_t& joint1, const JointConstPtr_t& joint2,
       const Transform3f& frame1    , const Transform3f& frame2)
      {
        std::vector<bool> conf (robot->configSize(), false);
        std::vector<bool> vel  (robot->numberDof(), false);
        inputVariable (joint1, conf, vel);
        inputVariable (joint2->parentJoint(), conf, vel);

        ExplicitRelativeTransformation* ptr =
          new ExplicitRelativeTransformation (name, robot, joint1, joint2,
              frame1, frame2,
              vectorOfBoolToIntervals(conf),
              jointConfInterval(joint2),
              vectorOfBoolToIntervals(vel),
              jointVelInterval(joint2));
        ExplicitRelativeTransformationPtr_t shPtr (ptr);
        ptr->init (shPtr);
        return shPtr;
      }

    ExplicitRelativeTransformation::ExplicitRelativeTransformation (
        const std::string& name      , const DevicePtr_t& robot,
        const JointConstPtr_t& joint1, const JointConstPtr_t& joint2,
        const Transform3f& frame1    , const Transform3f& frame2,
        const segments_t inConf , const segments_t outConf,
        const segments_t inVel  , const segments_t outVel ,
        std::vector <bool> /*mask*/)
      : DifferentiableFunction (
          BlockIndex::cardinal(inConf),  BlockIndex::cardinal(inVel),
          pinocchio::LiegroupSpace::SE3 (), name),
      robot_ (robot),
      parentJoint_ (joint2->parentJoint ()),
      joint1_ (joint1), joint2_ (joint2),
      inConf_ (inConf),   inVel_  (inVel),
      outConf_ (outConf), outVel_ (outVel),
      F1inJ1_invF2inJ2_ (frame1)
    {
      if (!frame2.isIdentity()) {
        g_    = DifferentiableFunctionPtr_t (new TimesFrameFunction
            (frame2, "Output function " + name));
        ginv_ = DifferentiableFunctionPtr_t (new TimesFrameFunction
            (frame2.inverse(), "Inverse output function " + name));
      }
    }

    void ExplicitRelativeTransformation::forwardKinematics (vectorIn_t arg) const
    {
      qsmall_ = inConf_.rview(robot_->currentConfiguration());
      if (qsmall_ != arg) {
        q_ = robot_->currentConfiguration();
        inConf_.lview(q_) = arg;
        robot_->currentConfiguration(q_);
      }
      robot_->computeForwardKinematics ();
    }

    void ExplicitRelativeTransformation::impl_compute
    (LiegroupElement& result, vectorIn_t argument) const
    {
      forwardKinematics (argument);

      // J1 * M1/J1 = J2 * M2/J2
      // J2 = J1 * M1/J1 * M2/J2^{-1}
      // J2 = J2_{parent} * T
      // T = J2_{parent}^{-1} * J2
      // T = J2_{parent}^{-1} * J1 * F1/J1 * F2/J2^{-1}
      freeflyerPose_ =
        joint1_->currentTransformation () * F1inJ1_invF2inJ2_;

      if (parentJoint_)
        freeflyerPose_ = parentJoint_->currentTransformation ().actInv(freeflyerPose_);

      freeflyerPose_ =
        joint2_->positionInParentFrame ().actInv (freeflyerPose_);

      typedef Transform3f::Quaternion_t Q_t;
      result.vector ().head<3>() = freeflyerPose_.translation();
      result.vector ().tail<4>() = Q_t(freeflyerPose_.rotation()).coeffs();
    }

    void ExplicitRelativeTransformation::impl_jacobian (matrixOut_t jacobian, vectorIn_t arg) const
    {
      LiegroupElement result (outputSpace ());
      impl_compute (result, arg);
      Configuration_t q (robot_->currentConfiguration ());
      outConf_.lview (q) = result.vector ();
      robot_->currentConfiguration (q);
      robot_->computeForwardKinematics ();

      const JointJacobian_t& J1 (joint1_->jacobian());
      // const JointJacobian_t& J2_parent (parentJoint_->jacobian());

      const matrix3_t& R1 (joint1_->currentTransformation().rotation());
      const matrix3_t& R2 (joint2_->currentTransformation().rotation());
      const matrix3_t& R2_inParentFrame (joint2_->positionInParentFrame().
                                         rotation());

      const vector3_t& t1 (joint1_->currentTransformation().translation());

      cross1_ = se3::skew((R1 * F1inJ1_invF2inJ2_.translation()).eval());
      if (parentJoint_) {
        const vector3_t& t2_parent (parentJoint_       ->currentTransformation().translation());
        cross2_ = se3::skew((t2_parent - t1).eval());

        J2_parent_minus_J1_.noalias() = parentJoint_->jacobian() - J1;
      } else {
        cross2_ = - se3::skew(t1);
        // J2_parent_minus_J1_ = - J1;
      }

      matrix3_t R2t_R1 (R2.transpose() * R1);
      // Express velocity of J1 * M1/J1 * M2/J2^{-1} in J2_{parent}.
      if (parentJoint_) {
        const matrix3_t&       R2_parent (parentJoint_->currentTransformation().rotation());
        const JointJacobian_t& J2_parent (parentJoint_->jacobian());

        tmpJac_.noalias() = (R2_inParentFrame.transpose() * R2_parent.transpose()) *
          ( cross1_ * (omega(J2_parent_minus_J1_))
            - cross2_ * omega(J2_parent)
            - trans(J2_parent_minus_J1_));
      } else {
        tmpJac_.noalias()  = (- R2.transpose() * cross1_ * R1) * omega(J1);
        tmpJac_.noalias() += R2t_R1 * trans(J1);
      }

      jacobian.topRows<3>() = inVel_.rview(tmpJac_);
      // jacobian.topRows<3>().setZero();

      if (parentJoint_) {
        const matrix3_t&       R2_parent (parentJoint_->currentTransformation().rotation());
        const JointJacobian_t& J2_parent (parentJoint_->jacobian());

        // J = p2RT2 * 0RTp2 * [ p2
        tmpJac_.noalias() = ( R2.transpose() * R2_parent ) * omega(J2_parent)
          - (R2.transpose() * R1) * omega(J1);
      } else {
        tmpJac_.noalias() = R2t_R1 * omega(J1);
      }

      jacobian.bottomRows<3>() = inVel_.rview(tmpJac_);
    }

    constraints::ExplicitPtr_t ExplicitRelativeTransformation::createNumericalConstraint ()
    {
      return constraints::Explicit::create (robot_->configSpace (),
                                            weak_.lock(), inConf_.indices(),
                                            inVel_.indices(), outConf_.indices(),
                                            outVel_.indices());
    }
  } // namespace core
} // namespace hpp
<|endoftext|>


<commit_before>/*************** <auto-copyright.pl BEGIN do not edit this line> **************
 *
 * VR Juggler is (C) Copyright 1998-2007 by Iowa State University
 *
 * Original Authors:
 *   Allen Bierbaum, Christopher Just,
 *   Patrick Hartling, Kevin Meinert,
 *   Carolina Cruz-Neira, Albert Baker
 *
 * 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; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 *
 *************** <auto-copyright.pl END do not edit this line> ***************/

#include <gadget/Devices/DriverConfig.h>

#include <string>
#include <boost/bind.hpp>

#include <gmtl/Matrix.h>
#include <gmtl/Vec.h>
#include <gmtl/MatrixOps.h>
#include <gmtl/Generate.h>
#include <gmtl/AxisAngle.h>

#include <vpr/System.h>

#include <jccl/Config/ConfigElement.h>
#include <gadget/Type/DeviceConstructor.h>
#include <gadget/gadgetParam.h>
#include <drivers/EssentialReality/P5Glove/P5GloveStandalone.h> /* standalone dataglove driver */
#include <drivers/EssentialReality/P5Glove/P5GloveWrapper.h> /* Gadgeteer dataglove driver */

extern "C"
{

GADGET_DRIVER_EXPORT(vpr::Uint32) getGadgeteerVersion()
{
   return __GADGET_version;
}

GADGET_DRIVER_EXPORT(void) initDevice(gadget::InputManager* inputMgr)
{
   new gadget::DeviceConstructor<gadget::P5GloveWrapper>(inputMgr);
}

}

namespace gadget
{

P5GloveWrapper::P5GloveWrapper()
   : mGlove(NULL)
   , mAnalogP5(5)
   , mDigitalP5(3)
   , mPositionP5(1)
   , mExitFlag(false)
{
   mAnalogP5[0] = 0.0f;
   mAnalogP5[1] = 0.0f;
   mAnalogP5[2] = 0.0f;
   mAnalogP5[3] = 0.0f;
   mAnalogP5[5] = 0.0f;
}

P5GloveWrapper::~P5GloveWrapper()
{
   stopSampling();      // Stop the glove

   if ( NULL != mGlove )
   {
      delete mGlove;       // Delete the glove
      mGlove = NULL;
   }
}

bool P5GloveWrapper::config(jccl::ConfigElementPtr e)
{
   if ( ! (Input::config(e) /*&& Glove::config(e)*/ ) )
   {
      return false;
   }

   mGloveNumber = e->getProperty<int>("number");

   vprASSERT(mThread == NULL);      // This should have been set by Input(c)
   mGlove = new P5GloveStandalone();
   return true;
}

std::string P5GloveWrapper::getElementType()
{
   return "p5_glove";
}

bool P5GloveWrapper::startSampling()
{
   bool started(false);

   if ( mThread == NULL )
   {
      vprDEBUG(gadgetDBG_INPUT_MGR, vprDBG_STATE_LVL)
         << "[p5glove] Begin sampling\n" << vprDEBUG_FLUSH;

      int maxAttempts(0);
      bool result(false);

      while ( result == false && maxAttempts < 5 )
      {
         vprDEBUG(gadgetDBG_INPUT_MGR, vprDBG_STATE_LVL)
            << "[p5glove] Connecting to glove number " << mGloveNumber
            << "...\n" << vprDEBUG_FLUSH;
         result = mGlove->connectToHardware(mGloveNumber);

         if ( result == false )
         {
            vprDEBUG(gadgetDBG_INPUT_MGR, vprDBG_CRITICAL_LVL)
               << "[p5glove] ERROR: Can't open or it is already opened."
               << vprDEBUG_FLUSH;
            vpr::System::usleep(14500);
            ++maxAttempts;
         }
      }

      vprDEBUG(gadgetDBG_INPUT_MGR, vprDBG_STATE_LVL)
         << "[p5glove] Successfully connected, Now sampling dataglove data.\n"
         << vprDEBUG_FLUSH;

      // Create a new thread to handle the control and set exit flag to false
      mExitFlag = false;
      vprDEBUG(gadgetDBG_INPUT_MGR, vprDBG_STATE_LVL)
         << "[p5glove] Spawning control thread.\n" << vprDEBUG_FLUSH;

      try
      {
         mThread = new vpr::Thread(boost::bind(&P5GloveWrapper::controlLoop,
                                               this));
         mActive = true;
         started = true;

         vprDEBUG(gadgetDBG_INPUT_MGR, vprDBG_STATE_LVL)
            << "[p5glove] P5Glove is active.\n" << vprDEBUG_FLUSH;
      }
      catch (vpr::Exception& ex)
      {
         vprDEBUG(gadgetDBG_INPUT_MGR, vprDBG_CRITICAL_LVL)
            << clrOutBOLD(clrRED, "ERROR")
            << ": Failed to spawn thread for P5 Glove driver!\n"
            << vprDEBUG_FLUSH;
         vprDEBUG_NEXT(gadgetDBG_INPUT_MGR, vprDBG_CRITICAL_LVL)
            << ex.what() << std::endl << vprDEBUG_FLUSH;
         started = false;
      }
   }

   return started;
}

void P5GloveWrapper::controlLoop()
{
   vprDEBUG(gadgetDBG_INPUT_MGR, vprDBG_STATE_LVL)
      << "[p5glove] Entered control thread\n" << vprDEBUG_FLUSH;

   // Go until mExitFlag is set to true
   while ( ! mExitFlag )
   {
      sample();
   }
}

bool P5GloveWrapper::sample()
{
   P5GloveStandalone::Record rec;
   bool sample_read = mGlove->readRecordsFromHardware(rec);

   if ( sample_read )
   {
// Furst, we set the flexion of the gloves
      mAnalogP5[0] = rec.thumb;     // Thumb (0.0 - 1.0)
      mAnalogP5[1] = rec.index;     // Index
      mAnalogP5[2] = rec.middle;    // Middle
      mAnalogP5[3] = rec.ring;     // Ring
      mAnalogP5[4] = rec.pinky;    // Pinky
      addAnalogSample(mAnalogP5);
      swapAnalogBuffers();

// Then, we define the buttons of the glove
      mDigitalP5[0] = rec.buttonA;
      mDigitalP5[1] = rec.buttonB;
      mDigitalP5[2] = rec.buttonC;
      addDigitalSample(mDigitalP5);
      swapDigitalBuffers();

// Finally, we set the position of the glove ...
      const gmtl::AxisAnglef rotation(rec.rotationAngle, rec.rotationAxis[0],
                                      rec.rotationAxis[1],
                                      rec.rotationAxis[2]);
      const gmtl::Vec3f translation(static_cast<float>(rec.position[0]),
                                    static_cast<float>(rec.position[1]),
                                    static_cast<float>(rec.position[2]));
      gmtl::Matrix44f position = gmtl::makeTrans<gmtl::Matrix44f>(translation);
      position = position * gmtl::make<gmtl::Matrix44f>(rotation);
      mPositionP5[0].mPosData = position;
      addPositionSample(mPositionP5);
      swapPositionBuffers();
   }

   return true;
}

void P5GloveWrapper::updateData()
{
   swapAnalogBuffers();
   swapDigitalBuffers();
   swapPositionBuffers();
}

bool P5GloveWrapper::stopSampling()
{
   if ( mThread != NULL )
   {
      mExitFlag = true;

      //Wait for thread to exit normally after setting exit flag
      mThread->join();
      delete mThread;
      mThread = NULL;

      vprDEBUG(gadgetDBG_INPUT_MGR, vprDBG_CONFIG_LVL)
         << "[p5glove] stopping P5Glove.." << std::endl << vprDEBUG_FLUSH;
   }

   return true;
}

} // End of gadget namespace
<commit_msg>If the connection to the hardware fails, do not spawn a sampling thread.<commit_after>/*************** <auto-copyright.pl BEGIN do not edit this line> **************
 *
 * VR Juggler is (C) Copyright 1998-2007 by Iowa State University
 *
 * Original Authors:
 *   Allen Bierbaum, Christopher Just,
 *   Patrick Hartling, Kevin Meinert,
 *   Carolina Cruz-Neira, Albert Baker
 *
 * 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; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 *
 *************** <auto-copyright.pl END do not edit this line> ***************/

#include <gadget/Devices/DriverConfig.h>

#include <string>
#include <boost/bind.hpp>

#include <gmtl/Matrix.h>
#include <gmtl/Vec.h>
#include <gmtl/MatrixOps.h>
#include <gmtl/Generate.h>
#include <gmtl/AxisAngle.h>

#include <vpr/System.h>

#include <jccl/Config/ConfigElement.h>
#include <gadget/Type/DeviceConstructor.h>
#include <gadget/gadgetParam.h>
#include <drivers/EssentialReality/P5Glove/P5GloveStandalone.h> /* standalone dataglove driver */
#include <drivers/EssentialReality/P5Glove/P5GloveWrapper.h> /* Gadgeteer dataglove driver */

extern "C"
{

GADGET_DRIVER_EXPORT(vpr::Uint32) getGadgeteerVersion()
{
   return __GADGET_version;
}

GADGET_DRIVER_EXPORT(void) initDevice(gadget::InputManager* inputMgr)
{
   new gadget::DeviceConstructor<gadget::P5GloveWrapper>(inputMgr);
}

}

namespace gadget
{

P5GloveWrapper::P5GloveWrapper()
   : mGlove(NULL)
   , mAnalogP5(5)
   , mDigitalP5(3)
   , mPositionP5(1)
   , mExitFlag(false)
{
   mAnalogP5[0] = 0.0f;
   mAnalogP5[1] = 0.0f;
   mAnalogP5[2] = 0.0f;
   mAnalogP5[3] = 0.0f;
   mAnalogP5[5] = 0.0f;
}

P5GloveWrapper::~P5GloveWrapper()
{
   stopSampling();      // Stop the glove

   if ( NULL != mGlove )
   {
      delete mGlove;       // Delete the glove
      mGlove = NULL;
   }
}

bool P5GloveWrapper::config(jccl::ConfigElementPtr e)
{
   if ( ! (Input::config(e) /*&& Glove::config(e)*/ ) )
   {
      return false;
   }

   mGloveNumber = e->getProperty<int>("number");

   vprASSERT(mThread == NULL);      // This should have been set by Input(c)
   mGlove = new P5GloveStandalone();
   return true;
}

std::string P5GloveWrapper::getElementType()
{
   return "p5_glove";
}

bool P5GloveWrapper::startSampling()
{
   bool started(false);

   if ( mThread == NULL )
   {
      vprDEBUG(gadgetDBG_INPUT_MGR, vprDBG_STATE_LVL)
         << "[p5glove] Begin sampling\n" << vprDEBUG_FLUSH;

      int max_attempts(0);
      bool result(false);

      while ( result == false && max_attempts < 5 )
      {
         vprDEBUG(gadgetDBG_INPUT_MGR, vprDBG_STATE_LVL)
            << "[p5glove] Connecting to glove number " << mGloveNumber
            << "...\n" << vprDEBUG_FLUSH;
         result = mGlove->connectToHardware(mGloveNumber);

         if ( result == false )
         {
            vprDEBUG(gadgetDBG_INPUT_MGR, vprDBG_CRITICAL_LVL)
               << "[p5glove] ERROR: Can't open or it is already opened."
               << vprDEBUG_FLUSH;
            vpr::System::usleep(14500);
            ++max_attempts;
         }
      }

      if ( result )
      {
         vprDEBUG(gadgetDBG_INPUT_MGR, vprDBG_STATE_LVL)
            << "[p5glove] Successfully connected, Now sampling dataglove "
            << "data.\n" << vprDEBUG_FLUSH;

         // Create a new thread to handle the control and set exit flag to
         // false.
         mExitFlag = false;
         vprDEBUG(gadgetDBG_INPUT_MGR, vprDBG_STATE_LVL)
            << "[p5glove] Spawning control thread.\n" << vprDEBUG_FLUSH;

         try
         {
            mThread =
               new vpr::Thread(boost::bind(&P5GloveWrapper::controlLoop,
                                           this));
            mActive = true;
            started = true;

            vprDEBUG(gadgetDBG_INPUT_MGR, vprDBG_STATE_LVL)
               << "[p5glove] P5Glove is active.\n" << vprDEBUG_FLUSH;
         }
         catch (vpr::Exception& ex)
         {
            vprDEBUG(gadgetDBG_INPUT_MGR, vprDBG_CRITICAL_LVL)
               << clrOutBOLD(clrRED, "ERROR")
               << ": Failed to spawn thread for P5 Glove driver!\n"
               << vprDEBUG_FLUSH;
            vprDEBUG_NEXT(gadgetDBG_INPUT_MGR, vprDBG_CRITICAL_LVL)
               << ex.what() << std::endl << vprDEBUG_FLUSH;
            started = false;
         }
      }
   }

   return started;
}

void P5GloveWrapper::controlLoop()
{
   vprDEBUG(gadgetDBG_INPUT_MGR, vprDBG_STATE_LVL)
      << "[p5glove] Entered control thread\n" << vprDEBUG_FLUSH;

   // Go until mExitFlag is set to true
   while ( ! mExitFlag )
   {
      sample();
   }
}

bool P5GloveWrapper::sample()
{
   P5GloveStandalone::Record rec;
   bool sample_read = mGlove->readRecordsFromHardware(rec);

   if ( sample_read )
   {
// Furst, we set the flexion of the gloves
      mAnalogP5[0] = rec.thumb;     // Thumb (0.0 - 1.0)
      mAnalogP5[1] = rec.index;     // Index
      mAnalogP5[2] = rec.middle;    // Middle
      mAnalogP5[3] = rec.ring;     // Ring
      mAnalogP5[4] = rec.pinky;    // Pinky
      addAnalogSample(mAnalogP5);
      swapAnalogBuffers();

// Then, we define the buttons of the glove
      mDigitalP5[0] = rec.buttonA;
      mDigitalP5[1] = rec.buttonB;
      mDigitalP5[2] = rec.buttonC;
      addDigitalSample(mDigitalP5);
      swapDigitalBuffers();

// Finally, we set the position of the glove ...
      const gmtl::AxisAnglef rotation(rec.rotationAngle, rec.rotationAxis[0],
                                      rec.rotationAxis[1],
                                      rec.rotationAxis[2]);
      const gmtl::Vec3f translation(static_cast<float>(rec.position[0]),
                                    static_cast<float>(rec.position[1]),
                                    static_cast<float>(rec.position[2]));
      gmtl::Matrix44f position = gmtl::makeTrans<gmtl::Matrix44f>(translation);
      position = position * gmtl::make<gmtl::Matrix44f>(rotation);
      mPositionP5[0].mPosData = position;
      addPositionSample(mPositionP5);
      swapPositionBuffers();
   }

   return true;
}

void P5GloveWrapper::updateData()
{
   swapAnalogBuffers();
   swapDigitalBuffers();
   swapPositionBuffers();
}

bool P5GloveWrapper::stopSampling()
{
   if ( mThread != NULL )
   {
      mExitFlag = true;

      //Wait for thread to exit normally after setting exit flag
      mThread->join();
      delete mThread;
      mThread = NULL;

      vprDEBUG(gadgetDBG_INPUT_MGR, vprDBG_CONFIG_LVL)
         << "[p5glove] stopping P5Glove.." << std::endl << vprDEBUG_FLUSH;
   }

   return true;
}

} // End of gadget namespace
<|endoftext|>