TEST_F(OsgSkeletonRepresentationRenderTests, WithNeutralPoseTest)
{
auto graphics = std::make_shared<SurgSim::Graphics::OsgSkeletonRepresentation>("Skeleton");
graphics->loadModel("OsgSkeletonRepresentationRenderTests/rigged_cylinder.osgt");
graphics->setSkinningShaderFileName("Shaders/skinning.vert");
graphics->setNeutralBonePose("Bone",
makeRigidTransform(makeRotationQuaternion(M_PI_4, Vector3d(1.0, 1.0, 1.0)), Vector3d::Zero()));
auto sceneElement = std::make_shared<SurgSim::Framework::BasicSceneElement>("Rigged Cylinder");
sceneElement->addComponent(graphics);
scene->addSceneElement(sceneElement);
viewElement->setPose(SurgSim::Math::makeRigidTransform(
Vector3d(-0.3, 0.3, -0.3),
Vector3d(0.0, 0.0, 0.0),
Vector3d(0.0, 0.0, 1.0)));
runtime->start();
boost::this_thread::sleep(boost::posix_time::milliseconds(2000));
std::pair<RigidTransform3d, RigidTransform3d> rootTransform;
rootTransform.first =
makeRigidTransform(makeRotationQuaternion(0.0, Vector3d(1.0, 1.0, 1.0)), Vector3d::Zero());
rootTransform.second =
makeRigidTransform(makeRotationQuaternion(0.0, Vector3d(1.0, 1.0, 1.0)), Vector3d::Zero());
std::pair<RigidTransform3d, RigidTransform3d> boneTransform;
boneTransform.first =
makeRigidTransform(makeRotationQuaternion(0.0, Vector3d(1.0, 0.0, 0.0)), Vector3d::Zero());
boneTransform.second =
makeRigidTransform(makeRotationQuaternion(0.0, Vector3d(1.0, 0.0, 0.0)), Vector3d(0.0, 4.0, 0.0));
int numSteps = 1000;
for (int i = 0; i < numSteps; ++i)
{
double t = static_cast<double>(i) / numSteps;
RigidTransform3d pose = interpolate(rootTransform, t);
sceneElement->setPose(pose);
pose = interpolate(boneTransform, t / 3.0);
graphics->setBonePose("Bone", RigidTransform3d::Identity());
graphics->setBonePose("Bone_001", pose);
graphics->setBonePose("Bone_002", pose);
graphics->setBonePose("Bone_003", pose);
/// The total number of steps should complete in 5 seconds
boost::this_thread::sleep(boost::posix_time::milliseconds(5000 / numSteps));
}
runtime->stop();
}
TEST(OsgViewElementRenderTest, StereoView)
{
std::shared_ptr<Runtime> runtime = std::make_shared<Runtime>();
std::shared_ptr<OsgManager> manager = std::make_shared<OsgManager>();
runtime->addManager(manager);
runtime->addManager(std::make_shared<SurgSim::Framework::BehaviorManager>());
std::shared_ptr<Scene> scene = runtime->getScene();
/// Add a graphics component to the scene
std::shared_ptr<OsgViewElement> viewElement = std::make_shared<OsgViewElement>("view");
auto boxElement = std::make_shared<SurgSim::Framework::BasicSceneElement>("box");
RigidTransform3d pose =
makeRigidTransform(Vector3d(1.0, 1.0, 1.0), Vector3d(0.0, 0.0, 0.0), Vector3d(0.0, 1.0, 0.0));
viewElement->setPose(pose);
scene->addSceneElement(viewElement);
auto box = std::make_shared<OsgBoxRepresentation>("box");
box->setSizeXYZ(0.1, 0.1, 0.2);
boxElement->addComponent(box);
RigidTransform3d from =
makeRigidTransform(Vector3d(0.2, 0.0, 0.0), Vector3d(0.0, 0.0, 0.0), Vector3d(0.0, 1.0, 0.0));
RigidTransform3d to =
makeRigidTransform(Vector3d(-0.2, 0.0, 0.0), Vector3d(0.0, 0.0, 0.0), Vector3d(0.0, 1.0, 0.0));
auto interpolator = std::make_shared<SurgSim::Blocks::PoseInterpolator>("interpolator");
interpolator->setDuration(2.0);
interpolator->setStartingPose(from);
interpolator->setEndingPose(to);
interpolator->setPingPong(true);
interpolator->setTarget(boxElement);
boxElement->addComponent(interpolator);
scene->addSceneElement(boxElement);
auto osgView = std::static_pointer_cast<OsgView>(viewElement->getView());
osgView->setStereoMode(View::STEREO_MODE_HORIZONTAL_SPLIT);
osgView->setDisplayType(View::DISPLAY_TYPE_MONITOR);
osgView->setEyeSeparation(0.06);
osgView->setScreenWidth(0.486918);
osgView->setScreenHeight(0.273812);
osgView->setScreenDistance(1.0);
runtime->start();
boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
runtime->stop();
}
TEST_F(ContactConstraintGenerationTests, LocalPoses)
{
//Move the collision representation away from the physics representation for the sphere
collision0->setLocalPose(makeRigidTransform(Quaterniond::Identity(), Vector3d(5.0, 0.0, 0.0)));
std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(collision0, collision1);
SurgSim::Collision::SphereDoubleSidedPlaneDcdContact contactCalculation;
contactCalculation.calculateContact(pair);
ASSERT_EQ(1u, pair->getContacts().size());
pairs.push_back(pair);
state->setCollisionPairs(pairs);
ContactConstraintGeneration generator;
generator.update(0.1, state);
ASSERT_EQ(1u, state->getConstraintGroup(CONSTRAINT_GROUP_TYPE_CONTACT).size());
auto constraint = state->getConstraintGroup(CONSTRAINT_GROUP_TYPE_CONTACT)[0];
auto localization = constraint->getLocalizations().first;
auto location = pair->getContacts().front()->penetrationPoints.first;
Vector3d localizationGlobalPosition = localization->calculatePosition();
Vector3d locationGlobalPosition = collision0->getPose() * location.rigidLocalPosition.getValue();
EXPECT_TRUE(localizationGlobalPosition.isApprox(locationGlobalPosition)) <<
"The contact location is not in the same position as the localization produced by constraint generation";
}
