// Gets a concatenated list of contact points corresponding to a single body regardless of whether it is body A or body B
// This allows all the points to be transformed into world space using a single forwardKin call.
// INPUTS:
// cindA: list of indexes into the original list of m contact pairs where the body appeared as body A
// cindB: list of iindexes into the original list of m contact pairs where the body appeared as body B
// xA: (3 x m) matrix where each column represents a contact point on body A
// xB: (3 x m) matrix where each column represents a contact point on body B
// OUTPUTS:
// bodyPoints: (4 x n) matrix of contact points containing occurrences of body A contact points followed
// by body B contact points where n = size(cindA) + size(cindB)
// NOTE: the output is a matrix of 4-vector columns in homogeneous coordinates (x,y,z,1)'
void getBodyPoints(std::vector<size_t> const & cindA, std::vector<size_t> const & cindB, Matrix3xd const & xA, Matrix3xd const & xB, MatrixXd & bodyPoints)
{
size_t i = 0;
size_t numPtsA = cindA.size();
size_t numPtsB = cindB.size();
bodyPoints.resize(4, numPtsA + numPtsB);
for (i = 0 ; i < numPtsA ; i++ ) {
bodyPoints.col(i) << xA.col(cindA[i]), 1.0; //homogeneous coordinates
}
for (i = 0 ; i < numPtsB ; i++ ) {
bodyPoints.col(numPtsA + i) << xB.col(cindB[i]), 1.0;
}
}
inline void buildSparseMatrix(Matrix3xd const & pts, SparseMatrix<double> & sparse)
{
const int m = pts.cols();
const int numNonZero = 3*m;
sparse.resize(m, numNonZero);
sparse.reserve(VectorXi::Constant(numNonZero, 1));
int j = 0;
for (int i = 0 ; i < m ; i++) {
for (int k = 0 ; k < 3 ; k++) {
sparse.insert(i, j) = pts(j);
j++;
}
}
}
inline void buildSparseMatrix(Matrix3xd const & pts, SparseMatrix<double> & sparse)
{
typedef SparseMatrix<double>::Index SparseIndex;
const SparseIndex m = static_cast<SparseIndex>(pts.cols());
const SparseIndex numNonZero = 3*m;
sparse.resize(m, numNonZero);
sparse.reserve(VectorXi::Constant(numNonZero, 1));
SparseIndex j = 0;
for (SparseIndex i = 0 ; i < m ; i++) {
for (SparseIndex k = 0 ; k < 3 ; k++) {
sparse.insert(i, j) = pts(j);
j++;
}
}
}
