Vector.h

Go to the documentation of this file.

// This file is a part of the OpenSurgSim project.
// Copyright 2012-2015, 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.


#ifndef SURGSIM_MATH_VECTOR_H
#define SURGSIM_MATH_VECTOR_H

#include <array>
#include <vector>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "SurgSim/Framework/Assert.h"

namespace SurgSim
{
namespace Math
{

typedef Eigen::Matrix<float,  2, 1>  Vector2f;

typedef Eigen::Matrix<float,  3, 1>  Vector3f;

typedef Eigen::Matrix<float,  4, 1>  Vector4f;

typedef Eigen::Matrix<float,  6, 1>  Vector6f;

typedef Eigen::Matrix<double, 2, 1>  Vector2d;

typedef Eigen::Matrix<double, 3, 1>  Vector3d;

typedef Eigen::Matrix<double, 4, 1>  Vector4d;

typedef Eigen::Matrix<double, 6, 1> Vector6d;

typedef Eigen::Matrix<double, Eigen::Dynamic, 1> Vector;

template <class Vector, class SubVector>
void addSubVector(const SubVector& subVector, Eigen::Index blockId, Eigen::Index blockSize, Vector* vector)
{
    vector->segment(blockSize * blockId, blockSize) += subVector;
}

template <class Vector, class SubVector>
void addSubVector(const Eigen::Ref<const SubVector>& subVector,
    const std::vector<size_t>& blockIds, Eigen::Index blockSize, Vector* vector)
{
    const Eigen::Index numBlocks = static_cast<Eigen::Index>(blockIds.size());
    for (Eigen::Index block = 0; block < numBlocks; ++block)
    {
        vector->segment(blockSize * static_cast<Eigen::Index>(blockIds[block]), blockSize) +=
            subVector.segment(blockSize * block, blockSize);
    }
}

template <class Vector, class SubVector>
void setSubVector(const SubVector& subVector, Eigen::Index blockId, Eigen::Index blockSize, Vector* vector)
{
    vector->segment(blockSize * blockId, blockSize) = subVector;
}

template <class Vector>
Eigen::VectorBlock<Vector> getSubVector(Vector& vector, Eigen::Index blockId, Eigen::Index blockSize) // NOLINT
{
    return vector.segment(blockSize * blockId, blockSize);
}

template <class Vector, class SubVector>
void getSubVector(const Vector& vector, const std::vector<size_t>& blockIds, Eigen::Index blockSize,
    SubVector* subVector)
{
    const Eigen::Index numBlocks = static_cast<Eigen::Index>(blockIds.size());
    for (Eigen::Index block = 0; block < numBlocks; ++block)
    {
        subVector->segment(blockSize * block, blockSize) =
            vector.segment(blockSize * static_cast<Eigen::Index>(blockIds[block]), blockSize);
    }
}

template <typename T, int size, int TOpt>
Eigen::Matrix<T, size, 1, TOpt> interpolate(
    const Eigen::Matrix<T, size, 1, TOpt>& previous,
    const Eigen::Matrix<T, size, 1, TOpt>& next,
    T t)
{
    return previous + t * (next - previous);
}

template <class T, int VOpt>
bool buildOrthonormalBasis(Eigen::Matrix<T, 3, 1, VOpt>* i,
                           Eigen::Matrix<T, 3, 1, VOpt>* j,
                           Eigen::Matrix<T, 3, 1, VOpt>* k)
{
    SURGSIM_ASSERT(i != nullptr) << "Parameter [in, out] 'i' is a nullptr";
    SURGSIM_ASSERT(j != nullptr) << "Parameter [out] 'j' is a nullptr";
    SURGSIM_ASSERT(k != nullptr) << "Parameter [out] 'k' is a nullptr";

    if (i->isZero())
    {
        return false;
    }

    i->normalize();
    *j = i->unitOrthogonal();
    *k = i->cross(*j);

    return true;
}

template <class T, int VOpt>
Eigen::Matrix<T, 3, 1, VOpt> robustCrossProduct(const std::array<Eigen::Matrix<T, 3, 1, VOpt>, 2>& p,
        const std::array<Eigen::Matrix<T, 3, 1, VOpt>, 2>& q,
        T epsilon)
{

    auto p0p1 = p[1] - p[0];
    auto p1q0 = q[0] - p[1];
    auto p0q0 = q[0] - p[0];
    auto p1q1 = q[1] - p[1];
    auto pXq = p0p1.cross(p1q0);
    auto norm = pXq.norm();
    if (norm < epsilon)
    {
        pXq = p0p1.cross(p0q0);
        norm = pXq.norm();
    }
    if (norm < epsilon)
    {
        pXq = p0p1.cross(p1q1);
        norm = pXq.norm();
    }
    pXq *= static_cast<T>(1.0 / norm);
    return pXq;
}

};  // namespace Math
};  // namespace SurgSim

#endif  // SURGSIM_MATH_VECTOR_H