std::vector<ContactSpaceSampleData> uniform_sample_Quat( libm3d::model& P, libm3d::model& Q, double d, double shift, std::size_t nrot, const Collider3D& collider, RNG& rng) { std::vector<ContactSpaceSampleData> samples; double r_[4]; double R[3][3]; DataVector v(7); for(std::size_t i = 0; i < nrot; ++i) { rng.quaternion(r_); Quaternion r(r_[0], r_[1], r_[2], r_[3]); Quat2Rot(R, r); std::vector<DataVector> samples_ = computePointMKDiff(P, Q, R, d, shift); for(std::size_t j = 0; j < samples_.size(); ++j) { v[0] = samples_[j][0]; v[1] = samples_[j][1]; v[2] = samples_[j][2]; v[3] = r_[0]; v[4] = r_[1]; v[5] = r_[2]; v[6] = r_[3]; bool col = collider.isCollide(v); samples.push_back(ContactSpaceSampleData(v, col)); } } return samples; }
std::vector<DataVector> uniform_sample0_Euler( libm3d::model& P, libm3d::model& Q, double d, double shift, std::size_t nrot, RNG& rng) { std::vector<DataVector> samples; double r_[4]; double R[3][3]; DataVector v(6); double a, b, c; for(std::size_t i = 0; i < nrot; ++i) { rng.quaternion(r_); Quaternion r(r_[0], r_[1], r_[2], r_[3]); Quat2Rot(R, r); Quat2Euler(a, b, c, r); std::vector<DataVector> samples_ = computePointMKDiff(P, Q, R, d, shift); for(std::size_t j = 0; j < samples_.size(); ++j) { v[0] = samples_[j][0]; v[1] = samples_[j][1]; v[2] = samples_[j][2]; v[3] = a; v[4] = b; v[5] = c; samples.push_back(v); } } return samples; }