diff --git a/apps/speedTest/Main.cpp b/apps/speedTest/Main.cpp index 4423b1f2e41c6..efa2a4b9d8d21 100644 --- a/apps/speedTest/Main.cpp +++ b/apps/speedTest/Main.cpp @@ -36,6 +36,7 @@ */ #include +#include #include "dart/dynamics/Skeleton.h" #include "dart/dynamics/DegreeOfFreedom.h" diff --git a/dart/common/Signal.h b/dart/common/Signal.h new file mode 100644 index 0000000000000..82f933a85e1a1 --- /dev/null +++ b/dart/common/Signal.h @@ -0,0 +1,430 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COMMON_SIGNAL_H_ +#define DART_COMMON_SIGNAL_H_ + +#include +#include +#include +#include +#include +#include + +namespace dart { +namespace common { + +namespace signal { +namespace detail { + +/// class ConnectionBodyBase +class ConnectionBodyBase +{ +public: + /// Constructor + ConnectionBodyBase() : mIsConnected(true) {} + + /// Destructor + virtual ~ConnectionBodyBase() {} + + /// Disconnect + void disconnect() { mIsConnected = false; } + + /// Get true if this connection body is connected to the signal + bool isConnected() const { return mIsConnected; } + +protected: + /// Connection flag + bool mIsConnected; +}; + +/// class ConnectionBody +template +class ConnectionBody : public ConnectionBodyBase +{ +public: + /// Constructor given slot + ConnectionBody(const SlotType& _slot) : ConnectionBodyBase(), mSlot(_slot) {} + + /// Move constructor given slot + ConnectionBody(SlotType&& _slot) + : ConnectionBodyBase(), mSlot(std::forward(_slot)) {} + + /// Destructor + virtual ~ConnectionBody() {} + + /// Get slot + const SlotType& getSlot() { return mSlot; } + +private: + /// Slot + SlotType mSlot; +}; + +/// DefaultCombiner -- return the last result +template +struct DefaultCombiner +{ + typedef T result_type; + + template + static T process(InputIterator first, InputIterator last) + { + // If there are no slots to call, just return the + // default-constructed value + if (first == last) + return T(); + + return *(--last); + } +}; + +} // namespace detail +} // namespace signal + +/// class Connection +class Connection +{ +public: + /// Default constructor + Connection() {} + + /// Copy constructor + Connection(const Connection& _other) + : mWeakConnectionBody(_other.mWeakConnectionBody) {} + + /// Move constructor + Connection(Connection&& _other) + : mWeakConnectionBody(std::move(_other.mWeakConnectionBody)) {} + + /// Constructor given connection body + Connection( + const std::weak_ptr& _connectionBody) + : mWeakConnectionBody(_connectionBody) {} + + /// Move constructor given connection body + Connection( + std::weak_ptr&& _connectionBody) + : mWeakConnectionBody(std::move(_connectionBody)) {} + + /// Destructor + virtual ~Connection() {} + + /// Get true if the slot is connected + bool isConnected() const + { + std::shared_ptr + connectionBody(mWeakConnectionBody.lock()); + + if (nullptr == connectionBody) + return false; + + return connectionBody->isConnected(); + } + + /// Disconnect the connection + void disconnect() const + { + std::shared_ptr + connectionBody(mWeakConnectionBody.lock()); + + if (nullptr == connectionBody) + return; + + connectionBody->disconnect(); + } + +private: + /// Weak pointer to connection body in the signal + std::weak_ptr mWeakConnectionBody; +}; + +/// class ScopedConnection +class ScopedConnection : public Connection +{ +public: + /// Default constructor + ScopedConnection(const Connection& _other) : Connection(_other) {} + + /// Move constructor + ScopedConnection(Connection&& _other) : Connection(std::move(_other)) {} + + /// Destructor + virtual ~ScopedConnection() { disconnect(); } +}; + +template class Combiner = signal::detail::DefaultCombiner> +class Signal; + +/// Signal implements a signal/slot mechanism +template class Combiner> +class Signal<_Res(_ArgTypes...), Combiner> +{ +public: + using ResultType = _Res; + using SlotType = std::function; + using SignalType = Signal<_Res(_ArgTypes...), Combiner>; + + using ConnectionBodyType = signal::detail::ConnectionBody; + using ConnectionSetType + = std::set, + std::owner_less>>; + + /// Constructor + Signal() {} + + /// Destructor + virtual ~Signal() { disconnectAll(); } + + /// Connect a slot to the signal + Connection connect(const SlotType& _slot) + { + auto newConnectionBody = std::make_shared(_slot); + mConnectionBodies.insert(newConnectionBody); + + return Connection(std::move(newConnectionBody)); + } + + /// Connect a slot to the signal + Connection connect(SlotType&& _slot) + { + auto newConnectionBody + = std::make_shared(std::forward(_slot)); + mConnectionBodies.insert(newConnectionBody); + + return Connection(std::move(newConnectionBody)); + } + + /// Disconnect given connection + void disconnect(const Connection& _connection) const + { + _connection.disconnect(); + } + + /// Disconnect all the connections + void disconnectAll() { mConnectionBodies.clear(); } + + /// Cleanup all the disconnected connections + void clenaupConnections() + { + // Counts all the connected conection bodies + for (const auto& connectionBody : mConnectionBodies) + { + if (!connectionBody->isConnected()) + mConnectionBodies.erase(connectionBody); + } + } + + /// Get the number of connections + size_t getNumConnections() const + { + size_t numConnections = 0; + + // Counts all the connected conection bodies + for (const auto& connectionBody : mConnectionBodies) + { + if (connectionBody->isConnected()) + ++numConnections; + } + + return numConnections; + } + + /// Raise the signal + template + ResultType raise(ArgTypes&&... _args) + { + std::vector res(mConnectionBodies.size()); + auto resIt = res.begin(); + + for (auto itr = mConnectionBodies.begin(); itr != mConnectionBodies.end(); ) + { + if ((*itr)->isConnected()) + { + *(resIt++) = (*itr)->getSlot()(std::forward(_args)...); + ++itr; + } + else + { + mConnectionBodies.erase(itr++); + } + } + + return Combiner::process(res.begin(), resIt); + } + + /// Raise the signal + template + ResultType operator()(ArgTypes&&... _args) + { + return raise(std::forward(_args)...); + } + +private: + /// Connection set + ConnectionSetType mConnectionBodies; +}; + +/// Signal implements a signal/slot mechanism for the slots don't return a value +template +class Signal +{ +public: + using SlotType = std::function; + using SignalType = Signal; + + using ConnectionBodyType = signal::detail::ConnectionBody; + using ConnectionSetType + = std::set, + std::owner_less>>; + + /// Constructor + Signal() {} + + /// Destructor + virtual ~Signal() { disconnectAll(); } + + /// Connect a slot to the signal + Connection connect(const SlotType& _slot) + { + auto newConnectionBody = std::make_shared(_slot); + mConnectionBodies.insert(newConnectionBody); + + return Connection(std::move(newConnectionBody)); + } + + /// Connect a slot to the signal + Connection connect(SlotType&& _slot) + { + auto newConnectionBody + = std::make_shared(std::forward(_slot)); + mConnectionBodies.insert(newConnectionBody); + + return Connection(std::move(newConnectionBody)); + } + + /// Disconnect given connection + void disconnect(const Connection& _connection) const + { + _connection.disconnect(); + } + + /// Disconnect all the connections + void disconnectAll() { mConnectionBodies.clear(); } + + /// Cleanup all the disconnected connections + void clenaupConnections() + { + // Counts all the connected conection bodies + for (const auto& connectionBody : mConnectionBodies) + { + if (!connectionBody->isConnected()) + mConnectionBodies.erase(connectionBody); + } + } + + /// Get the number of connections + size_t getNumConnections() const + { + size_t numConnections = 0; + + // Counts all the connected conection bodies + for (const auto& connectionBody : mConnectionBodies) + { + if (connectionBody->isConnected()) + ++numConnections; + } + + return numConnections; + } + + /// Raise the signal + template + void raise(ArgTypes&&... _args) + { + for (auto itr = mConnectionBodies.begin(); itr != mConnectionBodies.end(); ) + { + if ((*itr)->isConnected()) + { + (*itr)->getSlot()(std::forward(_args)...); + ++itr; + } + else + { + mConnectionBodies.erase(itr++); + } + } + } + + /// Raise the signal + template + void operator()(ArgTypes&&... _args) + { + raise(std::forward(_args)...); + } + +private: + /// Connection set + ConnectionSetType mConnectionBodies; +}; + +/// SlotRegister can be used as a public member for connecting slots to a +/// private Signal member. In this way you won't have to write forwarding +/// connect/disconnect boilerplate for your classes. +template +class SlotRegister +{ +public: + using SlotType = typename T::SlotType; + using SignalType = typename T::SignalType; + + /// Constructor given signal + SlotRegister(typename T::SignalType& _signal) : mSignal(_signal) {} + + /// Connect a slot to the signal + Connection connect(const SlotType& _slot) { return mSignal.connect(_slot); } + +private: + /// Signal + typename T::SignalType& mSignal; +}; + +} // namespace common +} // namespace dart + +#endif // DART_COMMON_SIGNAL_H_ + diff --git a/dart/dynamics/Entity.cpp b/dart/dynamics/Entity.cpp index 7c063485f279f..8f79c2ef34d7d 100644 --- a/dart/dynamics/Entity.cpp +++ b/dart/dynamics/Entity.cpp @@ -52,6 +52,18 @@ Entity::Entity(Frame* _refFrame, const std::string& _name, bool _quiet) mNeedTransformUpdate(true), mNeedVelocityUpdate(true), mNeedAccelerationUpdate(true), + mFrameChangedSignal(), + mNameChangedSignal(), + mVizShapeAddedSignal(), + mTransformUpdatedSignal(), + mVelocityChangedSignal(), + mAccelerationChangedSignal(), + onFrameChanged(mFrameChangedSignal), + onNameChanged(mNameChangedSignal), + onVizShapeAdded(mVizShapeAddedSignal), + onTransformUpdated(mTransformUpdatedSignal), + onVelocityChanged(mVelocityChangedSignal), + onAccelerationChanged(mAccelerationChangedSignal), mAmQuiet(_quiet) { changeParentFrame(_refFrame); @@ -64,9 +76,15 @@ Entity::~Entity() } //============================================================================== -const std::string& Entity::setName(const std::string &_name) +const std::string& Entity::setName(const std::string& _name) { + if (mName == _name) + return mName; + + const std::string oldName = mName; mName = _name; + mNameChangedSignal.raise(this, oldName, mName); + return mName; } @@ -80,6 +98,8 @@ const std::string& Entity::getName() const void Entity::addVisualizationShape(Shape* _p) { mVizShapes.push_back(_p); + + mVizShapeAddedSignal.raise(this, _p); } //============================================================================== @@ -153,6 +173,10 @@ bool Entity::isQuiet() const void Entity::notifyTransformUpdate() { mNeedTransformUpdate = true; + + // The actual transform hasn't updated yet. But when its getter is called, + // the transformation will be updated automatically. + mTransformUpdatedSignal.raise(this); } //============================================================================== @@ -165,6 +189,10 @@ bool Entity::needsTransformUpdate() const void Entity::notifyVelocityUpdate() { mNeedVelocityUpdate = true; + + // The actual velocity hasn't updated yet. But when its getter is called, + // the velocity will be updated automatically. + mVelocityChangedSignal.raise(this); } //============================================================================== @@ -177,6 +205,10 @@ bool Entity::needsVelocityUpdate() const void Entity::notifyAccelerationUpdate() { mNeedAccelerationUpdate = true; + + // The actual acceleration hasn't updated yet. But when its getter is called, + // the acceleration will be updated automatically. + mAccelerationChangedSignal.raise(this); } //============================================================================== @@ -188,32 +220,31 @@ bool Entity::needsAccelerationUpdate() const //============================================================================== void Entity::changeParentFrame(Frame* _newParentFrame) { - if(!mAmQuiet) + if (mParentFrame == _newParentFrame) + return; + + const Frame* oldParentFrame = mParentFrame; + + if (!mAmQuiet && nullptr != mParentFrame) { - if(mParentFrame) + EntityPtrSet::iterator it = mParentFrame->mChildEntities.find(this); + if (it != mParentFrame->mChildEntities.end()) { - EntityPtrSet::iterator it = mParentFrame->mChildEntities.find(this); - if(it != mParentFrame->mChildEntities.end()) - { - mParentFrame->mChildEntities.erase(it); - mParentFrame->processRemovedEntity(this); - } + mParentFrame->mChildEntities.erase(it); + mParentFrame->processRemovedEntity(this); } } - if(NULL == _newParentFrame) - { - mParentFrame = NULL; - return; - } - mParentFrame =_newParentFrame; - if(!mAmQuiet) + + if (!mAmQuiet && nullptr != mParentFrame) { mParentFrame->mChildEntities.insert(this); mParentFrame->processNewEntity(this); notifyTransformUpdate(); } + + mFrameChangedSignal.raise(this, oldParentFrame, mParentFrame); } //============================================================================== diff --git a/dart/dynamics/Entity.h b/dart/dynamics/Entity.h index c9d6a5b9e5c2a..12540b3c99792 100644 --- a/dart/dynamics/Entity.h +++ b/dart/dynamics/Entity.h @@ -41,6 +41,7 @@ #include #include +#include "dart/common/Signal.h" #include "dart/dynamics/Shape.h" namespace dart { @@ -68,9 +69,20 @@ class Entity public: friend class Frame; + using EntitySignal = common::Signal; + using FrameChangedSignal + = common::Signal; + using NameChangedSignal + = common::Signal; + using VizShapeAddedSignal + = common::Signal; + /// Constructor for typical usage - explicit Entity(Frame* _refFrame, const std::string& _name, - bool _quiet); + explicit Entity(Frame* _refFrame, const std::string& _name, bool _quiet); /// Destructor virtual ~Entity(); @@ -151,6 +163,49 @@ class Entity /// Does this Entity need an Acceleration update mutable bool mNeedAccelerationUpdate; + /// Frame changed signal + FrameChangedSignal mFrameChangedSignal; + + /// Name changed signal + NameChangedSignal mNameChangedSignal; + + /// Visualization added signal + VizShapeAddedSignal mVizShapeAddedSignal; + + /// Transform changed signal + EntitySignal mTransformUpdatedSignal; + + /// Velocity changed signal + EntitySignal mVelocityChangedSignal; + + /// Acceleration changed signal + EntitySignal mAccelerationChangedSignal; + +public: + //---------------------------------------------------------------------------- + /// \{ \name Slot registers + //---------------------------------------------------------------------------- + + /// Slot register for frame changed signal + common::SlotRegister onFrameChanged; + + /// Slot register for name changed signal + common::SlotRegister onNameChanged; + + /// Slot register for visualization changed signal + common::SlotRegister onVizShapeAdded; + + /// Slot register for transform updated signal + common::SlotRegister onTransformUpdated; + + /// Slot register for velocity updated signal + common::SlotRegister onVelocityChanged; + + /// Slot register for acceleration updated signal + common::SlotRegister onAccelerationChanged; + + /// \} + private: /// Whether or not this Entity is set to be quiet const bool mAmQuiet; @@ -163,8 +218,7 @@ class Detachable : public virtual Entity { public: /// Constructor - explicit Detachable(Frame* _refFrame, const std::string& _name, - bool _quiet); + explicit Detachable(Frame* _refFrame, const std::string& _name, bool _quiet); /// Allows the user to change the parent Frame of this Entity virtual void setParentFrame(Frame* _newParentFrame); diff --git a/dart/dynamics/Frame.cpp b/dart/dynamics/Frame.cpp index da223d1ccd0db..ae48d4dfbc7ee 100644 --- a/dart/dynamics/Frame.cpp +++ b/dart/dynamics/Frame.cpp @@ -474,6 +474,8 @@ void Frame::notifyTransformUpdate() for(Entity* entity : mChildEntities) entity->notifyTransformUpdate(); + + mTransformUpdatedSignal.raise(this); } //============================================================================== @@ -489,6 +491,8 @@ void Frame::notifyVelocityUpdate() for(Entity* entity : mChildEntities) entity->notifyVelocityUpdate(); + + mVelocityChangedSignal.raise(this); } //============================================================================== @@ -502,11 +506,16 @@ void Frame::notifyAccelerationUpdate() for(Entity* entity : mChildEntities) entity->notifyAccelerationUpdate(); + + mAccelerationChangedSignal.raise(this); } //============================================================================== void Frame::changeParentFrame(Frame* _newParentFrame) { + if (mParentFrame == _newParentFrame) + return; + if(_newParentFrame) { if(_newParentFrame->descendsFrom(this)) diff --git a/dart/dynamics/Frame.h b/dart/dynamics/Frame.h index 4d90924836887..82d32c88f0b97 100644 --- a/dart/dynamics/Frame.h +++ b/dart/dynamics/Frame.h @@ -232,22 +232,23 @@ class Frame : public virtual Entity //-------------------------------------------------------------------------- // Render this Frame as well as any Entities it contains - virtual void draw(renderer::RenderInterface *_ri = NULL, - const Eigen::Vector4d &_color = Eigen::Vector4d::Ones(), - bool _useDefaultColor = true, int _depth = 0) const; + virtual void draw( + renderer::RenderInterface *_ri = NULL, + const Eigen::Vector4d &_color = Eigen::Vector4d::Ones(), + bool _useDefaultColor = true, int _depth = 0) const override; /// Notify this Frame and all its children that its pose has changed - virtual void notifyTransformUpdate(); + virtual void notifyTransformUpdate() override; /// Notify this Frame and all its children that its velocity has changed - virtual void notifyVelocityUpdate(); + virtual void notifyVelocityUpdate() override; /// Notify this Frame and all its children that its acceleration has changed - virtual void notifyAccelerationUpdate(); + virtual void notifyAccelerationUpdate() override; protected: // Documentation inherited - virtual void changeParentFrame(Frame* _newParentFrame); + virtual void changeParentFrame(Frame* _newParentFrame) override; /// Called during a parent Frame change to allow extensions of the Frame class /// to handle new children in customized ways. This function is a no op unless diff --git a/unittests/testSignal.cpp b/unittests/testSignal.cpp new file mode 100644 index 0000000000000..d12e5506d791a --- /dev/null +++ b/unittests/testSignal.cpp @@ -0,0 +1,337 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +#include "dart/dart.h" + +using namespace std; +using namespace Eigen; +using namespace dart; +using namespace common; +using namespace dynamics; +using namespace simulation; + +static int callCount0 = 0; +static int callCount1 = 0; +static int callCount2 = 0; + +//============================================================================== +void foo0() { callCount0++; } +void foo1(int /*_val*/) { callCount1++; } +void foo2(int /*_val1*/, float /*_val2*/) { callCount2++; } +double foo3() { return 10.0; } + +//============================================================================== +class Viewer +{ +public: + static void onSignal1Static(int /*_val*/) { callCount1++; } + static void onSignal2Static(int /*_val1*/, float /*_val2*/) { callCount2++; } + void onSignal1(int /*_val*/) { callCount1++; } + void onSignal2(int /*_val1*/, float /*_val2*/) { callCount2++; } +}; + +//============================================================================== +TEST(Signal, Basic) +{ + Signal signal0; + Signal signal1; + Signal signal2; + Signal signal3; + + Connection connection0 = signal0.connect(&foo0); + Connection connection1 = signal1.connect(&foo1); + Connection connection2 = signal2.connect(&foo2); + Connection connection3 = signal3.connect(&foo3); + + EXPECT_EQ(signal0.getNumConnections(), 1); + EXPECT_EQ(signal1.getNumConnections(), 1); + EXPECT_EQ(signal2.getNumConnections(), 1); + EXPECT_EQ(signal3.getNumConnections(), 1); + + EXPECT_TRUE(connection0.isConnected()); + EXPECT_TRUE(connection1.isConnected()); + EXPECT_TRUE(connection2.isConnected()); + EXPECT_TRUE(connection3.isConnected()); + + connection0.disconnect(); + connection1.disconnect(); + connection2.disconnect(); + connection3.disconnect(); + + EXPECT_EQ(signal0.getNumConnections(), 0); + EXPECT_EQ(signal1.getNumConnections(), 0); + EXPECT_EQ(signal2.getNumConnections(), 0); + EXPECT_EQ(signal3.getNumConnections(), 0); + + signal0(); + + EXPECT_FALSE(connection0.isConnected()); + EXPECT_FALSE(connection1.isConnected()); + EXPECT_FALSE(connection2.isConnected()); + EXPECT_FALSE(connection3.isConnected()); +} + +//============================================================================== +TEST(Signal, NonStaticMemberFunction) +{ + Signal signal1; + Signal signal2; + Viewer viewer; + + // Connect static member function + signal1.connect(&Viewer::onSignal1Static); + signal2.connect(&Viewer::onSignal2Static); + + // Connect non-static member function + using placeholders::_1; + using placeholders::_2; + signal1.connect(bind(&Viewer::onSignal1, &viewer, _1)); + signal2.connect(bind(&Viewer::onSignal2, &viewer, _1, _2)); + + // The signal should have the maximum number of listeners + EXPECT_EQ(signal1.getNumConnections(), 2); + EXPECT_EQ(signal2.getNumConnections(), 2); + + // Check the number of calls + callCount1 = 0; + callCount2 = 0; + signal1.raise(0); + signal2.raise(0, 0); + EXPECT_EQ(callCount1, 2); + EXPECT_EQ(callCount2, 2); +} + +//============================================================================== +TEST(Signal, ScopedConnection) +{ + Signal signal; + Connection c = signal.connect(foo1); + EXPECT_EQ(signal.getNumConnections(), 1); + + { + ScopedConnection sc(signal.connect(foo1)); + EXPECT_EQ(signal.getNumConnections(), 2); + } + + EXPECT_EQ(signal.getNumConnections(), 1); +} + +//============================================================================== +TEST(Signal, ConnectionLifeTime) +{ + Signal* signal = new Signal(); + + Connection connection1 = signal->connect(foo1); + EXPECT_TRUE(connection1.isConnected()); + + Connection connection2 = signal->connect(foo1); + EXPECT_TRUE(connection2.isConnected()); + + { + ScopedConnection scopedConnection(connection2); + EXPECT_TRUE(scopedConnection.isConnected()); + EXPECT_TRUE(connection2.isConnected()); + // scopedConnection disconnected connection2 when it was destroyed. + } + EXPECT_FALSE(connection2.isConnected()); + + delete signal; + + // Although the signal is destroyed, its connection still works. For those + // connections, isConnected() returns false. + EXPECT_FALSE(connection1.isConnected()); +} + +//============================================================================== +float product(float x, float y) { return x * y; } +float quotient(float x, float y) { return x / y; } +float sum(float x, float y) { return x + y; } +float difference(float x, float y) { return x - y; } + +// combiner which returns the maximum value returned by all slots +template +struct signal_maximum +{ + typedef T result_type; + + template + static T process(InputIterator first, InputIterator last) + { + // If there are no slots to call, just return the + // default-constructed value + if (first == last) + return T(); + + T max_value = *first++; + + while (first != last) + { + if (max_value < *first) + max_value = *first; + ++first; + } + + return max_value; + } +}; + +// combiner which returns the maximum value returned by all slots +template +struct signal_sum +{ + typedef T result_type; + + template + static T process(InputIterator first, InputIterator last) + { + // If there are no slots to call, just return the + // default-constructed value + if (first == last) + return T(); + + T sum = *first; + first++; + + while (first != last) + { + sum += *first; + ++first; + } + + return sum; + } +}; + +//============================================================================== +TEST(Signal, ReturnValues) +{ + const float tol = 1e-6; + + const float a = 5.0f; + const float b = 3.0f; + + std::vector res(4); + res[0] = product(a, b); + res[1] = quotient(a, b); + res[2] = sum(a, b); + res[3] = difference(a, b); + + // signal_maximum + Signal signal1; + signal1.connect(&product); + signal1.connect("ient); + signal1.connect(&sum); + signal1.connect(&difference); + EXPECT_EQ(signal1(5, 3), *std::max_element(res.begin(), res.end())); + + // signal_sum + Signal signal2; + signal2.connect(&product); + signal2.connect("ient); + signal2.connect(&sum); + signal2.connect(&difference); + EXPECT_NEAR(signal2(5, 3), std::accumulate(res.begin(), res.end(), 0.0), tol); +} + +//============================================================================== +void frameChangecCallback(const Entity* _entity, + const Frame* _oldParentFrame, + const Frame* _newParentFrame) +{ + assert(_entity); + + std::string oldFrameName + = _oldParentFrame == nullptr ? "(empty)" : _oldParentFrame->getName(); + std::string newFrameName + = _newParentFrame == nullptr ? "(empty)" : _newParentFrame->getName(); + + std::cout << "[" << _entity->getName() << "]: " + << oldFrameName << " --> " << newFrameName << std::endl; +} + +//============================================================================== +void vizShapeAddedCallback(const Entity* _entity, + const Shape* _newShape) +{ + assert(_entity); + + std::cout << "[" << _entity->getName() << "]: New shape '" + << _newShape->getShapeType() << "' type added." << std::endl; +} + +//============================================================================== +TEST(Signal, FrameSignals) +{ + Isometry3d tf1(Isometry3d::Identity()); + tf1.translate(Vector3d(0.1,-0.1,0)); + + Isometry3d tf2(Isometry3d::Identity()); + tf2.translate(Vector3d(0,0.1,0)); + tf2.rotate(AngleAxisd(45.0*M_PI/180.0, Vector3d(1,0,0))); + + Isometry3d tf3(Isometry3d::Identity()); + tf3.translate(Vector3d(0,0,0.1)); + tf3.rotate(AngleAxisd(60*M_PI/180.0, Vector3d(0,1,0))); + + SimpleFrame F1(Frame::World(), "F1", tf1); + SimpleFrame F2(&F1, "F2", tf2); + SimpleFrame F3(&F2, "F3", tf3); + + Connection cf1 = F1.onFrameChanged.connect(frameChangecCallback); + Connection cf2 = F2.onFrameChanged.connect(frameChangecCallback); + ScopedConnection cf3(F3.onFrameChanged.connect(frameChangecCallback)); + + Connection cv1 = F1.onVizShapeAdded.connect(vizShapeAddedCallback); + Connection cv2 = F2.onVizShapeAdded.connect(vizShapeAddedCallback); + ScopedConnection cv3(F3.onVizShapeAdded.connect(vizShapeAddedCallback)); + + F1.addVisualizationShape(new BoxShape(Vector3d(0.05, 0.05, 0.02))); + F2.addVisualizationShape(new BoxShape(Vector3d(0.05, 0.05, 0.02))); + F3.addVisualizationShape(new BoxShape(Vector3d(0.05, 0.05, 0.02))); + + F3.setParentFrame(&F1); +} + +//============================================================================== +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} +