<commit_before>#include "AnimationLoader.hpp"
#include <assimp/Importer.hpp>
#include <assimp/scene.h>
#include <assimp/postprocess.h>
#include <Core/Log/Log.hpp>
#include <vector>
#include <Core/Animation/Pose/Pose.hpp>
#include <Core/Utils/Graph/AdjacencyList.hpp>
#include <Core/Animation/Handle/HandleWeight.hpp>
#include <iostream>
#include <map>
#include <string.h>
#include <set>
#include <Core/Animation/Pose/PoseOperation.hpp>
#include <Core/Mesh/MeshTypes.hpp>

namespace AnimationPlugin
{
    namespace AnimationLoader
    {
        struct aiStringComparator
        {
            bool operator()(const aiString& left, const aiString& right)
            {
                return strcmp(left.C_Str(), right.C_Str()) < 0;
            }
        };
        typedef std::map<aiString, int, aiStringComparator> BoneMap;

        void recursiveSkeletonRead(const aiNode* node, aiMatrix4x4 accTransform, BoneMap& bones, AnimationData& data, int parent);
        void assimpToCore(const aiMatrix4x4& inMatrix, Ra::Core::Transform &outMatrix);
        void assimpToCore(const aiVector3D& inTranslation, const aiQuaternion& inRotation, const aiVector3D& inScaling, Ra::Core::Transform& outTransform);
        void assimpToCore(const aiQuaternion& inQuat, Ra::Core::Quaternion& outQuat);
        void assimpToCore(const aiVector3D& inVec, Ra::Core::Vector3& outVec);
        void getUniqueKeyTimes(aiAnimation* animation, std::vector<double> &times);
        void getTransformFromKey(const aiNodeAnim* key, int i, Ra::Core::Transform& keyTransform);

        AnimationData loadFile( const std::string& name, const FancyMeshPlugin::MeshLoadingInfo& info)
        {
            Assimp::Importer importer;
            const aiScene* scene = importer.ReadFile(name, aiProcess_Triangulate | aiProcess_JoinIdenticalVertices | aiProcess_GenSmoothNormals |
                                                      aiProcess_SortByPType | aiProcess_FixInfacingNormals | aiProcess_CalcTangentSpace | aiProcess_GenUVCoords);

            AnimationData animData;
            animData.hasLoaded = false;

            if (scene == NULL)
            {
                LOG( logERROR ) << "Error while loading file \"" << name << "\" : " << importer.GetErrorString() << ".";
                return animData;
            }
            if (info.index < 0 || info.index >= scene->mNumMeshes)
            {
                LOG(logDEBUG) << "Invalid mesh index: " << info.index << " requested, but " << scene->mNumMeshes << " meshes have been found";
                return animData;
            }

            // skeleton loading
            aiMesh* mesh = scene->mMeshes[info.index];
            if (mesh->mNumBones == 0)
            {
                LOG(logDEBUG) << "Mesh #" << info.index << ": no skeleton found.";
                return animData;
            }

            std::string skelName = std::string(mesh->mName.C_Str()) + "_skeleton";
            animData.name = skelName;
            int vertexCount = 0;
            for (int i = 0; i < info.vertexMap.size(); i++)
            {
                if (info.vertexMap[i] >= vertexCount)
                    vertexCount = info.vertexMap[i] + 1;
            }

            BoneMap boneMap; // first: name of the boneNode, second: index of the bone in the hierarchy / pose
            animData.weights.resize(vertexCount, mesh->mNumBones);

            for (int i = 0; i < mesh->mNumBones; i++)
            {
                boneMap[mesh->mBones[i]->mName] = -1; // the true index will get written during the recursive read of the scene
                for (int j = 0; j < mesh->mBones[i]->mNumWeights; j++)
                {
                    aiVertexWeight vertexWeight = mesh->mBones[i]->mWeights[j];
                    int id = info.vertexMap[vertexWeight.mVertexId];
                    animData.weights.coeffRef(id, i) = vertexWeight.mWeight;
                }
            }

            // find the bone nodes and create the corresponding skeleton
            recursiveSkeletonRead(scene->mRootNode, aiMatrix4x4(), boneMap, animData, -1);

            // Store the names of each bone
            animData.boneNames.resize(boneMap.size());
            for (std::pair<aiString, int> p : boneMap)
            {
                CORE_ASSERT(p.second < boneMap.size(), "Invalid bone index");
                animData.boneNames[p.second] = std::string(p.first.C_Str());
            }
            LOG(logDEBUG) << "Found a skeleton of " << boneMap.size() << " bones";

            // animation loading
            LOG(logDEBUG) << "Found " << scene->mNumAnimations << " animations";

            animData.animations.resize(scene->mNumAnimations);
            for (int k = 0; k < scene->mNumAnimations; k++)
            {
                aiAnimation* animation = scene->mAnimations[k];
                int channelCount = animation->mNumChannels;
                int boneCount = boneMap.size();

                // Get the sorted set of key pose timestamps
                std::vector<double> timeSet;
                getUniqueKeyTimes(animation, timeSet);
                int keyCount = timeSet.size();

                // Allocate the poses
                std::vector<Ra::Core::Animation::Pose> poses;
                for (int i = 0; i < keyCount; i++)
                    poses.push_back(Ra::Core::Animation::Pose(boneCount));

                // Track which bones have an animation
                std::vector<bool> animatedBones( boneCount );
                for (int i = 0; i < boneCount; i++)
                    animatedBones[i] = false;

                // Add the animated bone transforms to the poses + interpolate when necessary
                for (int i = 0; i < channelCount; i++)
                {
                    aiNodeAnim* currentNodeAnim = animation->mChannels[i];
                    if (boneMap.find(currentNodeAnim->mNodeName) == boneMap.end()) // We should be able to ignore bones that do not affect the mesh
                        continue;
                    //CORE_ASSERT(boneMap.find(currentNodeAnim->mNodeName) != boneMap.end(), "Unknown bone channel");

                    int channelKeyCount = currentNodeAnim->mNumPositionKeys;
                    int boneIndex = boneMap[currentNodeAnim->mNodeName];
                    animatedBones[boneIndex] = true;

                    int channelKeyIndex = 0;
                    for (int j = 0; j < keyCount; j++)
                    {
                        double channelKeyTime = currentNodeAnim->mPositionKeys[channelKeyIndex].mTime;
                        if (channelKeyTime == timeSet[j])
                        {
                            Ra::Core::Transform keyTransform;
                            getTransformFromKey(currentNodeAnim, channelKeyIndex, keyTransform);
                            poses[j][boneIndex] = keyTransform;
                            if (channelKeyIndex < channelKeyCount - 1)
                                channelKeyIndex++;
                        }
                        else if (channelKeyIndex == 0 || channelKeyIndex == channelKeyCount - 1) // the first channel key is after the current key
                        {
                            Ra::Core::Transform keyTransform;
                            getTransformFromKey(currentNodeAnim, channelKeyIndex, keyTransform);
                            poses[j][boneIndex] = keyTransform;
                        }
                        else if (channelKeyTime > timeSet[j]) // the current key is between two channel keys
                        {
                            // interpolate between the previous and current channel key
                            Ra::Core::Transform previousKeyTransform;
                            Ra::Core::Transform nextKeyTransform;
                            Ra::Core::Transform keyTransform;
                            getTransformFromKey(currentNodeAnim, channelKeyIndex - 1, previousKeyTransform);
                            getTransformFromKey(currentNodeAnim, channelKeyIndex, nextKeyTransform);

                            double prevChannelKeyTime = currentNodeAnim->mPositionKeys[channelKeyIndex - 1].mTime;
                            Scalar t = (timeSet[j] - prevChannelKeyTime) / (channelKeyTime - prevChannelKeyTime);
                            Ra::Core::Animation::interpolateTransforms(previousKeyTransform, nextKeyTransform, t, keyTransform);

                            poses[j][boneIndex] = keyTransform;
                        }
                        else
                        {
                            CORE_ASSERT(false, "AnimationLoader.cpp: should not be there");
                        }

                        if (animData.hierarchy.isRoot(boneIndex))
                            poses[j][boneIndex] = animData.baseTransform * poses[j][boneIndex];
                    }
                }

                // add the non animated bone transforms to the poses
                for (int i = 0; i < boneCount; i++)
                {
                    if (!animatedBones[i])
                    {
                        for (int j = 0; j < keyCount; j++)
                        {
                            poses[j][i] = animData.pose[i];
                        }
                    }
                }

                // finally create the animation object
                Scalar animationRate = animation->mTicksPerSecond > 0.0 ? animation->mTicksPerSecond : 50.0;
                for (int i = 0; i < keyCount; i++)
                {
                    Scalar keyTime = timeSet[i] / animationRate;
                    animData.animations[k].addKeyPose(poses[i], keyTime);
                }
                animData.animations[k].normalize();
            }

            animData.hasLoaded = true;
            return animData;
        }

        void recursiveSkeletonRead(const aiNode* node, aiMatrix4x4 accTransform, BoneMap &boneMap, AnimationData& data, int parent)
        {
            aiMatrix4x4 currentTransform  = accTransform * node->mTransformation;
            bool isBoneNode = boneMap.find(node->mName) != boneMap.end();
            int currentIndex = parent;

            if (!isBoneNode && node->mNumChildren == 0 && parent != -1) // Catch the end bones
                isBoneNode = true;

            if (isBoneNode)
            {
                if (parent == -1)
                {
                    assimpToCore(accTransform, data.baseTransform);
                }

                // store the bone in the hierarchy
                currentIndex = data.hierarchy.addNode(parent);
                // store the index in the BoneMap
                boneMap[node->mName] = currentIndex;

                // store the transform for the bone
                Ra::Core::Transform tr;
                assimpToCore(currentTransform, tr);
                data.pose.push_back(tr);

                // initialize the transform for the child bones
                currentTransform = aiMatrix4x4();
            }

            for (int i = 0; i < node->mNumChildren; i++)
                recursiveSkeletonRead(node->mChildren[i], currentTransform, boneMap, data, currentIndex);
        }

        void getTransformFromKey(const aiNodeAnim* key, int i, Ra::Core::Transform& keyTransform)
        {
            aiVector3D keyPosition = key->mPositionKeys[i].mValue;
            aiQuaternion keyRotation = key->mRotationKeys[i].mValue;
            aiVector3D keyScaling = key->mScalingKeys[i].mValue;

            // convert the key to a transform matrix
            assimpToCore(keyPosition, keyRotation, keyScaling, keyTransform);
        }

        void getUniqueKeyTimes(aiAnimation* animation, std::vector<double>& times)
        {
            int channelCount = animation->mNumChannels;
            std::set<double> timeSet;
            for (int i = 0; i < channelCount; i++)
            {
                aiNodeAnim* currentNodeAnim = animation->mChannels[i];

                int channelKeyCount = currentNodeAnim->mNumRotationKeys;
                for (int j = 0; j < channelKeyCount; j++)
                {
                    const aiVectorKey& positionKey = currentNodeAnim->mPositionKeys[j];
                    const aiQuatKey& rotationKey = currentNodeAnim->mRotationKeys[j];
                    const aiVectorKey& scalingKey = currentNodeAnim->mScalingKeys[j];

                    CORE_ASSERT(positionKey.mTime == rotationKey.mTime && positionKey.mTime == scalingKey.mTime, "Invalid key times");

                    timeSet.insert(positionKey.mTime);
                }
            }
            std::copy(timeSet.begin(), timeSet.end(), std::back_inserter(times));
        }

        void assimpToCore( const aiMatrix4x4& inMatrix, Ra::Core::Transform& outMatrix )
        {
            for ( uint i = 0; i < 4; ++i )
            {
                for ( uint j = 0; j < 4; ++j )
                {
                    outMatrix(i, j) = inMatrix[i][j];
                }
            }
        }

        void assimpToCore(const aiQuaternion& inQuat, Ra::Core::Quaternion& outQuat)
        {
            outQuat = Ra::Core::Quaternion(inQuat.w, inQuat.x, inQuat.y, inQuat.z);
        }

        void assimpToCore(const aiVector3D& inVec, Ra::Core::Vector3& outVec)
        {
            outVec = Ra::Core::Vector3(inVec.x, inVec.y, inVec.z);
        }

