// 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; } }