Vector.h
Go to the documentation of this file.
1 // This file is a part of the OpenSurgSim project.
2 // Copyright 2012-2013, SimQuest Solutions Inc.
3 //
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 //
9 //
10 // Unless required by applicable law or agreed to in writing, software
12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 // See the License for the specific language governing permissions and
14 // limitations under the License.
15
18
19 #ifndef SURGSIM_MATH_VECTOR_H
20 #define SURGSIM_MATH_VECTOR_H
21
22 #include <vector>
23
24 #include <Eigen/Core>
25 #include <Eigen/Geometry>
26
28
29 namespace SurgSim
30 {
31 namespace Math
32 {
33
36 typedef Eigen::Matrix<float, 2, 1> Vector2f;
37
40 typedef Eigen::Matrix<float, 3, 1> Vector3f;
41
44 typedef Eigen::Matrix<float, 4, 1> Vector4f;
45
48 typedef Eigen::Matrix<float, 6, 1> Vector6f;
49
52 typedef Eigen::Matrix<double, 2, 1> Vector2d;
53
56 typedef Eigen::Matrix<double, 3, 1> Vector3d;
57
60 typedef Eigen::Matrix<double, 4, 1> Vector4d;
61
64 typedef Eigen::Matrix<double, 6, 1> Vector6d;
65
67 typedef Eigen::Matrix<double, Eigen::Dynamic, 1> Vector;
68
76 template <class Vector, class SubVector>
77 void addSubVector(const SubVector& subVector, size_t blockId, size_t blockSize, Vector* vector)
78 {
79  vector->segment(blockSize * blockId, blockSize) += subVector;
80 }
81
89 template <class Vector, class SubVector>
90 void addSubVector(const SubVector& subVector, const std::vector<size_t> blockIds, size_t blockSize, Vector* vector)
91 {
92  const size_t numBlocks = blockIds.size();
93
94  for (size_t block = 0; block < numBlocks; block++)
95  {
96  size_t blockId = blockIds[block];
97
98  vector->segment(blockSize * blockId, blockSize) += subVector.segment(blockSize * block, blockSize);
99  }
100 }
101
109 template <class Vector, class SubVector>
110 void setSubVector(const SubVector& subVector, size_t blockId, size_t blockSize, Vector* vector)
111 {
112  vector->segment(blockSize * blockId, blockSize) = subVector;
113 }
114
124 template <class Vector>
125 Eigen::VectorBlock<Vector> getSubVector(Vector& vector, size_t blockId, size_t blockSize) // NOLINT
126 {
127  return vector.segment(blockSize * blockId, blockSize);
128 }
129
137 template <class Vector, class SubVector>
138 void getSubVector(const Vector& vector, const std::vector<size_t> blockIds, size_t blockSize, SubVector* subVector)
139 {
140  const size_t numBlocks = blockIds.size();
141
142  for (size_t block = 0; block < numBlocks; block++)
143  {
144  size_t blockId = blockIds[block];
145
146  subVector->segment(blockSize * block, blockSize) = vector.segment(blockSize * blockId, blockSize);
147  }
148 }
149
160 template <typename T, int size, int TOpt>
161 Eigen::Matrix<T, size, 1, TOpt> interpolate(
162  const Eigen::Matrix<T, size, 1, TOpt> &previous,
163  const Eigen::Matrix<T, size, 1, TOpt> &next,
164  T t)
165 {
166  return previous + t * (next - previous);
167 }
168
176 template <class T, int VOpt>
177 bool buildOrthonormalBasis(Eigen::Matrix<T, 3, 1, VOpt>* i,
178  Eigen::Matrix<T, 3, 1, VOpt>* j,
179  Eigen::Matrix<T, 3, 1, VOpt>* k)
180 {
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";
184
185  if (i->isZero())
186  {
187  return false;
188  }
189
190  i->normalize();
191  *j = i->unitOrthogonal();
192  *k = i->cross(*j);
193
194  return true;
195 }
196 }; // namespace Math
197 }; // namespace SurgSim
198
199 #endif // SURGSIM_MATH_VECTOR_H
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