19 #ifndef SURGSIM_MATH_VECTOR_H 20 #define SURGSIM_MATH_VECTOR_H 26 #include <Eigen/Geometry> 68 typedef Eigen::Matrix<double, Eigen::Dynamic, 1>
Vector;
77 template <
class Vector,
class SubVector>
78 void addSubVector(
const SubVector& subVector,
size_t blockId,
size_t blockSize, Vector* vector)
80 vector->segment(blockSize * blockId, blockSize) += subVector;
90 template <
class Vector,
class SubVector>
91 void addSubVector(
const SubVector& subVector,
const std::vector<size_t> blockIds,
size_t blockSize, Vector* vector)
93 const size_t numBlocks = blockIds.size();
95 for (
size_t block = 0; block < numBlocks; block++)
97 size_t blockId = blockIds[block];
99 vector->segment(blockSize * blockId, blockSize) += subVector.segment(blockSize * block, blockSize);
110 template <
class Vector,
class SubVector>
111 void setSubVector(
const SubVector& subVector,
size_t blockId,
size_t blockSize, Vector* vector)
113 vector->segment(blockSize * blockId, blockSize) = subVector;
125 template <
class Vector>
126 Eigen::VectorBlock<Vector>
getSubVector(Vector& vector,
size_t blockId,
size_t blockSize)
128 return vector.segment(blockSize * blockId, blockSize);
138 template <
class Vector,
class SubVector>
139 void getSubVector(
const Vector& vector,
const std::vector<size_t> blockIds,
size_t blockSize, SubVector* subVector)
141 const size_t numBlocks = blockIds.size();
143 for (
size_t block = 0; block < numBlocks; block++)
145 size_t blockId = blockIds[block];
147 subVector->segment(blockSize * block, blockSize) = vector.segment(blockSize * blockId, blockSize);
161 template <
typename T,
int size,
int TOpt>
163 const Eigen::Matrix<T, size, 1, TOpt>& previous,
164 const Eigen::Matrix<T, size, 1, TOpt>& next,
167 return previous + t * (next - previous);
177 template <
class T,
int VOpt>
179 Eigen::Matrix<T, 3, 1, VOpt>* j,
180 Eigen::Matrix<T, 3, 1, VOpt>* k)
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";
192 *j = i->unitOrthogonal();
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,
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();
219 pXq = p0p1.cross(p0q0);
224 pXq = p0p1.cross(p1q1);
227 pXq *=
static_cast<T
>(1.0 / norm);
234 #endif // SURGSIM_MATH_VECTOR_H Definition: CompoundShapeToGraphics.cpp:29
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
Eigen::Matrix< float, 4, 1 > Vector4f
A 4D vector of floats.
Definition: Vector.h:45
#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:178
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
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:65
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
A dynamic size column vector.
Definition: Vector.h:68
Eigen::Matrix< float, 3, 1 > Vector3f
A 3D vector of floats.
Definition: Vector.h:41
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
Eigen::Matrix< float, 6, 1 > Vector6f
A 6D vector of floats.
Definition: Vector.h:49
Eigen::Matrix< double, 2, 1 > Vector2d
A 2D vector of doubles.
Definition: Vector.h:53
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:111
Eigen::Matrix< float, 2, 1 > Vector2f
A 2D vector of floats.
Definition: Vector.h:37
Eigen::Matrix< double, 4, 1 > Vector4d
A 4D vector of doubles.
Definition: Vector.h:61
Eigen::Matrix< double, 3, 1 > Vector3d
A 3D vector of doubles.
Definition: Vector.h:57