void ShapeGroupElement::visit(boost::function<
boost::function<void(void)>
(const ShapeInfo &info, const Coordinate &relativeTo, const VectorTransformation2D &foldedTransform, bool isGroup, const VectorTransformation2D &thisTransform)
> visitor, const Coordinate &relativeTo, const VectorTransformation2D &parentFoldedTransform) const
{
const ShapeInfo &info = m_shapeInfo.get_value_or(ShapeInfo());
Coordinate coord = info.m_coordinates.get_value_or(Coordinate());
double centerX = ((double)coord.m_xs + (double)coord.m_xe) / (2 * EMUS_IN_INCH);
double centerY = ((double)coord.m_ys + (double)coord.m_ye) / (2 * EMUS_IN_INCH);
double relativeCenterX = ((double)relativeTo.m_xs + (double)relativeTo.m_xe) / (2 * EMUS_IN_INCH);
double relativeCenterY = ((double)relativeTo.m_ys + (double)relativeTo.m_ye) / (2 * EMUS_IN_INCH);
double offsetX = centerX - relativeCenterX;
double offsetY = centerY - relativeCenterY;
VectorTransformation2D foldedTransform = VectorTransformation2D::fromTranslate(-offsetX, -offsetY)
* parentFoldedTransform * VectorTransformation2D::fromTranslate(offsetX, offsetY) * m_transform;
boost::function<void(void)> afterOp = visitor(info, relativeTo, foldedTransform, isGroup(), m_transform);
for (unsigned i = 0; i < m_children.size(); ++i)
{
m_children[i]->visit(visitor, coord, foldedTransform);
}
afterOp();
}
//==============================================================================
TEST(Issue1184, Accuracy)
{
struct ShapeInfo
{
dart::dynamics::ShapePtr shape;
double offset;
};
std::function<ShapeInfo()> makePlaneGround =
[]()
{
return ShapeInfo{
std::make_shared<dart::dynamics::PlaneShape>(
Eigen::Vector3d::UnitZ(), 0.0),
0.0};
};
std::function<ShapeInfo()> makeBoxGround =
[]()
{
const double thickness = 0.1;
return ShapeInfo{
std::make_shared<dart::dynamics::BoxShape>(
Eigen::Vector3d(100.0, 100.0, thickness)),
-thickness/2.0};
};
std::function<dart::dynamics::ShapePtr(double)> makeBoxObject =
[](const double s) -> dart::dynamics::ShapePtr
{
return std::make_shared<dart::dynamics::BoxShape>(
Eigen::Vector3d::Constant(2*s));
};
std::function<dart::dynamics::ShapePtr(double)> makeSphereObject =
[](const double s) -> dart::dynamics::ShapePtr
{
return std::make_shared<dart::dynamics::SphereShape>(s);
};
#ifndef NDEBUG
const auto groundInfoFunctions = {makePlaneGround};
const auto objectShapeFunctions = {makeSphereObject};
const auto halfsizes = {10.0};
const auto fallingModes = {true};
const double dropHeight = 0.1;
const double tolerance = 1e-3;
#else
const auto groundInfoFunctions = {makePlaneGround, makeBoxGround};
const auto objectShapeFunctions = {makeBoxObject, makeSphereObject};
const auto halfsizes = {0.25, 1.0, 5.0, 10.0, 20.0};
const auto fallingModes = {true, false};
const double dropHeight = 1.0;
const double tolerance = 1e-3;
#endif
for(const auto& groundInfoFunction : groundInfoFunctions)
{
for(const auto& objectShapeFunction : objectShapeFunctions)
{
for(const double halfsize : halfsizes)
{
for(const bool falling : fallingModes)
{
auto world = dart::simulation::World::create("test");
world->getConstraintSolver()->setCollisionDetector(
dart::collision::BulletCollisionDetector::create());
Eigen::Isometry3d tf_object = Eigen::Isometry3d::Identity();
const double initialHeight = falling? dropHeight+halfsize : halfsize;
tf_object.translate(initialHeight*Eigen::Vector3d::UnitZ());
auto object = dart::dynamics::Skeleton::create("ball");
object->createJointAndBodyNodePair<dart::dynamics::FreeJoint>()
.first->setTransform(tf_object);
const auto objectShape = objectShapeFunction(halfsize);
object->getBodyNode(0)->createShapeNodeWith<
dart::dynamics::VisualAspect,
dart::dynamics::CollisionAspect>(objectShape);
world->addSkeleton(object);
const ShapeInfo groundInfo = groundInfoFunction();
auto ground = dart::dynamics::Skeleton::create("ground");
ground->createJointAndBodyNodePair<dart::dynamics::WeldJoint>()
.second->createShapeNodeWith<
dart::dynamics::VisualAspect,
dart::dynamics::CollisionAspect>(groundInfo.shape);
Eigen::Isometry3d tf_ground = Eigen::Isometry3d::Identity();
tf_ground.translate(groundInfo.offset*Eigen::Vector3d::UnitZ());
ground->getJoint(0)->setTransformFromParentBodyNode(tf_ground);
world->addSkeleton(ground);
// time until the object will strike
const double t_strike = falling?
sqrt(-2.0*dropHeight/world->getGravity()[2]) : 0.0;
// give the object some time to settle
const double min_time = 0.5;
const double t_limit = 30.0*t_strike + min_time;
double lowestHeight = std::numeric_limits<double>::infinity();
double time = 0.0;
while(time < t_limit)
{
world->step();
const double currentHeight =
object->getBodyNode(0)->getTransform().translation()[2];
if(currentHeight < lowestHeight)
lowestHeight = currentHeight;
time = world->getTime();
}
// The simulation should have run for at least two seconds
ASSERT_LE(min_time, time);
EXPECT_GE(halfsize+tolerance, lowestHeight)
<< "object type: " << objectShape->getType()
<< "\nground type: " << groundInfo.shape->getType()
<< "\nfalling: " << falling << "\n";
const double finalHeight =
object->getBodyNode(0)->getTransform().translation()[2];
EXPECT_NEAR(halfsize, finalHeight, tolerance)
<< "object type: " << objectShape->getType()
<< "\nground type: " << groundInfo.shape->getType()
<< "\nfalling: " << falling << "\n";
}
}
}
}
}
