Example #1
0
bool World::IsReachable(const Robot& robot, const Tree& tree, const Obstacle& obstacle) const
{
	Vector3  treePos = tree.GetPosition(); int room = ( tree.GetPosition()(1) > 0 ) ? 1 : - 1;
	const matrices::Matrix4& transform( robot.ToRobotCoordinates());
	numeric p1robPos = matrices::matrix4TimesVect3( transform, obstacle.GetP1())(1) + 0.05 * room;
	numeric p3robPos = matrices::matrix4TimesVect3( transform, obstacle.GetP3())(1) + 0.05 * room;
	if(! (p1robPos * room >= 0 || p3robPos * room >= 0))
	{
		return false;
	}
	//assert(pImpl_->instantiated_);
	return pImpl_->intersection_.Intersect(robot, tree, obstacle);
}
FilterDistance::FilterDistance(NUMBER treshold, const Tree& tree, const Vector3& target)
	: Filter_ABC()
	, treshold_(treshold)
	, target_(target)
	, treePos_(tree.GetPosition())
{
	// NOTHING
}
Sample::Sample(Tree& tree)
{
	Joint * j = tree.GetRoot();
	while(j)
	{
		angles_.push_back(j->GetTheta());
		position_ = j->GetS();
		j = j->pChild_;
	}
	position_ -= tree.GetPosition();
	jacobianProd_ = tree.GetJacobian()->GetJacobianProduct();
	jacobianProdInverse_ = tree.GetJacobian()->GetJacobianProductInverse();
}