        void assimpToCore(const aiVector3D &inTranslation, const aiQuaternion &inRotation, const aiVector3D &inScaling, Ra::Core::Transform& outTransform)
        {
            Ra::Core::Vector3 translation;
            Ra::Core::Vector3 scaling;
            Ra::Core::Quaternion rotation;
            assimpToCore(inTranslation, translation);
            assimpToCore(inScaling, scaling);
            assimpToCore(inRotation, rotation);
            outTransform.fromPositionOrientationScale(translation, rotation, scaling);
        }
    }
}
<commit_msg>Leaf bones weights problem fixed (MAYBE)<commit_after>#include "AnimationLoader.hpp"
#include <assimp/Importer.hpp>
#include <assimp/scene.h>
#include <assimp/postprocess.h>
#include <Core/Log/Log.hpp>
#include <vector>
#include <Core/Animation/Pose/Pose.hpp>
#include <Core/Utils/Graph/AdjacencyList.hpp>
#include <Core/Animation/Handle/HandleWeight.hpp>
#include <iostream>
#include <map>
#include <string.h>
#include <set>
#include <Core/Animation/Pose/PoseOperation.hpp>
#include <Core/Mesh/MeshTypes.hpp>

namespace AnimationPlugin
{
    namespace AnimationLoader
    {
        struct aiStringComparator
        {
            bool operator()(const aiString& left, const aiString& right)
            {
                return strcmp(left.C_Str(), right.C_Str()) < 0;
            }
        };
        typedef std::map<aiString, int, aiStringComparator> BoneMap;

        void recursiveSkeletonRead(const aiNode* node, aiMatrix4x4 accTransform, BoneMap& bones, AnimationData& data, int parent);
        void assimpToCore(const aiMatrix4x4& inMatrix, Ra::Core::Transform &outMatrix);
        void assimpToCore(const aiVector3D& inTranslation, const aiQuaternion& inRotation, const aiVector3D& inScaling, Ra::Core::Transform& outTransform);
        void assimpToCore(const aiQuaternion& inQuat, Ra::Core::Quaternion& outQuat);
        void assimpToCore(const aiVector3D& inVec, Ra::Core::Vector3& outVec);
        void getUniqueKeyTimes(aiAnimation* animation, std::vector<double> &times);
        void getTransformFromKey(const aiNodeAnim* key, int i, Ra::Core::Transform& keyTransform);
        void checkWeights( AnimationData& data );

        AnimationData loadFile( const std::string& name, const FancyMeshPlugin::MeshLoadingInfo& info)
        {
            Assimp::Importer importer;
            const aiScene* scene = importer.ReadFile(name, aiProcess_Triangulate | aiProcess_JoinIdenticalVertices | aiProcess_GenSmoothNormals |
                                                      aiProcess_SortByPType | aiProcess_FixInfacingNormals | aiProcess_CalcTangentSpace | aiProcess_GenUVCoords);

            AnimationData animData;
            animData.hasLoaded = false;

            if( scene == nullptr ) {
                LOG( logERROR ) << "Error while loading file \"" << name << "\" : " << importer.GetErrorString() << ".";
                return animData;
            }
            if( info.index < 0 || info.index >= scene->mNumMeshes ) {
                LOG(logDEBUG) << "Invalid mesh index: " << info.index << " requested, but " << scene->mNumMeshes << " meshes have been found";
                return animData;
            }

            // skeleton loading
            aiMesh* mesh = scene->mMeshes[info.index];
            if( mesh->mNumBones == 0 ) {
                LOG(logDEBUG) << "Mesh #" << info.index << ": no skeleton found.";
                return animData;
            } else {
                LOG(logDEBUG) << "Mesh #" << info.index << ": " << mesh->mNumBones << " bones found.";
            }

            std::string skelName = std::string(mesh->mName.C_Str()) + "_skeleton";
            animData.name = skelName;
            int vertexCount = 0;
            for( uint i = 0; i < info.vertexMap.size(); ++i ) {
                if( info.vertexMap[i] >= vertexCount )
                    vertexCount = info.vertexMap[i] + 1;
            }

            BoneMap boneMap; // first: name of the boneNode, second: index of the bone in the hierarchy / pose
            animData.weights.resize(vertexCount, mesh->mNumBones);

            for( uint i = 0; i < mesh->mNumBones; ++i ) {
                boneMap[mesh->mBones[i]->mName] = -1; // the true index will get written during the recursive read of the scene
                for(uint j = 0; j < mesh->mBones[i]->mNumWeights; ++j)
                {
                    aiVertexWeight vertexWeight = mesh->mBones[i]->mWeights[j];
                    int id = info.vertexMap[vertexWeight.mVertexId];
                    animData.weights.coeffRef(id, i) = vertexWeight.mWeight;
                }
            }

            // find the bone nodes and create the corresponding skeleton
            recursiveSkeletonRead(scene->mRootNode, aiMatrix4x4(), boneMap, animData, -1);

            // Store the names of each bone
            animData.boneNames.resize(boneMap.size());
            for( std::pair<aiString, int> p : boneMap )
            {
                CORE_ASSERT(p.second < boneMap.size(), "Invalid bone index");
                animData.boneNames[p.second] = std::string(p.first.C_Str());
            }
            LOG(logDEBUG) << "Found a skeleton of " << boneMap.size() << " bones";

            checkWeights( animData );

            // animation loading
            LOG(logDEBUG) << "Found " << scene->mNumAnimations << " animations";

            animData.animations.resize(scene->mNumAnimations);
            for(uint k = 0; k < scene->mNumAnimations; k++)
            {
                aiAnimation* animation = scene->mAnimations[k];
                int channelCount = animation->mNumChannels;
                int boneCount = boneMap.size();

                // Get the sorted set of key pose timestamps
                std::vector<double> timeSet;
                getUniqueKeyTimes(animation, timeSet);
                int keyCount = timeSet.size();

                // Allocate the poses
                std::vector<Ra::Core::Animation::Pose> poses;
                for(uint i = 0; i < keyCount; ++i)
                    poses.push_back(Ra::Core::Animation::Pose(boneCount));

                // Track which bones have an animation
                std::vector<bool> animatedBones( boneCount );
                for(uint i = 0; i < boneCount; ++i)
                    animatedBones[i] = false;

                // Add the animated bone transforms to the poses + interpolate when necessary
                for(uint i = 0; i < channelCount; ++i)
                {
                    aiNodeAnim* currentNodeAnim = animation->mChannels[i];
                    if(boneMap.find(currentNodeAnim->mNodeName) == boneMap.end()) // We should be able to ignore bones that do not affect the mesh
                        continue;
                    //CORE_ASSERT(boneMap.find(currentNodeAnim->mNodeName) != boneMap.end(), "Unknown bone channel");

                    int channelKeyCount = currentNodeAnim->mNumPositionKeys;
                    int boneIndex = boneMap[currentNodeAnim->mNodeName];
                    animatedBones[boneIndex] = true;

                    int channelKeyIndex = 0;
                    for(uint j = 0; j < keyCount; ++j)
                    {
                        double channelKeyTime = currentNodeAnim->mPositionKeys[channelKeyIndex].mTime;
                        if(channelKeyTime == timeSet[j])
                        {
                            Ra::Core::Transform keyTransform;
                            getTransformFromKey(currentNodeAnim, channelKeyIndex, keyTransform);
                            poses[j][boneIndex] = keyTransform;
                            if(channelKeyIndex < channelKeyCount - 1)
                                channelKeyIndex++;
                        }
                        else if(channelKeyIndex == 0 || channelKeyIndex == channelKeyCount - 1) // the first channel key is after the current key
                        {
                            Ra::Core::Transform keyTransform;
                            getTransformFromKey(currentNodeAnim, channelKeyIndex, keyTransform);
                            poses[j][boneIndex] = keyTransform;
                        }
                        else if(channelKeyTime > timeSet[j]) // the current key is between two channel keys
                        {
                            // interpolate between the previous and current channel key
                            Ra::Core::Transform previousKeyTransform;
                            Ra::Core::Transform nextKeyTransform;
                            Ra::Core::Transform keyTransform;
                            getTransformFromKey(currentNodeAnim, channelKeyIndex - 1, previousKeyTransform);
                            getTransformFromKey(currentNodeAnim, channelKeyIndex, nextKeyTransform);

                            double prevChannelKeyTime = currentNodeAnim->mPositionKeys[channelKeyIndex - 1].mTime;
                            Scalar t = (timeSet[j] - prevChannelKeyTime) / (channelKeyTime - prevChannelKeyTime);
                            Ra::Core::Animation::interpolateTransforms(previousKeyTransform, nextKeyTransform, t, keyTransform);

                            poses[j][boneIndex] = keyTransform;
                        }
                        else
                        {
                            CORE_ASSERT(false, "AnimationLoader.cpp: should not be there");
                        }

                        if(animData.hierarchy.isRoot(boneIndex))
                            poses[j][boneIndex] = animData.baseTransform * poses[j][boneIndex];
                    }
                }

                // add the non animated bone transforms to the poses
                for(uint i = 0; i < boneCount; ++i)
                {
                    if(!animatedBones[i])
                    {
                        for(uint j = 0; j < keyCount; ++j)
                        {
                            poses[j][i] = animData.pose[i];
                        }
                    }
                }

                // finally create the animation object
                Scalar animationRate = animation->mTicksPerSecond > 0.0 ? animation->mTicksPerSecond : 50.0;
                for(uint i = 0; i < keyCount; ++i)
                {
                    Scalar keyTime = timeSet[i] / animationRate;
                    animData.animations[k].addKeyPose(poses[i], keyTime);
                }
                animData.animations[k].normalize();
            }

            animData.hasLoaded = true;
            return animData;
        }

        void recursiveSkeletonRead(const aiNode* node, aiMatrix4x4 accTransform, BoneMap &boneMap, AnimationData& data, int parent)
        {
            aiMatrix4x4 currentTransform  = accTransform * node->mTransformation;
            bool isBoneNode = boneMap.find(node->mName) != boneMap.end();
            int currentIndex = parent;

            if(!isBoneNode && node->mNumChildren == 0 && parent != -1) // Catch the end bones
                isBoneNode = true;

            if(isBoneNode)
            {
                if(parent == -1)
                {
                    assimpToCore(accTransform, data.baseTransform);
                }

                // store the bone in the hierarchy
                currentIndex = data.hierarchy.addNode(parent);
                // store the index in the BoneMap
                boneMap[node->mName] = currentIndex;

                // store the transform forthe bone
                Ra::Core::Transform tr;
                assimpToCore(currentTransform, tr);
                data.pose.push_back(tr);

                // initialize the transform forthe child bones
                currentTransform = aiMatrix4x4();
            }

            for(uint i = 0; i < node->mNumChildren; ++i)
                recursiveSkeletonRead(node->mChildren[i], currentTransform, boneMap, data, currentIndex);
        }

        void getTransformFromKey(const aiNodeAnim* key, int i, Ra::Core::Transform& keyTransform)
        {
            aiVector3D   keyPosition = key->mPositionKeys[i].mValue;
            aiQuaternion keyRotation = key->mRotationKeys[i].mValue;
            aiVector3D   keyScaling  = key->mScalingKeys[i].mValue;

            // convert the key to a transform matrix
            assimpToCore(keyPosition, keyRotation, keyScaling, keyTransform);
        }

        void getUniqueKeyTimes(aiAnimation* animation, std::vector<double>& times)
        {
            int channelCount = animation->mNumChannels;
            std::set<double> timeSet;
            for(uint i = 0; i < channelCount; ++i)
            {
                aiNodeAnim* currentNodeAnim = animation->mChannels[i];

                int channelKeyCount = currentNodeAnim->mNumRotationKeys;
                for(uint j = 0; j < channelKeyCount; ++j)
                {
                    const aiVectorKey& positionKey = currentNodeAnim->mPositionKeys[j];
                    const aiQuatKey& rotationKey = currentNodeAnim->mRotationKeys[j];
                    const aiVectorKey& scalingKey = currentNodeAnim->mScalingKeys[j];

                    CORE_ASSERT(positionKey.mTime == rotationKey.mTime && positionKey.mTime == scalingKey.mTime, "Invalid key times");

                    timeSet.insert(positionKey.mTime);
                }
            }
            std::copy(timeSet.begin(), timeSet.end(), std::back_inserter(times));
        }

        void assimpToCore( const aiMatrix4x4& inMatrix, Ra::Core::Transform& outMatrix )
        {
            for( uint i = 0; i < 4; ++i )
            {
                for( uint j = 0; j < 4; ++j )
                {
                    outMatrix(i, j) = inMatrix[i][j];
                }
            }
        }

        void assimpToCore(const aiQuaternion& inQuat, Ra::Core::Quaternion& outQuat)
        {
            outQuat = Ra::Core::Quaternion(inQuat.w, inQuat.x, inQuat.y, inQuat.z);
        }

