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, size_t blockId, size_t blockSize, Vector* vector)
{
vector->segment(blockSize * blockId, blockSize) += subVector;
}
template <class Vector, class SubVector>
void addSubVector(const SubVector& subVector, const std::vector<size_t> blockIds, size_t blockSize, Vector* vector)
{
const size_t numBlocks = blockIds.size();
for (size_t block = 0; block < numBlocks; block++)
{
size_t blockId = blockIds[block];
vector->segment(blockSize * blockId, blockSize) += subVector.segment(blockSize * block, blockSize);
}
}
template <class Vector, class SubVector>
void setSubVector(const SubVector& subVector, size_t blockId, size_t blockSize, Vector* vector)
{
vector->segment(blockSize * blockId, blockSize) = subVector;
}
template <class Vector>
Eigen::VectorBlock<Vector> getSubVector(Vector& vector, size_t blockId, size_t blockSize) // NOLINT
{
return vector.segment(blockSize * blockId, blockSize);
}
template <class Vector, class SubVector>
void getSubVector(const Vector& vector, const std::vector<size_t> blockIds, size_t blockSize, SubVector* subVector)
{
const size_t numBlocks = blockIds.size();
for (size_t block = 0; block < numBlocks; block++)
{
size_t blockId = blockIds[block];
subVector->segment(blockSize * block, blockSize) = vector.segment(blockSize * blockId, 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
SurgSim
Definition: CompoundShapeToGraphics.cpp:29
SurgSim::Math::getSubVector
Eigen::VectorBlock< Vector > getSubVector(Vector &vector, size_t blockId, size_t blockSize)
Helper method to access a sub-vector from a vector, for the sake of clarity.
Definition: Vector.h:126
SurgSim::Math::Vector4f
Eigen::Matrix< float, 4, 1 > Vector4f
A 4D vector of floats.
Definition: Vector.h:45
SURGSIM_ASSERT
#define SURGSIM_ASSERT(condition)
Assert that condition is true.
Definition: Assert.h:77
SurgSim::Math::buildOrthonormalBasis
bool buildOrthonormalBasis(Eigen::Matrix< T, 3, 1, VOpt > *i, Eigen::Matrix< T, 3, 1, VOpt > *j, Eigen::Matrix< T, 3, 1, VOpt > *k)
Helper method to construct an orthonormal basis (i, j, k) given the 1st vector direction.
Definition: Vector.h:178
SurgSim::Math::addSubVector
void addSubVector(const SubVector &subVector, size_t blockId, size_t blockSize, Vector *vector)
Helper method to add a sub-vector into a vector, for the sake of clarity.
Definition: Vector.h:78
SurgSim::Math::interpolate
Eigen::Quaternion< T, QOpt > interpolate(const Eigen::Quaternion< T, QOpt > &q0, const Eigen::Quaternion< T, QOpt > &q1, T t)
Interpolate (slerp) between 2 quaternions.
Definition: Quaternion.h:149
SurgSim::Math::Vector6d
Eigen::Matrix< double, 6, 1 > Vector6d
A 6D matrix of doubles.
Definition: Vector.h:65
SurgSim::Math::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
A dynamic size column vector.
Definition: Vector.h:68
SurgSim::Math::Vector3f
Eigen::Matrix< float, 3, 1 > Vector3f
A 3D vector of floats.
Definition: Vector.h:41
SurgSim::Math::robustCrossProduct
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)
Calculate the best unit normal we can find in the direction of pXq for one of the endpoints of q...
Definition: Vector.h:206
SurgSim::Math::Vector6f
Eigen::Matrix< float, 6, 1 > Vector6f
A 6D vector of floats.
Definition: Vector.h:49
SurgSim::Math::Vector2d
Eigen::Matrix< double, 2, 1 > Vector2d
A 2D vector of doubles.
Definition: Vector.h:53
Assert.h
The header that provides the assertion API.
SurgSim::Math::setSubVector
void setSubVector(const SubVector &subVector, size_t blockId, size_t blockSize, Vector *vector)
Helper method to set a sub-vector into a vector, for the sake of clarity.
Definition: Vector.h:111
SurgSim::Math::Vector2f
Eigen::Matrix< float, 2, 1 > Vector2f
A 2D vector of floats.
Definition: Vector.h:37
SurgSim::Math::Vector4d
Eigen::Matrix< double, 4, 1 > Vector4d
A 4D vector of doubles.
Definition: Vector.h:61
SurgSim::Math::Vector3d
Eigen::Matrix< double, 3, 1 > Vector3d
A 3D vector of doubles.
Definition: Vector.h:57