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>
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>
113 vector->segment(blockSize * blockId, blockSize) = subVector;
125 template <
class Vector>
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