        void assimpToCore(const aiVector3D& inVec, Ra::Core::Vector3& outVec)
        {
            outVec = Ra::Core::Vector3(inVec.x, inVec.y, inVec.z);
        }

        void assimpToCore(const aiVector3D &inTranslation, const aiQuaternion &inRotation, const aiVector3D &inScaling, Ra::Core::Transform& outTransform)
        {
            Ra::Core::Vector3 translation;
            Ra::Core::Vector3 scaling;
            Ra::Core::Quaternion rotation;
            assimpToCore(inTranslation, translation);
            assimpToCore(inScaling, scaling);
            assimpToCore(inRotation, rotation);
            outTransform.fromPositionOrientationScale(translation, rotation, scaling);
        }

        void checkWeights( AnimationData &data ) {
            Ra::Core::Graph::AdjacencyList&    graph   = data.hierarchy;
            Ra::Core::Animation::WeightMatrix& weights = data.weights;
            const uint g_size = graph.size();
            const uint w_rows = weights.rows();
            const uint w_cols = weights.cols();
            if( g_size > w_cols ) {
                Ra::Core::Animation::WeightMatrix newWeights( w_rows, g_size );
                newWeights.reserve( weights.size() );
                std::map< uint, uint > table;
                for( uint i = 0; i < w_cols; ++i ) {
                    uint j = i;
                    while( graph.isLeaf( j ) ) {
                        ++j;
                    }
                    table[i] = j;
                }
                for( const auto& it : table ) {
                    newWeights.col( it.second ) = weights.col( it.first );
                }
                weights.swap( newWeights );
            }
            return;
        }
    }
}
<|endoftext|>


<commit_before><commit_msg>MeanPtV2 correlation: efficiency check is done for a given efficiency, and not for the whole array<commit_after><|endoftext|>


<commit_before><commit_msg>PWGGA/GammaConv: Changed Cuts (Rotation Back & Exotics) for EDC pp 13 TeV<commit_after><|endoftext|>


<commit_before>/*
 * Copyright (C) 2006-2016  Music Technology Group - Universitat Pompeu Fabra
 *
 * This file is part of Essentia
 *
 * Essentia is free software: you can redistribute it and/or modify it under
 * the terms of the GNU Affero General Public License as published by the Free
 * Software Foundation (FSF), either version 3 of the License, or (at your
 * option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 * FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
 * details.
 *
 * You should have received a copy of the Affero GNU General Public License
 * version 3 along with this program.  If not, see http://www.gnu.org/licenses/
 */

#include "danceability.h"

using namespace std;
namespace essentia {
namespace standard {

const char* Danceability::name = "Danceability";
const char* Danceability::category = "Rhythm";
const char* Danceability::description = DOC("Calculates the danceability vector for a given audio signal. The algorithm is derived from Detrended Fluctuation Analysis (DFA) described in [1]. The parameters minTau and maxTau are used to define the range of time over which DFA will be performed. The output of this algorithm is the danceability of the audio signal. These values usually range from 0 to 3 (higher values meaning more danceable).\n\n"
"Exception is thrown when minTau is greater than maxTau.\n\n"
"References:\n"
"  [1] Streich, S. and Herrera, P., Detrended Fluctuation Analysis of Music\n"
"  Signals: Danceability Estimation and further Semantic Characterization,\n"
"  Proceedings of the AES 118th Convention, Barcelona, Spain, 2005");

Real Danceability::stddev(const vector<Real>& array, int start, int end) const {

  Real mean_array = mean(array, start, end);
  Real var = 0.0;

  for (int i=start; i<end; i++) {
    Real d = array[i] - mean_array;
    var += d*d;
  }

  return sqrt(var / (end - start - 1.0));
}

void Danceability::configure() {

  Real minTau = parameter("minTau").toReal();
  Real maxTau = parameter("maxTau").toReal();
  Real tauIncrement = parameter("tauMultiplier").toReal();

  if (minTau > maxTau) {
    throw EssentiaException("Danceability: minTau cannot be larger than maximumTauInMs");
  }

  // tau is the number of blocks of 10ms we calculate each time
  _tau.clear();
  for (Real tau = minTau; tau <= maxTau; tau *= tauIncrement) {
    _tau.push_back(int(tau / 10.0));
  }
}

void Danceability::compute() {

  const vector<Real>& signal = _signal.get();
  Real& danceability = _danceability.get();
  Real sampleRate = parameter("sampleRate").toReal();

  //---------------------------------------------------------------------
  // preprocessing:
  // cut up into 10 ms frames and calculate the stddev for each slice
  // store in s(n), then integrate

  int numSamples = signal.size();
  int frameSize = int(0.01 * sampleRate); // 10ms
  int numFrames = numSamples / frameSize;

  vector<Real> s(numFrames, 0.0);

  for (int i=0; i<numFrames; i++) {
    int frameBegin = i * frameSize;
    int frameEnd = min((i+1) * frameSize, numSamples);

    s[i] = stddev(signal, frameBegin, frameEnd);
  }

  // subtract the mean from the array to make it have 0 DC
  Real mean_s = mean(s,0,s.size());
  for (int i=0; i<numFrames; i++)
    s[i] -= mean_s;

  // integrate the signal
  for (int i=1; i<(int)s.size(); i++)
    s[i] += s[i-1];

  //---------------------------------------------------------------------
  // processing

  vector<Real> F(_tau.size(), 0.0);

  int nFValues = 0;

  // for each tau (i.e. block size)
  for (int i=0; i<(int)_tau.size(); i++) {

    int tau = _tau[i];

    // the original algorithm slides/jumps the blocks forward with
    // one sample, but that's very CPU intensive.
    // So, ... for large tau values, lets take larger jumps
    int jump = max(tau/50, 1);

    // perhaps we're working on a short file, then we don't have all values...
    if(numFrames >= tau)
    {
      // cut up the audio in tau-sized blocks
      for(int k=0; k<numFrames - tau; k += jump)
      {
        int frameBegin = k;
        int frameEnd = k + tau;

        // find the average residual error in this block
        // the residual error is sum( squared( signal - linear_regression ) )
        F[i] += residualError(s, frameBegin, frameEnd);
      }

      // the square root of the total residual error, for all the
      // blocks of size tau
      if (numFrames == tau) {
         F[i] = 0.0;
      }
      else {
         F[i] = sqrt(F[i] / ((Real)(numFrames - tau)/(Real)jump));
      }

      nFValues++;
    }
    else
    {
      break;
    }
  }

  danceability = 0.0;

  // the original article tells us: for small tau's we need to adjust alpha (danceability)
  for (int i=0; i<nFValues - 1; i++) {
    if (F[i+1] != 0.0) {
      danceability += log10(F[i+1] / F[i]) / log10( ((Real)_tau[i+1]+3.0) / ((Real)_tau[i]+3.0));
      if (log10(F[i+1] / F[i]) < 0) {
        cout << "DEBUG:" << i <<", " << F[i+1] << ", " << F[i] << endl;
      }
    }
    else {
      danceability = 0.0;
      return;
    }
  }

  danceability /= (nFValues-1);

  if (danceability > 0.0) {
    // negative values occur very very seldom, therefore we can ignore them
    danceability = 1.0 / danceability;
  }
  else {
     danceability = 0.0;
  }
}

} // namespace standard
} // namespace essentia

#include "poolstorage.h"
#include "algorithmfactory.h"

namespace essentia {
namespace streaming {

const char* Danceability::name = standard::Danceability::name;
const char* Danceability::category = standard::Danceability::category;
const char* Danceability::description = standard::Danceability::description;

Danceability::Danceability() : AlgorithmComposite() {

  _danceabilityAlgo = standard::AlgorithmFactory::create("Danceability");
  _poolStorage = new PoolStorage<Real>(&_pool, "internal.signal");

  declareInput(_signal, 1, "signal", "the input signal");
  declareOutput(_danceability, 0, "danceability", "the danceability value. Normal values range from 0 to ~3. The higher, the more danceable.");

  _signal >> _poolStorage->input("data"); // attach input proxy
}

Danceability::~Danceability() {
  delete _danceabilityAlgo;
  delete _poolStorage;
}

void Danceability::reset() {
  AlgorithmComposite::reset();
  _poolStorage->reset();
}

AlgorithmStatus Danceability::process() {
  if (!shouldStop()) return PASS;

  Real danceability;

  _danceabilityAlgo->input("signal").set(_pool.value<vector<Real> >("internal.signal"));
  _danceabilityAlgo->output("danceability").set(danceability);
  _danceabilityAlgo->compute();

  _danceability.push(danceability);
  return FINISHED;
}

} // namespace streaming
} // namespace essentia
<commit_msg>Remove debug couts<commit_after>/*
 * Copyright (C) 2006-2016  Music Technology Group - Universitat Pompeu Fabra
 *
 * This file is part of Essentia
 *
 * Essentia is free software: you can redistribute it and/or modify it under
 * the terms of the GNU Affero General Public License as published by the Free
 * Software Foundation (FSF), either version 3 of the License, or (at your
 * option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 * FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
 * details.
 *
 * You should have received a copy of the Affero GNU General Public License
 * version 3 along with this program.  If not, see http://www.gnu.org/licenses/
 */

#include "danceability.h"

using namespace std;
namespace essentia {
namespace standard {

const char* Danceability::name = "Danceability";
const char* Danceability::category = "Rhythm";
const char* Danceability::description = DOC("Calculates the danceability vector for a given audio signal. The algorithm is derived from Detrended Fluctuation Analysis (DFA) described in [1]. The parameters minTau and maxTau are used to define the range of time over which DFA will be performed. The output of this algorithm is the danceability of the audio signal. These values usually range from 0 to 3 (higher values meaning more danceable).\n\n"
"Exception is thrown when minTau is greater than maxTau.\n\n"
"References:\n"
"  [1] Streich, S. and Herrera, P., Detrended Fluctuation Analysis of Music\n"
"  Signals: Danceability Estimation and further Semantic Characterization,\n"
"  Proceedings of the AES 118th Convention, Barcelona, Spain, 2005");

Real Danceability::stddev(const vector<Real>& array, int start, int end) const {

  Real mean_array = mean(array, start, end);
  Real var = 0.0;

  for (int i=start; i<end; i++) {
    Real d = array[i] - mean_array;
    var += d*d;
  }

  return sqrt(var / (end - start - 1.0));
}

void Danceability::configure() {

  Real minTau = parameter("minTau").toReal();
  Real maxTau = parameter("maxTau").toReal();
  Real tauIncrement = parameter("tauMultiplier").toReal();

  if (minTau > maxTau) {
    throw EssentiaException("Danceability: minTau cannot be larger than maximumTauInMs");
  }

  // tau is the number of blocks of 10ms we calculate each time
  _tau.clear();
  for (Real tau = minTau; tau <= maxTau; tau *= tauIncrement) {
    _tau.push_back(int(tau / 10.0));
  }
}

void Danceability::compute() {

  const vector<Real>& signal = _signal.get();
  Real& danceability = _danceability.get();
  Real sampleRate = parameter("sampleRate").toReal();

  //---------------------------------------------------------------------
  // preprocessing:
  // cut up into 10 ms frames and calculate the stddev for each slice
  // store in s(n), then integrate

  int numSamples = signal.size();
  int frameSize = int(0.01 * sampleRate); // 10ms
  int numFrames = numSamples / frameSize;

  vector<Real> s(numFrames, 0.0);

  for (int i=0; i<numFrames; i++) {
    int frameBegin = i * frameSize;
    int frameEnd = min((i+1) * frameSize, numSamples);

    s[i] = stddev(signal, frameBegin, frameEnd);
  }

  // subtract the mean from the array to make it have 0 DC
  Real mean_s = mean(s,0,s.size());
  for (int i=0; i<numFrames; i++)
    s[i] -= mean_s;

  // integrate the signal
  for (int i=1; i<(int)s.size(); i++)
    s[i] += s[i-1];

  //---------------------------------------------------------------------
  // processing

  vector<Real> F(_tau.size(), 0.0);

  int nFValues = 0;

  // for each tau (i.e. block size)
  for (int i=0; i<(int)_tau.size(); i++) {

    int tau = _tau[i];

    // the original algorithm slides/jumps the blocks forward with
    // one sample, but that's very CPU intensive.
    // So, ... for large tau values, lets take larger jumps
    int jump = max(tau/50, 1);

    // perhaps we're working on a short file, then we don't have all values...
    if(numFrames >= tau)
    {
      // cut up the audio in tau-sized blocks
      for(int k=0; k<numFrames - tau; k += jump)
      {
        int frameBegin = k;
        int frameEnd = k + tau;

        // find the average residual error in this block
        // the residual error is sum( squared( signal - linear_regression ) )
        F[i] += residualError(s, frameBegin, frameEnd);
      }

      // the square root of the total residual error, for all the
      // blocks of size tau
      if (numFrames == tau) {
         F[i] = 0.0;
      }
      else {
         F[i] = sqrt(F[i] / ((Real)(numFrames - tau)/(Real)jump));
      }

      nFValues++;
    }
    else
    {
      break;
    }
  }

  danceability = 0.0;

  // the original article tells us: for small tau's we need to adjust alpha (danceability)
  for (int i=0; i<nFValues - 1; i++) {
    if (F[i+1] != 0.0) {
      danceability += log10(F[i+1] / F[i]) / log10( ((Real)_tau[i+1]+3.0) / ((Real)_tau[i]+3.0));
    }
    else {
      danceability = 0.0;
      return;
    }
  }

  danceability /= (nFValues-1);

  if (danceability > 0.0) {
    // negative values occur very very seldom, therefore we can ignore them
    danceability = 1.0 / danceability;
  }
  else {
     danceability = 0.0;
  }
}

} // namespace standard
} // namespace essentia

