Vector.h
Go to the documentation of this file.
1 // This file is a part of the OpenSurgSim project.
2 // Copyright 2012-2015, 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 <array>
23 #include <vector>
24 
25 #include <Eigen/Core>
26 #include <Eigen/Geometry>
27 
29 
30 namespace SurgSim
31 {
32 namespace Math
33 {
34 
37 typedef Eigen::Matrix<float, 2, 1> Vector2f;
38 
41 typedef Eigen::Matrix<float, 3, 1> Vector3f;
42 
45 typedef Eigen::Matrix<float, 4, 1> Vector4f;
46 
49 typedef Eigen::Matrix<float, 6, 1> Vector6f;
50 
53 typedef Eigen::Matrix<double, 2, 1> Vector2d;
54 
57 typedef Eigen::Matrix<double, 3, 1> Vector3d;
58 
61 typedef Eigen::Matrix<double, 4, 1> Vector4d;
62 
65 typedef Eigen::Matrix<double, 6, 1> Vector6d;
66 
68 typedef Eigen::Matrix<double, Eigen::Dynamic, 1> Vector;
69 
77 template <class Vector, class SubVector>
78 void addSubVector(const SubVector& subVector, size_t blockId, size_t blockSize, Vector* vector)
79 {
80  vector->segment(blockSize * blockId, blockSize) += subVector;
81 }
82 
90 template <class Vector, class SubVector>
91 void addSubVector(const SubVector& subVector, const std::vector<size_t> blockIds, size_t blockSize, Vector* vector)
92 {
93  const size_t numBlocks = blockIds.size();
94 
95  for (size_t block = 0; block < numBlocks; block++)
96  {
97  size_t blockId = blockIds[block];
98 
99  vector->segment(blockSize * blockId, blockSize) += subVector.segment(blockSize * block, blockSize);
100  }
101 }
102 
110 template <class Vector, class SubVector>
111 void setSubVector(const SubVector& subVector, size_t blockId, size_t blockSize, Vector* vector)
112 {
113  vector->segment(blockSize * blockId, blockSize) = subVector;
114 }
115 
125 template <class Vector>
126 Eigen::VectorBlock<Vector> getSubVector(Vector& vector, size_t blockId, size_t blockSize) // NOLINT
127 {
128  return vector.segment(blockSize * blockId, blockSize);
129 }
130 
138 template <class Vector, class SubVector>
139 void getSubVector(const Vector& vector, const std::vector<size_t> blockIds, size_t blockSize, SubVector* subVector)
140 {
141  const size_t numBlocks = blockIds.size();
142 
143  for (size_t block = 0; block < numBlocks; block++)
144  {
145  size_t blockId = blockIds[block];
146 
147  subVector->segment(blockSize * block, blockSize) = vector.segment(blockSize * blockId, blockSize);
148  }
149 }
150 
161 template <typename T, int size, int TOpt>
162 Eigen::Matrix<T, size, 1, TOpt> interpolate(
163  const Eigen::Matrix<T, size, 1, TOpt>& previous,
164  const Eigen::Matrix<T, size, 1, TOpt>& next,
165  T t)
166 {
167  return previous + t * (next - previous);
168 }
169 
177 template <class T, int VOpt>
178 bool buildOrthonormalBasis(Eigen::Matrix<T, 3, 1, VOpt>* i,
179  Eigen::Matrix<T, 3, 1, VOpt>* j,
180  Eigen::Matrix<T, 3, 1, VOpt>* k)
181 {
182  SURGSIM_ASSERT(i != nullptr) << "Parameter [in, out] 'i' is a nullptr";
183  SURGSIM_ASSERT(j != nullptr) << "Parameter [out] 'j' is a nullptr";
184  SURGSIM_ASSERT(k != nullptr) << "Parameter [out] 'k' is a nullptr";
185 
186  if (i->isZero())
187  {
188  return false;
189  }
190 
191  i->normalize();
192  *j = i->unitOrthogonal();
193  *k = i->cross(*j);
194 
195  return true;
196 }
197 
205 template <class T, int VOpt>
206 Eigen::Matrix<T, 3, 1, VOpt> robustCrossProduct(const std::array<Eigen::Matrix<T, 3, 1, VOpt>, 2>& p,
207  const std::array<Eigen::Matrix<T, 3, 1, VOpt>, 2>& q,
208  T epsilon)
209 {
210 
211  auto p0p1 = p[1] - p[0];
212  auto p1q0 = q[0] - p[1];
213  auto p0q0 = q[0] - p[0];
214  auto p1q1 = q[1] - p[1];
215  auto pXq = p0p1.cross(p1q0);
216  auto norm = pXq.norm();
217  if (norm < epsilon)
218  {
219  pXq = p0p1.cross(p0q0);
220  norm = pXq.norm();
221  }
222  if (norm < epsilon)
223  {
224  pXq = p0p1.cross(p1q1);
225  norm = pXq.norm();
226  }
227  pXq *= static_cast<T>(1.0 / norm);
228  return pXq;
229 }
230 
231 }; // namespace Math
232 }; // namespace SurgSim
233 
234 #endif // SURGSIM_MATH_VECTOR_H
SURGSIM_ASSERT
#define SURGSIM_ASSERT(condition)
Assert that condition is true.
Definition: Assert.h:77
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::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::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
Assert.h
The header that provides the assertion API.
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::Vector3d
Eigen::Matrix< double, 3, 1 > Vector3d
A 3D vector of doubles.
Definition: Vector.h:57
SurgSim::Math::Vector4f
Eigen::Matrix< float, 4, 1 > Vector4f
A 4D vector of floats.
Definition: Vector.h:45
SurgSim
Definition: CompoundShapeToGraphics.cpp:30
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::Vector3f
Eigen::Matrix< float, 3, 1 > Vector3f
A 3D vector of floats.
Definition: Vector.h:41
SurgSim::Math::Vector4d
Eigen::Matrix< double, 4, 1 > Vector4d
A 4D vector of doubles.
Definition: Vector.h:61
SurgSim::Math::Vector2d
Eigen::Matrix< double, 2, 1 > Vector2d
A 2D vector of doubles.
Definition: Vector.h:53
SurgSim::Math::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
A dynamic size column vector.
Definition: Vector.h:68
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::Vector6d
Eigen::Matrix< double, 6, 1 > Vector6d
A 6D matrix of doubles.
Definition: Vector.h:65
SurgSim::Math::Vector2f
Eigen::Matrix< float, 2, 1 > Vector2f
A 2D vector of floats.
Definition: Vector.h:37
SurgSim::Math::Vector6f
Eigen::Matrix< float, 6, 1 > Vector6f
A 6D vector of floats.
Definition: Vector.h:49