void CollisionWorldDistanceField::updateDistanceObject(const std::string& id,
boost::shared_ptr<CollisionWorldDistanceField::DistanceFieldCacheEntry>& dfce,
EigenSTL::vector_Vector3d& add_points,
EigenSTL::vector_Vector3d& subtract_points)
{
std::map<std::string, std::vector<PosedBodyPointDecompositionPtr> >::iterator cur_it = dfce->posed_body_point_decompositions_.find(id);
if(cur_it != dfce->posed_body_point_decompositions_.end()) {
for(unsigned int i = 0; i < cur_it->second.size(); i++) {
subtract_points.insert(subtract_points.end(),
cur_it->second[i]->getCollisionPoints().begin(),
cur_it->second[i]->getCollisionPoints().end());
}
}
ObjectConstPtr object = getObject(id);
if(object) {
std::vector<PosedBodyPointDecompositionPtr> shape_points;
for(unsigned int i = 0; i < object->shapes_.size(); i++) {
BodyDecompositionConstPtr bd = getBodyDecompositionCacheEntry(object->shapes_[i],
resolution_);
shape_points.push_back(boost::make_shared<PosedBodyPointDecomposition>(bd, object->shape_poses_[i]));
add_points.insert(add_points.end(),
shape_points.back()->getCollisionPoints().begin(),
shape_points.back()->getCollisionPoints().end());
}
dfce->posed_body_point_decompositions_[id] = shape_points;
} else {
dfce->posed_body_point_decompositions_.erase(id);
}
}
void mesh_core::Plane::leastSquaresGeneral(
const EigenSTL::vector_Vector3d& points,
Eigen::Vector3d* average)
{
if (points.empty())
{
normal_ = Eigen::Vector3d(0,0,1);
d_ = 0;
if (average)
*average = Eigen::Vector3d::Zero();
return;
}
// find c, the average of the points
Eigen::Vector3d c;
c.setZero();
EigenSTL::vector_Vector3d::const_iterator p = points.begin();
EigenSTL::vector_Vector3d::const_iterator end = points.end();
for ( ; p != end ; ++p)
c += *p;
c *= 1.0/double(points.size());
// Find the matrix
Eigen::Matrix3d m;
m.setZero();
p = points.begin();
for ( ; p != end ; ++p)
{
Eigen::Vector3d cp = *p - c;
m(0,0) += cp.x() * cp.x();
m(1,0) += cp.x() * cp.y();
m(2,0) += cp.x() * cp.z();
m(1,1) += cp.y() * cp.y();
m(2,1) += cp.y() * cp.z();
m(2,2) += cp.z() * cp.z();
}
m(0,1) = m(1,0);
m(0,2) = m(2,0);
m(1,2) = m(2,1);
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(m);
if (eigensolver.info() == Eigen::Success)
{
normal_ = eigensolver.eigenvectors().col(0);
normal_.normalize();
}
else
{
normal_ = Eigen::Vector3d(0,0,1);
}
d_ = -c.dot(normal_);
if (average)
*average = c;
}
void mesh_core::generateAABB(
const EigenSTL::vector_Vector3d& points,
Eigen::Vector3d& min,
Eigen::Vector3d& max)
{
min = Eigen::Vector3d(std::numeric_limits<double>::max(),
std::numeric_limits<double>::max(),
std::numeric_limits<double>::max());
max = Eigen::Vector3d(-std::numeric_limits<double>::max(),
-std::numeric_limits<double>::max(),
-std::numeric_limits<double>::max());
EigenSTL::vector_Vector3d::const_iterator it = points.begin();
EigenSTL::vector_Vector3d::const_iterator end = points.end();
for ( ; it != end ; ++it)
{
min = min.array().min(it->array());
max = max.array().max(it->array());
}
}
void mesh_core::Plane::leastSquaresFast(
const EigenSTL::vector_Vector3d& points,
Eigen::Vector3d* average)
{
Eigen::Matrix3d m;
Eigen::Vector3d b;
Eigen::Vector3d c;
m.setZero();
b.setZero();
c.setZero();
EigenSTL::vector_Vector3d::const_iterator p = points.begin();
EigenSTL::vector_Vector3d::const_iterator end = points.end();
for ( ; p != end ; ++p)
{
m(0,0) += p->x() * p->x();
m(1,0) += p->x() * p->y();
m(2,0) += p->x();
m(1,1) += p->y() * p->y();
m(2,1) += p->y();
b(0) += p->x() * p->z();
b(1) += p->y() * p->z();
b(2) += p->z();
c += *p;
}
m(0,1) = m(1,0);
m(0,2) = m(2,0);
m(1,2) = m(2,1);
m(2,2) = double(points.size());
c *= 1.0/double(points.size());
normal_ = m.colPivHouseholderQr().solve(b);
if (normal_.squaredNorm() > std::numeric_limits<double>::epsilon())
normal_.normalize();
d_ = -c.dot(normal_);
if (average)
*average = c;
}