#include "poolstorage.h"
#include "algorithmfactory.h"

namespace essentia {
namespace streaming {

const char* Danceability::name = standard::Danceability::name;
const char* Danceability::category = standard::Danceability::category;
const char* Danceability::description = standard::Danceability::description;

Danceability::Danceability() : AlgorithmComposite() {

  _danceabilityAlgo = standard::AlgorithmFactory::create("Danceability");
  _poolStorage = new PoolStorage<Real>(&_pool, "internal.signal");

  declareInput(_signal, 1, "signal", "the input signal");
  declareOutput(_danceability, 0, "danceability", "the danceability value. Normal values range from 0 to ~3. The higher, the more danceable.");

  _signal >> _poolStorage->input("data"); // attach input proxy
}

Danceability::~Danceability() {
  delete _danceabilityAlgo;
  delete _poolStorage;
}

void Danceability::reset() {
  AlgorithmComposite::reset();
  _poolStorage->reset();
}

AlgorithmStatus Danceability::process() {
  if (!shouldStop()) return PASS;

  Real danceability;

  _danceabilityAlgo->input("signal").set(_pool.value<vector<Real> >("internal.signal"));
  _danceabilityAlgo->output("danceability").set(danceability);
  _danceabilityAlgo->compute();

  _danceability.push(danceability);
  return FINISHED;
}

} // namespace streaming
} // namespace essentia
<|endoftext|>


<commit_before>/**
 *  For conditions of distribution and use, see copyright notice in license.txt
 *
 *  @file   SceneStructureWindow.cpp
 *  @brief  Window with tree view of contents of scene.
 *
 *          Detailed desc here.
 */

#include "StableHeaders.h"
#include "SceneStructureWindow.h"
#include "SceneTreeWidget.h"

#include "Framework.h"
#include "SceneManager.h"
#include "ECEditorWindow.h"
#include "UiServiceInterface.h"
#include "EC_Name.h"

using namespace Scene;

SceneStructureWindow::SceneStructureWindow(Foundation::Framework *fw) :
    framework(fw),
    showComponents(false)
{
    QVBoxLayout *layout = new QVBoxLayout(this);
    layout->setContentsMargins(0, 0, 0, 0);
    setLayout(layout);
    setWindowTitle(tr("Scene Structure"));
    resize(200,300);

    QCheckBox *cb = new QCheckBox(tr("Show components"), this);
    connect(cb, SIGNAL(toggled(bool)), this, SLOT(ShowComponents(bool)));
    layout->addWidget(cb);

    treeWidget = new SceneTreeWidget(fw, this);
    layout->addWidget(treeWidget);
}

SceneStructureWindow::~SceneStructureWindow()
{
    SetScene(ScenePtr());
}

void SceneStructureWindow::SetScene(const Scene::ScenePtr &s)
{
    if (!scene.expired() && (s == scene.lock()))
        return;

    if (s == 0)
    {
        disconnect(s.get());
        Clear();
        return;
    }

    scene = s;
    SceneManager *scenePtr = scene.lock().get();
    connect(scenePtr, SIGNAL(EntityCreated(Scene::Entity *, AttributeChange::Type)), SLOT(AddEntity(Scene::Entity *)));
    connect(scenePtr, SIGNAL(EntityRemoved(Scene::Entity *, AttributeChange::Type)), SLOT(RemoveEntity(Scene::Entity *)));
    connect(scenePtr, SIGNAL(ComponentAdded(Scene::Entity *, IComponent *, AttributeChange::Type)),
        SLOT(AddComponent(Scene::Entity *, IComponent *)));
    connect(scenePtr, SIGNAL(ComponentRemoved(Scene::Entity *, IComponent *, AttributeChange::Type)),
        SLOT(RemoveComponent(Scene::Entity *, IComponent *)));

    Populate();
}

void SceneStructureWindow::ShowComponents(bool show)
{
    showComponents = show;
    treeWidget->showComponents =show;
    for (int i = 0; i < treeWidget->topLevelItemCount(); ++i)
    {
        QTreeWidgetItem *item = treeWidget->topLevelItem(i);
        for(int j = 0; j < item->childCount(); ++j)
            item->child(j)->setHidden(!showComponents);
    }
}

void SceneStructureWindow::changeEvent(QEvent* e)
{
    if (e->type() == QEvent::LanguageChange)
        setWindowTitle(tr("Scene Structure"));
    else
        QWidget::changeEvent(e);
}

void SceneStructureWindow::Populate()
{
    ScenePtr s = scene.lock();
    if (!s)
    {
        // warning print
        return;
    }

    SceneManager::iterator it = s->begin();
    while(it != s->end())
    {
        AddEntity((*it).get());
        ++it;
    }
}

void SceneStructureWindow::Clear()
{
    for (int i = 0; i < treeWidget->topLevelItemCount(); ++i)
    {
        QTreeWidgetItem *item = treeWidget->topLevelItem(i);
        SAFE_DELETE(item);
    }
}

void SceneStructureWindow::AddEntity(Scene::Entity* entity)
{
    EntityTreeWidgetItem *item = new EntityTreeWidgetItem(entity->GetId());
    item->setText(0, QString("%1 %2").arg(entity->GetId()).arg(entity->GetName()));
    // Set local entity's font color blue
    if (entity->IsLocal())
        item->setTextColor(0, QColor(Qt::blue));
    treeWidget->addTopLevelItem(item);

    foreach(ComponentPtr c, entity->GetComponentVector())
        AddComponent(entity, c.get());
}

void SceneStructureWindow::RemoveEntity(Scene::Entity* entity)
{
    for (int i = 0; i < treeWidget->topLevelItemCount(); ++i)
    {
        EntityTreeWidgetItem *item = dynamic_cast<EntityTreeWidgetItem *>(treeWidget->topLevelItem(i));
        if (item && (item->id == entity->GetId()))
        {
            SAFE_DELETE(item);
            break;
        }
    }
}

void SceneStructureWindow::AddComponent(Scene::Entity* entity, IComponent* comp)
{
    for (int i = 0; i < treeWidget->topLevelItemCount(); ++i)
    {
        EntityTreeWidgetItem *eItem = dynamic_cast<EntityTreeWidgetItem *>(treeWidget->topLevelItem(i));
        if (eItem && (eItem->id == entity->GetId()))
        {
            ComponentTreeWidgetItem *cItem = new ComponentTreeWidgetItem(comp->TypeName(), comp->Name(), eItem);
            cItem->setText(0, QString("%1 %2").arg(comp->TypeName()).arg(comp->Name()));
            cItem->setHidden(!showComponents);
            eItem->addChild(cItem);

            // If name component exists, retrieve name from it. Also hook up change signal so that UI keeps synch with the name.
            if (comp->TypeName() == EC_Name::TypeNameStatic())
            {
                eItem->setText(0, QString("%1 %2").arg(entity->GetId()).arg(entity->GetName()));
                connect(comp, SIGNAL(OnAttributeChanged(IAttribute *, AttributeChange::Type)), SLOT(UpdateEntityName(IAttribute *)));
            }
        }
    }
}

void SceneStructureWindow::RemoveComponent(Scene::Entity* entity, IComponent* comp)
{
    for (int i = 0; i < treeWidget->topLevelItemCount(); ++i)
    {
        EntityTreeWidgetItem *eItem = dynamic_cast<EntityTreeWidgetItem *>(treeWidget->topLevelItem(i));
        if (eItem && (eItem->id == entity->GetId()))
        {
            for (int j = 0; j < eItem->childCount(); ++j)
            {
                ComponentTreeWidgetItem *cItem = dynamic_cast<ComponentTreeWidgetItem *>(eItem->child(j));
                if (cItem && (cItem->typeName == comp->TypeName()) && (cItem->name == comp->Name()))
                {
                    eItem->removeChild(cItem);
                    SAFE_DELETE(cItem);
                    break;
                }
            }

            if (comp->TypeName() == EC_Name::TypeNameStatic())
                eItem->setText(0, QString("%1").arg(entity->GetId()));
        }
    }
}

void SceneStructureWindow::UpdateEntityName(IAttribute *attr)
{
    EC_Name *nameComp = dynamic_cast<EC_Name *>(sender());
    if (!nameComp || (attr != &nameComp->name) || (nameComp->GetParentEntity() == 0))
        return;

    Entity *entity = nameComp->GetParentEntity();
    for(int i = 0; i < treeWidget->topLevelItemCount(); ++i)
    {
        EntityTreeWidgetItem *item = dynamic_cast<EntityTreeWidgetItem *>(treeWidget->topLevelItem(i));
        if (item && (item->id == entity->GetId()))
            item->setText(0, QString("%1 %2").arg(entity->GetId()).arg(entity->GetName()));
    }
}

void SceneStructureWindow::UpdateComponentName(const QString &oldName, const QString &newName)
{
}

<commit_msg>Update scene struct UI when component name changes. Note: note tested yet.<commit_after>/**
 *  For conditions of distribution and use, see copyright notice in license.txt
 *
 *  @file   SceneStructureWindow.cpp
 *  @brief  Window with tree view of contents of scene.
 *
 *          Detailed desc here.
 */

#include "StableHeaders.h"
#include "SceneStructureWindow.h"
#include "SceneTreeWidget.h"

#include "Framework.h"
#include "SceneManager.h"
#include "ECEditorWindow.h"
#include "UiServiceInterface.h"
#include "EC_Name.h"

using namespace Scene;

SceneStructureWindow::SceneStructureWindow(Foundation::Framework *fw) :
    framework(fw),
    showComponents(false)
{
    QVBoxLayout *layout = new QVBoxLayout(this);
    layout->setContentsMargins(0, 0, 0, 0);
    setLayout(layout);
    setWindowTitle(tr("Scene Structure"));
    resize(200,300);

    QCheckBox *cb = new QCheckBox(tr("Show components"), this);
    connect(cb, SIGNAL(toggled(bool)), this, SLOT(ShowComponents(bool)));
    layout->addWidget(cb);

    treeWidget = new SceneTreeWidget(fw, this);
    layout->addWidget(treeWidget);
}

SceneStructureWindow::~SceneStructureWindow()
{
    SetScene(ScenePtr());
}

void SceneStructureWindow::SetScene(const Scene::ScenePtr &s)
{
    if (!scene.expired() && (s == scene.lock()))
        return;

    if (s == 0)
    {
        disconnect(s.get());
        Clear();
        return;
    }

    scene = s;
    SceneManager *scenePtr = scene.lock().get();
    connect(scenePtr, SIGNAL(EntityCreated(Scene::Entity *, AttributeChange::Type)), SLOT(AddEntity(Scene::Entity *)));
    connect(scenePtr, SIGNAL(EntityRemoved(Scene::Entity *, AttributeChange::Type)), SLOT(RemoveEntity(Scene::Entity *)));
    connect(scenePtr, SIGNAL(ComponentAdded(Scene::Entity *, IComponent *, AttributeChange::Type)),
        SLOT(AddComponent(Scene::Entity *, IComponent *)));
    connect(scenePtr, SIGNAL(ComponentRemoved(Scene::Entity *, IComponent *, AttributeChange::Type)),
        SLOT(RemoveComponent(Scene::Entity *, IComponent *)));

    Populate();
}

