19 #ifndef SURGSIM_MATH_VECTOR_H
20 #define SURGSIM_MATH_VECTOR_H
25 #include <Eigen/Geometry>
67 typedef Eigen::Matrix<double, Eigen::Dynamic, 1>
Vector;
76 template <
class Vector,
class SubVector>
77 void addSubVector(
const SubVector& subVector,
size_t blockId,
size_t blockSize, Vector* vector)
79 vector->segment(blockSize * blockId, blockSize) += subVector;
89 template <
class Vector,
class SubVector>
90 void addSubVector(
const SubVector& subVector,
const std::vector<size_t> blockIds,
size_t blockSize, Vector* vector)
92 const size_t numBlocks = blockIds.size();
94 for (
size_t block = 0; block < numBlocks; block++)
96 size_t blockId = blockIds[block];
98 vector->segment(blockSize * blockId, blockSize) += subVector.segment(blockSize * block, blockSize);
109 template <
class Vector,
class SubVector>
110 void setSubVector(
const SubVector& subVector,
size_t blockId,
size_t blockSize, Vector* vector)
112 vector->segment(blockSize * blockId, blockSize) = subVector;
124 template <
class Vector>
125 Eigen::VectorBlock<Vector>
getSubVector(Vector& vector,
size_t blockId,
size_t blockSize)
127 return vector.segment(blockSize * blockId, blockSize);
137 template <
class Vector,
class SubVector>
138 void getSubVector(
const Vector& vector,
const std::vector<size_t> blockIds,
size_t blockSize, SubVector* subVector)
140 const size_t numBlocks = blockIds.size();
142 for (
size_t block = 0; block < numBlocks; block++)
144 size_t blockId = blockIds[block];
146 subVector->segment(blockSize * block, blockSize) = vector.segment(blockSize * blockId, blockSize);
160 template <
typename T,
int size,
int TOpt>
162 const Eigen::Matrix<T, size, 1, TOpt> &previous,
163 const Eigen::Matrix<T, size, 1, TOpt> &next,
166 return previous + t * (next - previous);
176 template <
class T,
int VOpt>
178 Eigen::Matrix<T, 3, 1, VOpt>* j,
179 Eigen::Matrix<T, 3, 1, VOpt>* k)
181 SURGSIM_ASSERT(i !=
nullptr) <<
"Parameter [in, out] 'i' is a nullptr";
182 SURGSIM_ASSERT(j !=
nullptr) <<
"Parameter [out] 'j' is a nullptr";
183 SURGSIM_ASSERT(k !=
nullptr) <<
"Parameter [out] 'k' is a nullptr";
191 *j = i->unitOrthogonal();
199 #endif // SURGSIM_MATH_VECTOR_H
Definition: DriveElementFromInputBehavior.cpp:27
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:125
Eigen::Matrix< float, 4, 1 > Vector4f
A 4D vector of floats.
Definition: Vector.h:44
#define SURGSIM_ASSERT(condition)
Assert that condition is true.
Definition: Assert.h:77
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:177
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:77
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
Eigen::Matrix< double, 6, 1 > Vector6d
A 6D matrix of doubles.
Definition: Vector.h:64
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
A dynamic size column vector.
Definition: Vector.h:67
Eigen::Matrix< float, 3, 1 > Vector3f
A 3D vector of floats.
Definition: Vector.h:40
Eigen::Matrix< float, 6, 1 > Vector6f
A 6D vector of floats.
Definition: Vector.h:48
Eigen::Matrix< double, 2, 1 > Vector2d
A 2D vector of doubles.
Definition: Vector.h:52
The header that provides the assertion API.
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:110
Eigen::Matrix< float, 2, 1 > Vector2f
A 2D vector of floats.
Definition: Vector.h:36
Eigen::Matrix< double, 4, 1 > Vector4d
A 4D vector of doubles.
Definition: Vector.h:60
Eigen::Matrix< double, 3, 1 > Vector3d
A 3D vector of doubles.
Definition: Vector.h:56