10 _cp_local_force_list.clear();
11 _cp_penetration_list.clear();
13 _cp_frame_list.clear();
14 _cp_resti_list.clear();
22 for (
size_t i(0); i < _model->_nGroundContact; ++i) {
23 _cp_force_list[i].setZero();
24 for (
size_t j(0); j < _nCollision; ++j) {
25 if (_collision_list[j]->ContactDetection(_model->_pGC[i], penetration,
28 _cp_pos_list.push_back(_model->_pGC[i]);
30 _cp_frame_list.push_back(cp_frame);
31 _cp_penetration_list.push_back(penetration);
33 _idx_list.push_back(i);
35 _cp_resti_list.push_back(_collision_list[j]->getRestitutionCoeff());
36 _cp_mu_list.push_back(_collision_list[j]->getFrictionCoeff());
40 return _cp_pos_list.size();
typename Eigen::Matrix< T, 3, 3 > Mat3
typename Eigen::Matrix< T, 3, 1 > Vec3