void SceneStructureWindow::ShowComponents(bool show)
{
    showComponents = show;
    treeWidget->showComponents =show;
    for (int i = 0; i < treeWidget->topLevelItemCount(); ++i)
    {
        QTreeWidgetItem *item = treeWidget->topLevelItem(i);
        for(int j = 0; j < item->childCount(); ++j)
            item->child(j)->setHidden(!showComponents);
    }
}

void SceneStructureWindow::changeEvent(QEvent* e)
{
    if (e->type() == QEvent::LanguageChange)
        setWindowTitle(tr("Scene Structure"));
    else
        QWidget::changeEvent(e);
}

void SceneStructureWindow::Populate()
{
    ScenePtr s = scene.lock();
    if (!s)
    {
        // warning print
        return;
    }

    for(SceneManager::iterator it = s->begin(); it != s->end(); ++it)
        AddEntity((*it).get());
}

void SceneStructureWindow::Clear()
{
    for(int i = 0; i < treeWidget->topLevelItemCount(); ++i)
    {
        QTreeWidgetItem *item = treeWidget->topLevelItem(i);
        SAFE_DELETE(item);
    }
}

void SceneStructureWindow::AddEntity(Scene::Entity* entity)
{
    EntityTreeWidgetItem *item = new EntityTreeWidgetItem(entity->GetId());
    item->setText(0, QString("%1 %2").arg(entity->GetId()).arg(entity->GetName()));
    // Set local entity's font color blue
    if (entity->IsLocal())
        item->setTextColor(0, QColor(Qt::blue));
    treeWidget->addTopLevelItem(item);

    foreach(ComponentPtr c, entity->GetComponentVector())
        AddComponent(entity, c.get());
}

void SceneStructureWindow::RemoveEntity(Scene::Entity* entity)
{
    for (int i = 0; i < treeWidget->topLevelItemCount(); ++i)
    {
        EntityTreeWidgetItem *item = dynamic_cast<EntityTreeWidgetItem *>(treeWidget->topLevelItem(i));
        if (item && (item->id == entity->GetId()))
        {
            SAFE_DELETE(item);
            break;
        }
    }
}

void SceneStructureWindow::AddComponent(Scene::Entity* entity, IComponent* comp)
{
    for (int i = 0; i < treeWidget->topLevelItemCount(); ++i)
    {
        EntityTreeWidgetItem *eItem = dynamic_cast<EntityTreeWidgetItem *>(treeWidget->topLevelItem(i));
        if (eItem && (eItem->id == entity->GetId()))
        {
            ComponentTreeWidgetItem *cItem = new ComponentTreeWidgetItem(comp->TypeName(), comp->Name(), eItem);
            cItem->setText(0, QString("%1 %2").arg(comp->TypeName()).arg(comp->Name()));
            cItem->setHidden(!showComponents);
            eItem->addChild(cItem);

            connect(comp, SIGNAL(OnComponentNameChanged(const QString &, const QString &)), SLOT(UpdateComponentName(const QString &, const QString &)));

            // If name component exists, retrieve name from it. Also hook up change signal so that UI keeps synch with the name.
            if (comp->TypeName() == EC_Name::TypeNameStatic())
            {
                eItem->setText(0, QString("%1 %2").arg(entity->GetId()).arg(entity->GetName()));
                connect(comp, SIGNAL(OnAttributeChanged(IAttribute *, AttributeChange::Type)), SLOT(UpdateEntityName(IAttribute *)));
            }
        }
    }
}

void SceneStructureWindow::RemoveComponent(Scene::Entity* entity, IComponent* comp)
{
    for (int i = 0; i < treeWidget->topLevelItemCount(); ++i)
    {
        EntityTreeWidgetItem *eItem = dynamic_cast<EntityTreeWidgetItem *>(treeWidget->topLevelItem(i));
        if (eItem && (eItem->id == entity->GetId()))
        {
            for (int j = 0; j < eItem->childCount(); ++j)
            {
                ComponentTreeWidgetItem *cItem = dynamic_cast<ComponentTreeWidgetItem *>(eItem->child(j));
                if (cItem && (cItem->typeName == comp->TypeName()) && (cItem->name == comp->Name()))
                {
                    eItem->removeChild(cItem);
                    SAFE_DELETE(cItem);
                    break;
                }
            }

            if (comp->TypeName() == EC_Name::TypeNameStatic())
                eItem->setText(0, QString("%1").arg(entity->GetId()));
        }
    }
}

void SceneStructureWindow::UpdateEntityName(IAttribute *attr)
{
    EC_Name *nameComp = dynamic_cast<EC_Name *>(sender());
    if (!nameComp || (attr != &nameComp->name) || (nameComp->GetParentEntity() == 0))
        return;

    Entity *entity = nameComp->GetParentEntity();
    for(int i = 0; i < treeWidget->topLevelItemCount(); ++i)
    {
        EntityTreeWidgetItem *item = dynamic_cast<EntityTreeWidgetItem *>(treeWidget->topLevelItem(i));
        if (item && (item->id == entity->GetId()))
            item->setText(0, QString("%1 %2").arg(entity->GetId()).arg(entity->GetName()));
    }
}

void SceneStructureWindow::UpdateComponentName(const QString &oldName, const QString &newName)
{
    IComponent *comp = dynamic_cast<IComponent *>(sender());
    if (!comp)
        return;

    for (int i = 0; i < treeWidget->topLevelItemCount(); ++i)
    {
        QTreeWidgetItem *eItem = treeWidget->topLevelItem(i);
        for (int j = 0; j < eItem->childCount(); ++j)
        {
            ComponentTreeWidgetItem *cItem = dynamic_cast<ComponentTreeWidgetItem *>(eItem->child(j));
            if (cItem && (cItem->typeName == comp->TypeName()) && (cItem->name == oldName))
                cItem->setText(0, QString("%1 %2").arg(cItem->typeName).arg(newName));
        }
    }
}

<|endoftext|>


<commit_before>// This file is a part of the OpenSurgSim project.
// Copyright 2013, SimQuest Solutions 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.

#include "SurgSim/Framework/Log.h"
#include "SurgSim/Math/OdeSolverLinearEulerImplicit.h"
#include "SurgSim/Math/OdeState.h"

namespace SurgSim
{

namespace Math
{

OdeSolverLinearEulerImplicit::OdeSolverLinearEulerImplicit(OdeEquation* equation)
	: OdeSolverEulerImplicit(equation), m_initialized(false)
{
	m_name = "Ode Solver Linear Euler Implicit";

	// The system being linear, only 1 iteration is necessary to find the exact solution.
	setNewtonRaphsonMaximumIteration(1);
}

void OdeSolverLinearEulerImplicit::setNewtonRaphsonMaximumIteration(size_t maximumIteration)
{
	OdeSolverEulerImplicit::setNewtonRaphsonMaximumIteration(maximumIteration);
	SURGSIM_LOG_IF(maximumIteration != 1, SurgSim::Framework::Logger::getLogger("OdeSolver"), WARNING) <<
			"OdeSolverLinearEulerImplicit should have a maximum number of iteration of 1 for the Newton-Raphson. " <<
			"As the model is (supposed to be) linear, a single iteration will find the exact solution.";
}

void OdeSolverLinearEulerImplicit::solve(double dt, const OdeState& currentState, OdeState* newState,
		bool computeCompliance)
{
	if (!m_initialized)
	{
		// The compliance matrix is constant and used in all following calls, so we force its calculation on 1st pass.
		OdeSolverEulerImplicit::solve(dt, currentState, newState, true);
		m_constantK = m_equation.getK();
		m_initialized = true;
	}
	else
	{
		m_equation.updateFMDK(currentState, ODEEQUATIONUPDATE_F);

		Vector f = m_equation.getF();
		f -= m_constantK * currentState.getVelocities() * dt;
		Vector deltaV = m_equation.applyCompliance(currentState, f);

		newState->getVelocities() = currentState.getVelocities() + deltaV;
		newState->getPositions()  = currentState.getPositions()  + dt * newState->getVelocities();
	}
}

}; // namespace Math

}; // namespace SurgSim
<commit_msg>Improves performance of calculating f in OdeSolverLinearEulerImplicit.<commit_after>// This file is a part of the OpenSurgSim project.
// Copyright 2013, SimQuest Solutions 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.

#include "SurgSim/Framework/Log.h"
#include "SurgSim/Math/OdeSolverLinearEulerImplicit.h"
#include "SurgSim/Math/OdeState.h"

namespace SurgSim
{

namespace Math
{

OdeSolverLinearEulerImplicit::OdeSolverLinearEulerImplicit(OdeEquation* equation)
	: OdeSolverEulerImplicit(equation), m_initialized(false)
{
	m_name = "Ode Solver Linear Euler Implicit";

	// The system being linear, only 1 iteration is necessary to find the exact solution.
	setNewtonRaphsonMaximumIteration(1);
}

void OdeSolverLinearEulerImplicit::setNewtonRaphsonMaximumIteration(size_t maximumIteration)
{
	OdeSolverEulerImplicit::setNewtonRaphsonMaximumIteration(maximumIteration);
	SURGSIM_LOG_IF(maximumIteration != 1, SurgSim::Framework::Logger::getLogger("OdeSolver"), WARNING) <<
			"OdeSolverLinearEulerImplicit should have a maximum number of iteration of 1 for the Newton-Raphson. " <<
			"As the model is (supposed to be) linear, a single iteration will find the exact solution.";
}

void OdeSolverLinearEulerImplicit::solve(double dt, const OdeState& currentState, OdeState* newState,
		bool computeCompliance)
{
	if (!m_initialized)
	{
		// The compliance matrix is constant and used in all following calls, so we force its calculation on 1st pass.
		OdeSolverEulerImplicit::solve(dt, currentState, newState, true);
		m_constantK = m_equation.getK();
		m_initialized = true;
	}
	else
	{
		m_equation.updateFMDK(currentState, ODEEQUATIONUPDATE_F);

		Vector f = m_equation.getF();
		f -= m_constantK * (currentState.getVelocities() * dt);
		Vector deltaV = m_equation.applyCompliance(currentState, f);

		newState->getVelocities() = currentState.getVelocities() + deltaV;
		newState->getPositions()  = currentState.getPositions()  + dt * newState->getVelocities();
	}
}

}; // namespace Math

}; // namespace SurgSim
<|endoftext|>


<commit_before>// This file is a part of the OpenSurgSim project.
// Copyright 2013, SimQuest Solutions 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.

#include <memory>

#include <gtest/gtest.h>

#include "SurgSim/DataStructures/Location.h"
#include "SurgSim/Math/Quaternion.h"
#include "SurgSim/Math/RigidTransform.h"
#include "SurgSim/Math/SphereShape.h"
#include "SurgSim/Math/Vector.h"
#include "SurgSim/Physics/Constraint.h"
#include "SurgSim/Physics/ConstraintData.h"
#include "SurgSim/Physics/ContactConstraintData.h"
#include "SurgSim/Physics/FixedConstraintFrictionlessContact.h"
#include "SurgSim/Physics/FixedRepresentation.h"
#include "SurgSim/Physics/MlcpPhysicsProblem.h"
#include "SurgSim/Physics/RigidConstraintFrictionlessContact.h"
#include "SurgSim/Physics/RigidRepresentation.h"

using SurgSim::DataStructures::Location;
using SurgSim::Math::SphereShape;
using SurgSim::Math::Vector3d;

namespace
{
	const double epsilon = 1e-10;
};

namespace SurgSim
{
namespace Physics
{

class ConstraintTests : public ::testing::Test
{
protected:
	/// Fixed plane pose
	SurgSim::Math::RigidTransform3d m_poseFixed;
	/// Rigid sphere pose
	SurgSim::Math::RigidTransform3d m_poseRigid;
	/// Contact normal direction
	Vector3d m_n;
	/// Distance to origin of the contact plane equation
	double m_d;
	/// Sphere radius
	double m_radius;
	/// Simulation time step
	double m_dt;
	/// Mlcp index of the sphere representation
	size_t m_indexSphereRepresentation;
	/// Mlcp index of the plane representation
	size_t m_indexPlaneRepresentation;
	/// Mlcp index of the constraint (frictionless contact)
	size_t m_indexConstraint;
	/// Contact location on the plane (point on the plane with the most penetration)
	Vector3d m_contactPositionPlane;
	/// Contact location on the sphere (point on the sphere with the most penetration)
	Vector3d m_contactPositionSphere;

