// Computes surface tangent vectors for many normal vectors at once
// INPUTS:
// normals: (3 x m) matrix where each column is a normal vector in world coordinates
// OUTPUTS:
// tangents: list of k (3 x m) matrices where each column contains one of the k tangent vectors
// for the corresponding normal.
// NOTE:
// k = BASIS_VECTOR_HALF_COUNT is defined as a preprocessor directive so that
// Eigen templates can be optimized at compile time
void RigidBodyManipulator::surfaceTangents(Map<Matrix3xd> const & normals, std::vector< Map<Matrix3xd> > & tangents)
{
const size_t numContactPairs = normals.cols();
for (size_t curNormal = 0 ; curNormal < numContactPairs; curNormal++) {
Matrix3kd d;
surfaceTangentsSingle(normals.col(curNormal), d);
for (size_t k = 0 ; k < BASIS_VECTOR_HALF_COUNT ; k++) {
tangents[k].col(curNormal) = d.col(k);
}
}
}
void RigidBodyTree<T>::surfaceTangents(
Map<Matrix3Xd> const &normals,
// TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
std::vector<Map<Matrix3Xd> > &tangents) const {
const size_t numContactPairs = normals.cols();
for (size_t curNormal = 0; curNormal < numContactPairs; curNormal++) {
Matrix3kd d;
surfaceTangentsSingle(normals.col(curNormal), d);
for (size_t k = 0; k < BASIS_VECTOR_HALF_COUNT; k++) {
tangents[k].col(curNormal) = d.col(k);
}
}
}
// Computes surface tangent vectors for a single normal vector
// INPUTS:
// normal: (3 x 1) normal vector in world coordinates
// OUTPUTS:
// d: (3 x k) matrix where the k columns contain the surface tangents
// NOTE:
// k = BASIS_VECTOR_HALF_COUNT is defined as a preprocessor directive so that
// Eigen templates can be optimized at compile time
void surfaceTangentsSingle(Vector3d const &normal, Matrix3kd &d) {
Vector3d t1, t2;
double theta;
if (1.0 - normal(2) < EPSILON) { // handle the unit-normal case (since it's
// unit length, just check z)
t1 << 1.0, 0.0, 0.0;
} else if (1 + normal(2) < EPSILON) {
t1 << -1.0, 0.0, 0.0; // same for the reflected case
} else { // now the general case
t1 << normal(1), -normal(0), 0.0;
t1 /= sqrt(normal(1) * normal(1) + normal(0) * normal(0));
}
t2 = t1.cross(normal);
for (size_t k = 0; k < BASIS_VECTOR_HALF_COUNT; k++) {
theta = k * M_PI / BASIS_VECTOR_HALF_COUNT;
d.col(k) = cos(theta) * t1 + sin(theta) * t2;
}
}
