All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
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 //
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 //
8 // http://www.apache.org/licenses/LICENSE-2.0
9 //
10 // Unless required by applicable law or agreed to in writing, software
11 // distributed under the License is distributed on an "AS IS" BASIS,
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