	/// Rigid sphere
	std::shared_ptr<RigidRepresentation> m_rigid;
	/// Fixed plane
	std::shared_ptr<FixedRepresentation> m_fixed;

	/// Mlcp
	MlcpPhysicsProblem m_mlcpPhysicsProblem;
	/// Constraint
	std::shared_ptr<Constraint> m_constraint;
	/// Constraint data: frictionless contact
	std::shared_ptr<ContactConstraintData> m_constraintData;
	/// Location on the fixed plane
	Location m_locFixedPlane;
	/// Location on the rigid sphere
	Location m_locRigidSphere;

	/// Total number of degrees of freedom in the system (plane + sphere)
	size_t m_numDof;
	/// Total number of atomic constraint in the system (1 for a frictionless contact)
	size_t m_numConstraint;

	/// Setup the test case by creating all object
	void SetUp()
	{
		m_d = 0.0;
		m_radius = 0.01;
		m_dt = 1e-3;
		m_numDof = 0;
		m_indexSphereRepresentation = 0;
		m_indexConstraint = 0;
		m_numConstraint = 1;
		m_contactPositionPlane.setZero();
		m_contactPositionSphere.setZero();
		m_contactPositionSphere[1] = -m_radius;

		m_poseFixed.setIdentity();
		m_poseRigid.setIdentity();

		m_rigid = std::make_shared<RigidRepresentation>("Rigid");
		m_rigid->setLocalActive(true);
		m_rigid->setIsGravityEnabled(false);
		m_rigid->setLocalPose(m_poseRigid);
		{
			m_rigid->setDensity(1000.0);
			std::shared_ptr<SphereShape> shape = std::make_shared<SphereShape>(m_radius);
			m_rigid->setShape(shape);
		}
		m_numDof += m_rigid->getNumDof();

		m_indexPlaneRepresentation = m_indexSphereRepresentation + m_rigid->getNumDof();
		m_fixed = std::make_shared<FixedRepresentation>("Fixed");
		m_fixed->setLocalActive(true);
		m_fixed->setIsGravityEnabled(false);
		m_fixed->setLocalPose(m_poseFixed);
		m_numDof += m_fixed->getNumDof();

		m_locFixedPlane.rigidLocalPosition = m_contactPositionPlane;
		m_locRigidSphere.rigidLocalPosition = m_contactPositionSphere;

		m_constraintData = std::make_shared<ContactConstraintData>();

		clearMlcpPhysicsProblem(m_numDof, m_numConstraint);
	}

	/// Allocate and clear the Mlcp
	/// \param numDof The number of degrees of freedom in the system
	/// \param numConstraint The number of atomic constraints in the system
	void clearMlcpPhysicsProblem(size_t numDof, size_t numConstraint)
	{
		// Resize and zero all Eigen types
		m_mlcpPhysicsProblem.A.setZero(numConstraint, numConstraint);
		m_mlcpPhysicsProblem.b.setZero(numConstraint);
		m_mlcpPhysicsProblem.mu.setZero(numConstraint);
		m_mlcpPhysicsProblem.CHt.setZero(numDof, numConstraint);
		m_mlcpPhysicsProblem.H.resize(numConstraint, numDof);

		// Empty all std::vector types
		m_mlcpPhysicsProblem.constraintTypes.clear();
	}
};

TEST_F (ConstraintTests, TestConstructor)
{
	auto fixedRep = std::make_shared<FixedRepresentation>("fixed");
	auto rigidRep = std::make_shared<RigidRepresentation>("rigid");

	Location fixedLoc(m_contactPositionPlane);
	Location rigidLoc(m_contactPositionSphere);

	auto type = SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT;

	ASSERT_NO_THROW({Constraint c(type, m_constraintData, fixedRep, fixedLoc, rigidRep, rigidLoc);});

	EXPECT_THROW(
	{ Constraint c(type, nullptr, nullptr, fixedLoc, nullptr, fixedLoc); },
	SurgSim::Framework::AssertionFailure);
	EXPECT_THROW(
	{ Constraint c(type, m_constraintData, nullptr, fixedLoc, nullptr, fixedLoc); },
	SurgSim::Framework::AssertionFailure);
	EXPECT_THROW(
	{ Constraint c(type, m_constraintData, fixedRep, fixedLoc, nullptr, fixedLoc); },
	SurgSim::Framework::AssertionFailure);

	Constraint c(type, m_constraintData, fixedRep, fixedLoc, rigidRep, rigidLoc);

	EXPECT_EQ(m_constraintData, c.getData());
	EXPECT_EQ(type, c.getImplementations().first->getMlcpConstraintType());
	EXPECT_EQ(type, c.getImplementations().second->getMlcpConstraintType());
	EXPECT_EQ(fixedRep, c.getLocalizations().first->getRepresentation());
	EXPECT_EQ(rigidRep, c.getLocalizations().second->getRepresentation());
}

TEST_F (ConstraintTests, TestGetNumDof)
{
	auto fixedRep = std::make_shared<FixedRepresentation>("fixed");
	auto rigidRep = std::make_shared<RigidRepresentation>("rigid");

	Location fixedLoc(m_contactPositionPlane);
	Location rigidLoc(m_contactPositionSphere);

	auto type = SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT;

	{
		SCOPED_TRACE("1DOF for a frictionless contact");
		Constraint c(type, m_constraintData,
			m_fixed, m_locFixedPlane,
			m_rigid, m_locRigidSphere);
		EXPECT_EQ(1u, c.getNumDof());
	}

	{
		SCOPED_TRACE("1DOF for a frictionless contact between 2 fixed representations");
		Constraint c(type, m_constraintData, fixedRep, fixedLoc, fixedRep, fixedLoc);
		EXPECT_EQ(1u, c.getNumDof());
	}

	{
		SCOPED_TRACE("1DOF for a frictionless contact between 1 fixed representation and 1 rigid representation");
		Constraint c(type, m_constraintData, fixedRep, fixedLoc, rigidRep, rigidLoc);
		EXPECT_EQ(1u, c.getNumDof());
	}
}

// Test case: Rigid sphere at (0 0 0) with radius 0.01 colliding with Fixed plane Y=0
// Contact location on the rigid sphere is (0 -0.01 0)
// Contact location on the fixed plane is (0 0 0)
// Constraint: (Sphere - Plane).n >= 0 with n=(0 1 0) The normal should be the contact normal on the 2nd object
TEST_F (ConstraintTests, TestBuildMlcpSpherePlane)
{
	auto type = SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT;
	m_n.setZero();
	m_n[1] = 1.0;
	m_constraintData->setPlaneEquation(m_n, m_d);
	m_constraint = std::make_shared<Constraint>(type, m_constraintData,
												m_rigid, m_locRigidSphere,
												m_fixed, m_locFixedPlane);

	// Simulate 1 time step...to make sure all representation have a valid compliance matrix...
	{
		m_fixed->beforeUpdate(m_dt);
		m_rigid->beforeUpdate(m_dt);

		m_fixed->update(m_dt);
		m_rigid->update(m_dt);

		m_fixed->afterUpdate(m_dt);
		m_rigid->afterUpdate(m_dt);
	}

	// Fill up the Mlcp
	m_constraint->build(m_dt, &m_mlcpPhysicsProblem, m_indexSphereRepresentation, m_indexPlaneRepresentation,
		m_indexConstraint);

	// Violation b should be exactly -radius (the sphere center is on the plane)
	// This should not depend on the ordering of the object...the violation remains the same no matter what
	EXPECT_NEAR(-m_radius, m_mlcpPhysicsProblem.b[0], epsilon);

	// Constraint H should be
	// H = dt.[nx  ny  nz  nz.GPy-ny.GPz  nx.GPz-nz.GPx  ny.GPx-nx.GPy]
	// The rigid sphere being the 1st representation in the pair, it has the positive sign in the constraint !
	double sign = 1.0;
	Vector3d n_GP = m_n.cross(Vector3d(0.0, -m_radius, 0.0));

	SurgSim::Math::Matrix h = m_mlcpPhysicsProblem.H;
	EXPECT_NEAR(sign * m_dt * m_n[0] , h(0, 0), epsilon);
	EXPECT_NEAR(sign * m_dt * m_n[1] , h(0, 1), epsilon);
	EXPECT_NEAR(sign * m_dt * m_n[2] , h(0, 2), epsilon);
	EXPECT_NEAR(sign * m_dt * n_GP[0], h(0, 3), epsilon);
	EXPECT_NEAR(sign * m_dt * n_GP[1], h(0, 4), epsilon);
	EXPECT_NEAR(sign * m_dt * n_GP[2], h(0, 5), epsilon);

	// ConstraintTypes should contain 1 entry SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT
	ASSERT_EQ(1u, m_mlcpPhysicsProblem.constraintTypes.size());
	EXPECT_EQ(type, m_mlcpPhysicsProblem.constraintTypes[0]);
}

// Test case: Rigid sphere at (0 0 0) with radius 0.01 colliding with Fixed plane Y=0
// Contact location on the rigid sphere is (0 -0.01 0)
// Contact location on the fixed plane is (0 0 0)
// Constraint: (Plane - Sphere).n >= 0 with n=(0 -1 0) The normal should be the contact normal on the 2nd object
TEST_F (ConstraintTests, TestBuildMlcpPlaneSphere)
{
	auto type = SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT;
	m_n.setZero();
	m_n[1] = -1.0;
	m_constraintData->setPlaneEquation(m_n, m_d);
	m_constraint = std::make_shared<Constraint>(type, m_constraintData,
		m_fixed, m_locFixedPlane,
		m_rigid, m_locRigidSphere);

	// Simulate 1 time step...to make sure all representation have a valid compliance matrix...
	{
		m_fixed->beforeUpdate(m_dt);
		m_rigid->beforeUpdate(m_dt);

		m_fixed->update(m_dt);
		m_rigid->update(m_dt);

		m_fixed->afterUpdate(m_dt);
		m_rigid->afterUpdate(m_dt);
	}

	// Fill up the Mlcp
	m_constraint->build(m_dt, &m_mlcpPhysicsProblem, m_indexPlaneRepresentation, m_indexSphereRepresentation,
		m_indexConstraint);

	// Violation b should be exactly -radius (the sphere center is on the plane)
	// This should not depend on the ordering of the object...the violation remains the same no matter what
	EXPECT_NEAR(-m_radius, m_mlcpPhysicsProblem.b[0], epsilon);

	// Constraint H should be
	// H = dt.[nx  ny  nz  nz.GPy-ny.GPz  nx.GPz-nz.GPx  ny.GPx-nx.GPy]
	// The rigid sphere being the 2nd representation in the pair, it has the negative sign in the constraint !
	double sign = -1.0;
	Vector3d n_GP = m_n.cross(Vector3d(0.0, -m_radius, 0.0));

	SurgSim::Math::Matrix h = m_mlcpPhysicsProblem.H;
	EXPECT_NEAR(sign * m_dt * m_n[0] , h(0, 0), epsilon);
	EXPECT_NEAR(sign * m_dt * m_n[1] , h(0, 1), epsilon);
	EXPECT_NEAR(sign * m_dt * m_n[2] , h(0, 2), epsilon);
	EXPECT_NEAR(sign * m_dt * n_GP[0], h(0, 3), epsilon);
	EXPECT_NEAR(sign * m_dt * n_GP[1], h(0, 4), epsilon);
	EXPECT_NEAR(sign * m_dt * n_GP[2], h(0, 5), epsilon);

	// ConstraintTypes should contain 1 entry SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT
	ASSERT_EQ(1u, m_mlcpPhysicsProblem.constraintTypes.size());
	EXPECT_EQ(type, m_mlcpPhysicsProblem.constraintTypes[0]);
}

};  //  namespace Physics
};  //  namespace SurgSim
<commit_msg>ConstraintTests.cpp: Changed matrix variable h -> H<commit_after>// This file is a part of the OpenSurgSim project.
// Copyright 2013, SimQuest Solutions 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.

#include <memory>

#include <gtest/gtest.h>

#include "SurgSim/DataStructures/Location.h"
#include "SurgSim/Math/Quaternion.h"
#include "SurgSim/Math/RigidTransform.h"
#include "SurgSim/Math/SphereShape.h"
#include "SurgSim/Math/Vector.h"
#include "SurgSim/Physics/Constraint.h"
#include "SurgSim/Physics/ConstraintData.h"
#include "SurgSim/Physics/ContactConstraintData.h"
#include "SurgSim/Physics/FixedConstraintFrictionlessContact.h"
#include "SurgSim/Physics/FixedRepresentation.h"
#include "SurgSim/Physics/MlcpPhysicsProblem.h"
#include "SurgSim/Physics/RigidConstraintFrictionlessContact.h"
#include "SurgSim/Physics/RigidRepresentation.h"

using SurgSim::DataStructures::Location;
using SurgSim::Math::SphereShape;
using SurgSim::Math::Vector3d;

namespace
{
const double epsilon = 1e-10;
};

namespace SurgSim
{
namespace Physics
{

class ConstraintTests : public ::testing::Test
{
protected:
	/// Fixed plane pose
	SurgSim::Math::RigidTransform3d m_poseFixed;
	/// Rigid sphere pose
	SurgSim::Math::RigidTransform3d m_poseRigid;
	/// Contact normal direction
	Vector3d m_n;
	/// Distance to origin of the contact plane equation
	double m_d;
	/// Sphere radius
	double m_radius;
	/// Simulation time step
	double m_dt;
	/// Mlcp index of the sphere representation
	size_t m_indexSphereRepresentation;
	/// Mlcp index of the plane representation
	size_t m_indexPlaneRepresentation;
	/// Mlcp index of the constraint (frictionless contact)
	size_t m_indexConstraint;
	/// Contact location on the plane (point on the plane with the most penetration)
	Vector3d m_contactPositionPlane;
	/// Contact location on the sphere (point on the sphere with the most penetration)
	Vector3d m_contactPositionSphere;

	/// Rigid sphere
	std::shared_ptr<RigidRepresentation> m_rigid;
	/// Fixed plane
	std::shared_ptr<FixedRepresentation> m_fixed;

	/// Mlcp
	MlcpPhysicsProblem m_mlcpPhysicsProblem;
	/// Constraint
	std::shared_ptr<Constraint> m_constraint;
	/// Constraint data: frictionless contact
	std::shared_ptr<ContactConstraintData> m_constraintData;
	/// Location on the fixed plane
	Location m_locFixedPlane;
	/// Location on the rigid sphere
	Location m_locRigidSphere;

	/// Total number of degrees of freedom in the system (plane + sphere)
	size_t m_numDof;
	/// Total number of atomic constraint in the system (1 for a frictionless contact)
	size_t m_numConstraint;

	/// Setup the test case by creating all object
	void SetUp()
	{
		m_d = 0.0;
		m_radius = 0.01;
		m_dt = 1e-3;
		m_numDof = 0;
		m_indexSphereRepresentation = 0;
		m_indexConstraint = 0;
		m_numConstraint = 1;
		m_contactPositionPlane.setZero();
		m_contactPositionSphere.setZero();
		m_contactPositionSphere[1] = -m_radius;

		m_poseFixed.setIdentity();
		m_poseRigid.setIdentity();

		m_rigid = std::make_shared<RigidRepresentation>("Rigid");
		m_rigid->setLocalActive(true);
		m_rigid->setIsGravityEnabled(false);
		m_rigid->setLocalPose(m_poseRigid);
		{
			m_rigid->setDensity(1000.0);
			std::shared_ptr<SphereShape> shape = std::make_shared<SphereShape>(m_radius);
			m_rigid->setShape(shape);
		}
		m_numDof += m_rigid->getNumDof();

		m_indexPlaneRepresentation = m_indexSphereRepresentation + m_rigid->getNumDof();
		m_fixed = std::make_shared<FixedRepresentation>("Fixed");
		m_fixed->setLocalActive(true);
		m_fixed->setIsGravityEnabled(false);
		m_fixed->setLocalPose(m_poseFixed);
		m_numDof += m_fixed->getNumDof();

		m_locFixedPlane.rigidLocalPosition = m_contactPositionPlane;
		m_locRigidSphere.rigidLocalPosition = m_contactPositionSphere;

		m_constraintData = std::make_shared<ContactConstraintData>();

		clearMlcpPhysicsProblem(m_numDof, m_numConstraint);
	}

	/// Allocate and clear the Mlcp
	/// \param numDof The number of degrees of freedom in the system
	/// \param numConstraint The number of atomic constraints in the system
	void clearMlcpPhysicsProblem(size_t numDof, size_t numConstraint)
	{
		// Resize and zero all Eigen types
		m_mlcpPhysicsProblem.A.setZero(numConstraint, numConstraint);
		m_mlcpPhysicsProblem.b.setZero(numConstraint);
		m_mlcpPhysicsProblem.mu.setZero(numConstraint);
		m_mlcpPhysicsProblem.CHt.setZero(numDof, numConstraint);
		m_mlcpPhysicsProblem.H.resize(numConstraint, numDof);

		// Empty all std::vector types
		m_mlcpPhysicsProblem.constraintTypes.clear();
	}
};

TEST_F(ConstraintTests, TestConstructor)
{
	auto fixedRep = std::make_shared<FixedRepresentation>("fixed");
	auto rigidRep = std::make_shared<RigidRepresentation>("rigid");

	Location fixedLoc(m_contactPositionPlane);
	Location rigidLoc(m_contactPositionSphere);

	auto type = SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT;

	ASSERT_NO_THROW({Constraint c(type, m_constraintData, fixedRep, fixedLoc, rigidRep, rigidLoc);});

	EXPECT_THROW(
	{ Constraint c(type, nullptr, nullptr, fixedLoc, nullptr, fixedLoc); },
	SurgSim::Framework::AssertionFailure);
	EXPECT_THROW(
	{ Constraint c(type, m_constraintData, nullptr, fixedLoc, nullptr, fixedLoc); },
	SurgSim::Framework::AssertionFailure);
	EXPECT_THROW(
	{ Constraint c(type, m_constraintData, fixedRep, fixedLoc, nullptr, fixedLoc); },
	SurgSim::Framework::AssertionFailure);

	Constraint c(type, m_constraintData, fixedRep, fixedLoc, rigidRep, rigidLoc);

	EXPECT_EQ(m_constraintData, c.getData());
	EXPECT_EQ(type, c.getImplementations().first->getMlcpConstraintType());
	EXPECT_EQ(type, c.getImplementations().second->getMlcpConstraintType());
	EXPECT_EQ(fixedRep, c.getLocalizations().first->getRepresentation());
	EXPECT_EQ(rigidRep, c.getLocalizations().second->getRepresentation());
}

TEST_F(ConstraintTests, TestGetNumDof)
{
	auto fixedRep = std::make_shared<FixedRepresentation>("fixed");
	auto rigidRep = std::make_shared<RigidRepresentation>("rigid");

	Location fixedLoc(m_contactPositionPlane);
	Location rigidLoc(m_contactPositionSphere);

	auto type = SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT;

	{
		SCOPED_TRACE("1DOF for a frictionless contact");
		Constraint c(type, m_constraintData,
					 m_fixed, m_locFixedPlane,
					 m_rigid, m_locRigidSphere);
		EXPECT_EQ(1u, c.getNumDof());
	}

	{
		SCOPED_TRACE("1DOF for a frictionless contact between 2 fixed representations");
		Constraint c(type, m_constraintData, fixedRep, fixedLoc, fixedRep, fixedLoc);
		EXPECT_EQ(1u, c.getNumDof());
	}

	{
		SCOPED_TRACE("1DOF for a frictionless contact between 1 fixed representation and 1 rigid representation");
		Constraint c(type, m_constraintData, fixedRep, fixedLoc, rigidRep, rigidLoc);
		EXPECT_EQ(1u, c.getNumDof());
	}
}

// Test case: Rigid sphere at (0 0 0) with radius 0.01 colliding with Fixed plane Y=0
// Contact location on the rigid sphere is (0 -0.01 0)
// Contact location on the fixed plane is (0 0 0)
// Constraint: (Sphere - Plane).n >= 0 with n=(0 1 0) The normal should be the contact normal on the 2nd object
TEST_F(ConstraintTests, TestBuildMlcpSpherePlane)
{
	auto type = SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT;
	m_n.setZero();
	m_n[1] = 1.0;
	m_constraintData->setPlaneEquation(m_n, m_d);
	m_constraint = std::make_shared<Constraint>(type, m_constraintData,
				   m_rigid, m_locRigidSphere,
				   m_fixed, m_locFixedPlane);

	// Simulate 1 time step...to make sure all representation have a valid compliance matrix...
	{
		m_fixed->beforeUpdate(m_dt);
		m_rigid->beforeUpdate(m_dt);

		m_fixed->update(m_dt);
		m_rigid->update(m_dt);

		m_fixed->afterUpdate(m_dt);
		m_rigid->afterUpdate(m_dt);
	}

	// Fill up the Mlcp
	m_constraint->build(m_dt, &m_mlcpPhysicsProblem, m_indexSphereRepresentation, m_indexPlaneRepresentation,
						m_indexConstraint);

	// Violation b should be exactly -radius (the sphere center is on the plane)
	// This should not depend on the ordering of the object...the violation remains the same no matter what
	EXPECT_NEAR(-m_radius, m_mlcpPhysicsProblem.b[0], epsilon);

	// Constraint H should be
	// H = dt.[nx  ny  nz  nz.GPy-ny.GPz  nx.GPz-nz.GPx  ny.GPx-nx.GPy]
	// The rigid sphere being the 1st representation in the pair, it has the positive sign in the constraint !
	double sign = 1.0;
	Vector3d n_GP = m_n.cross(Vector3d(0.0, -m_radius, 0.0));

	SurgSim::Math::Matrix H = m_mlcpPhysicsProblem.H;
	EXPECT_NEAR(sign * m_dt * m_n[0] , H(0, 0), epsilon);
	EXPECT_NEAR(sign * m_dt * m_n[1] , H(0, 1), epsilon);
	EXPECT_NEAR(sign * m_dt * m_n[2] , H(0, 2), epsilon);
	EXPECT_NEAR(sign * m_dt * n_GP[0], H(0, 3), epsilon);
	EXPECT_NEAR(sign * m_dt * n_GP[1], H(0, 4), epsilon);
	EXPECT_NEAR(sign * m_dt * n_GP[2], H(0, 5), epsilon);

	// ConstraintTypes should contain 1 entry SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT
	ASSERT_EQ(1u, m_mlcpPhysicsProblem.constraintTypes.size());
	EXPECT_EQ(type, m_mlcpPhysicsProblem.constraintTypes[0]);
}

// Test case: Rigid sphere at (0 0 0) with radius 0.01 colliding with Fixed plane Y=0
// Contact location on the rigid sphere is (0 -0.01 0)
// Contact location on the fixed plane is (0 0 0)
// Constraint: (Plane - Sphere).n >= 0 with n=(0 -1 0) The normal should be the contact normal on the 2nd object
TEST_F(ConstraintTests, TestBuildMlcpPlaneSphere)
{
	auto type = SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT;
	m_n.setZero();
	m_n[1] = -1.0;
	m_constraintData->setPlaneEquation(m_n, m_d);
	m_constraint = std::make_shared<Constraint>(type, m_constraintData,
				   m_fixed, m_locFixedPlane,
				   m_rigid, m_locRigidSphere);

	// Simulate 1 time step...to make sure all representation have a valid compliance matrix...
	{
		m_fixed->beforeUpdate(m_dt);
		m_rigid->beforeUpdate(m_dt);

		m_fixed->update(m_dt);
		m_rigid->update(m_dt);

		m_fixed->afterUpdate(m_dt);
		m_rigid->afterUpdate(m_dt);
	}

	// Fill up the Mlcp
	m_constraint->build(m_dt, &m_mlcpPhysicsProblem, m_indexPlaneRepresentation, m_indexSphereRepresentation,
						m_indexConstraint);

	// Violation b should be exactly -radius (the sphere center is on the plane)
	// This should not depend on the ordering of the object...the violation remains the same no matter what
	EXPECT_NEAR(-m_radius, m_mlcpPhysicsProblem.b[0], epsilon);

	// Constraint H should be
	// H = dt.[nx  ny  nz  nz.GPy-ny.GPz  nx.GPz-nz.GPx  ny.GPx-nx.GPy]
	// The rigid sphere being the 2nd representation in the pair, it has the negative sign in the constraint !
	double sign = -1.0;
	Vector3d n_GP = m_n.cross(Vector3d(0.0, -m_radius, 0.0));

	SurgSim::Math::Matrix h = m_mlcpPhysicsProblem.H;
	EXPECT_NEAR(sign * m_dt * m_n[0] , h(0, 0), epsilon);
	EXPECT_NEAR(sign * m_dt * m_n[1] , h(0, 1), epsilon);
	EXPECT_NEAR(sign * m_dt * m_n[2] , h(0, 2), epsilon);
	EXPECT_NEAR(sign * m_dt * n_GP[0], h(0, 3), epsilon);
	EXPECT_NEAR(sign * m_dt * n_GP[1], h(0, 4), epsilon);
	EXPECT_NEAR(sign * m_dt * n_GP[2], h(0, 5), epsilon);

	// ConstraintTypes should contain 1 entry SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT
	ASSERT_EQ(1u, m_mlcpPhysicsProblem.constraintTypes.size());
	EXPECT_EQ(type, m_mlcpPhysicsProblem.constraintTypes[0]);
}

};  //  namespace Physics
};  //  namespace SurgSim
<|endoftext|>


<commit_before>/*=========================================================================

  Program:   Insight Segmentation & Registration Toolkit
  Module:    itkVectorGeometryTest.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

  Copyright (c) 2002 Insight Consortium. All rights reserved.
  See ITKCopyright.txt or http://www.itk.org/HTML/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notices for more information.

=========================================================================*/
/**
 *
 *  This program illustrates the use of Geometric objects
 *
 */

#include "itkVector.h"
#include <vnl/vnl_vector_ref.h>
#include <iostream>

  // Dimension & Type
  const     unsigned int    N = 3;
  typedef   double          ValueType;

  //  Vector type
  typedef    itk::Vector< ValueType, N >    VectorType;

//-------------------------
//
//   Main code
//
//-------------------------
int itkVectorGeometryTest(int, char**)
{

/*
  VectorType vv;
  vv = 0, 2, 4;

  if( vv[0] != 0 || vv[1] != 2 || vv[2] != 4 )
    {
    std::cerr << "Error initializing the Vector " << std::endl;
    return EXIT_FAILURE;
    }
  */

  VectorType va;
  va[0] = 1.0;
  va[1] = 2.0;
  va[2] = 7.0;

  std::cout << "va = { 1.0, 2.0, 7.0 } = ";
  std::cout << va << std::endl;

  VectorType vb;

  vb[0] = 1.0;
  vb[1] = 3.0;
  vb[2] = 5.0;

  std::cout << "vb = (1,3,5)   = ";
  std::cout << vb << std::endl;

  VectorType   vc  =  vb - va;
  std::cout << "vc  =  vb - va  = ";
  std::cout << vc << std::endl;

  VectorType   vd  =  va * 5.0;
  std::cout << "vd  =  va * 5.0 = ";
  std::cout << vd << std::endl;

  VectorType   ve  =  vd / 5.0;
  std::cout << "ve  =  vd * 5.0 = ";
  std::cout << ve << std::endl;

  vd += va;
  std::cout << "vd  +=  va      = ";
  std::cout << vd << std::endl;

  ve -= vb;
  std::cout << "ve  -=  vb      = ";
  std::cout << ve << std::endl;

  VectorType   vh  =  vb;
  std::cout << "vh   =  vb      = ";
  std::cout << vh << std::endl;

  VectorType   vg( va );
  std::cout << "vg( va )        = ";
  std::cout << vg << std::endl;

  ValueType norm2 = vg.GetSquaredNorm();
  std::cout << "vg squared norm = ";
  std::cout << norm2 << std::endl;

  ValueType norm  = vg.GetNorm();
  std::cout << "vg norm = ";
  std::cout << norm << std::endl;

  // Test for vnl interface

  // Test the no const version that returns an vnl_vector_ref
  vnl_vector_ref< ValueType > vnlVector = va.Get_vnl_vector();
  {
    std::cout << "vnl_vector_ref = va ";
    for( unsigned int i=0; i<N; i++ )
    {
      std::cout << vnlVector[i] << ", ";
    }
    std::cout << std::endl;

    std::cout << "vnl_vector_ref.begin() = va.Begin()";
    std::cout << std::endl;
    std::cout << vnlVector.begin() << " = ";
    std::cout << va.Begin() << std::endl;
  }

  // Test the const version that returns an vnl_vector
  const VectorType vf(va);
  vnl_vector<ValueType> vnlVector2 = vf.Get_vnl_vector();
  {
    std::cout << "vnl_vector = va ";
    for( unsigned int i=0; i<N; i++ )
    {
      std::cout << vnlVector2[i] << ", ";
    }
    std::cout << std::endl;

    std::cout << "vnl_vector.begin() != vf.Begin()";
    std::cout << std::endl;
    std::cout << vnlVector2.begin() << " = ";
    std::cout << vf.Begin() << std::endl;
  }

  // Test for operator== and operator!=
  {
    VectorType vv;
    vv[0] = 1;
    vv[1] = 3;
    vv[2] = 5;

    VectorType vw;
    vw.Fill( 0 );

    if( vv == vw )
      {
      std::cout << std::endl;
      std::cout << "Problem with operator==() " << std::endl;
      std::cout << "Vector " << vv;
      std::cout << " is reported as being equal to " << std::endl;
      std::cout << "Vector " << vw << std::endl;
      return EXIT_FAILURE;
      }

    VectorType ww;
    ww = vv;

    if( vv != ww )
    {
      std::cout << std::endl;
      std::cout << "Problem with operator!=() " << std::endl;
      std::cout << "Vector " << vv;
      std::cout << " is reported as being different from " << std::endl;
      std::cout << "Vector " << ww << std::endl;
      return EXIT_FAILURE;
    }

    if( !( vv == ww ) )
    {
      std::cout << std::endl;
      std::cout << "Problem with operator==() " << std::endl;
      std::cout << "Vector " << vv;
      std::cout << " is reported as not being equal to " << std::endl;
      std::cout << "Vector " << ww << std::endl;
      return EXIT_FAILURE;
    }

  }

  // Test for CastFrom() method
  {
  std::cout << "Test for CastFrom() method... ";

  // Dimension & Type
  const     unsigned int    N = 3;

  //  Vector Classes
  typedef    itk::Vector<  double, N >    DoubleVectorType;
  typedef    itk::Vector<  float , N >    FloatVectorType;

  DoubleVectorType dp;
  dp[0] = 1.0;
  dp[1] = 1.7;
  dp[2] = 1.9;

  FloatVectorType fp;
  fp[0] = 0.0;
  fp[1] = 0.0;
  fp[2] = 0.0;

  fp.CastFrom( dp );

  for(unsigned int i=0; i<N; i++)
    {
    FloatVectorType::ValueType val =
        static_cast< FloatVectorType::ValueType >( dp[i] );
    if( val != fp[i] )
      {
        std::cout << "Test failed at component " << i << std::endl;
        return EXIT_FAILURE;
      }
    }

  std::cout << " PASSED ! " << std::endl;

  }
  return EXIT_SUCCESS;

}

<commit_msg>ENH: globals move inside main()<commit_after>/*=========================================================================

  Program:   Insight Segmentation & Registration Toolkit
  Module:    itkVectorGeometryTest.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

  Copyright (c) 2002 Insight Consortium. All rights reserved.
  See ITKCopyright.txt or http://www.itk.org/HTML/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notices for more information.

=========================================================================*/
/**
 *
 *  This program illustrates the use of Geometric objects
 *
 */

#include "itkVector.h"
#include <vnl/vnl_vector_ref.h>
#include <iostream>

//-------------------------
//
//   Main code
//
//-------------------------
int itkVectorGeometryTest(int, char**)
{

  // Dimension & Type
  const     unsigned int    N = 3;
  typedef   double          ValueType;

  //  Vector type
  typedef    itk::Vector< ValueType, N >    VectorType;

/*
  VectorType vv;
  vv = 0, 2, 4;

  if( vv[0] != 0 || vv[1] != 2 || vv[2] != 4 )
    {
    std::cerr << "Error initializing the Vector " << std::endl;
    return EXIT_FAILURE;
    }
  */

  VectorType va;
  va[0] = 1.0;
  va[1] = 2.0;
  va[2] = 7.0;

  std::cout << "va = { 1.0, 2.0, 7.0 } = ";
  std::cout << va << std::endl;

  VectorType vb;

  vb[0] = 1.0;
  vb[1] = 3.0;
  vb[2] = 5.0;

  std::cout << "vb = (1,3,5)   = ";
  std::cout << vb << std::endl;

  VectorType   vc  =  vb - va;
  std::cout << "vc  =  vb - va  = ";
  std::cout << vc << std::endl;

  VectorType   vd  =  va * 5.0;
  std::cout << "vd  =  va * 5.0 = ";
  std::cout << vd << std::endl;

  VectorType   ve  =  vd / 5.0;
  std::cout << "ve  =  vd * 5.0 = ";
  std::cout << ve << std::endl;

  vd += va;
  std::cout << "vd  +=  va      = ";
  std::cout << vd << std::endl;

  ve -= vb;
  std::cout << "ve  -=  vb      = ";
  std::cout << ve << std::endl;

  VectorType   vh  =  vb;
  std::cout << "vh   =  vb      = ";
  std::cout << vh << std::endl;

  VectorType   vg( va );
  std::cout << "vg( va )        = ";
  std::cout << vg << std::endl;

  ValueType norm2 = vg.GetSquaredNorm();
  std::cout << "vg squared norm = ";
  std::cout << norm2 << std::endl;

  ValueType norm  = vg.GetNorm();
  std::cout << "vg norm = ";
  std::cout << norm << std::endl;

  // Test for vnl interface

  // Test the no const version that returns an vnl_vector_ref
  vnl_vector_ref< ValueType > vnlVector = va.Get_vnl_vector();
  {
    std::cout << "vnl_vector_ref = va ";
    for( unsigned int i=0; i<N; i++ )
    {
      std::cout << vnlVector[i] << ", ";
    }
    std::cout << std::endl;

    std::cout << "vnl_vector_ref.begin() = va.Begin()";
    std::cout << std::endl;
    std::cout << vnlVector.begin() << " = ";
    std::cout << va.Begin() << std::endl;
  }

  // Test the const version that returns an vnl_vector
  const VectorType vf(va);
  vnl_vector<ValueType> vnlVector2 = vf.Get_vnl_vector();
  {
    std::cout << "vnl_vector = va ";
    for( unsigned int i=0; i<N; i++ )
    {
      std::cout << vnlVector2[i] << ", ";
    }
    std::cout << std::endl;

    std::cout << "vnl_vector.begin() != vf.Begin()";
    std::cout << std::endl;
    std::cout << vnlVector2.begin() << " = ";
    std::cout << vf.Begin() << std::endl;
  }

  // Test for operator== and operator!=
  {
    VectorType vv;
    vv[0] = 1;
    vv[1] = 3;
    vv[2] = 5;

    VectorType vw;
    vw.Fill( 0 );

    if( vv == vw )
      {
      std::cout << std::endl;
      std::cout << "Problem with operator==() " << std::endl;
      std::cout << "Vector " << vv;
      std::cout << " is reported as being equal to " << std::endl;
      std::cout << "Vector " << vw << std::endl;
      return EXIT_FAILURE;
      }

    VectorType ww;
    ww = vv;

    if( vv != ww )
    {
      std::cout << std::endl;
      std::cout << "Problem with operator!=() " << std::endl;
      std::cout << "Vector " << vv;
      std::cout << " is reported as being different from " << std::endl;
      std::cout << "Vector " << ww << std::endl;
      return EXIT_FAILURE;
    }

    if( !( vv == ww ) )
    {
      std::cout << std::endl;
      std::cout << "Problem with operator==() " << std::endl;
      std::cout << "Vector " << vv;
      std::cout << " is reported as not being equal to " << std::endl;
      std::cout << "Vector " << ww << std::endl;
      return EXIT_FAILURE;
    }

  }

  // Test for CastFrom() method
  {
  std::cout << "Test for CastFrom() method... ";

  // Dimension & Type
  const     unsigned int    N = 3;

  //  Vector Classes
  typedef    itk::Vector<  double, N >    DoubleVectorType;
  typedef    itk::Vector<  float , N >    FloatVectorType;

  DoubleVectorType dp;
  dp[0] = 1.0;
  dp[1] = 1.7;
  dp[2] = 1.9;

  FloatVectorType fp;
  fp[0] = 0.0;
  fp[1] = 0.0;
  fp[2] = 0.0;

  fp.CastFrom( dp );

  for(unsigned int i=0; i<N; i++)
    {
    FloatVectorType::ValueType val =
        static_cast< FloatVectorType::ValueType >( dp[i] );
    if( val != fp[i] )
      {
        std::cout << "Test failed at component " << i << std::endl;
        return EXIT_FAILURE;
      }
    }

  std::cout << " PASSED ! " << std::endl;

  }
  return EXIT_SUCCESS;

}

<|endoftext|>