Commit 621038b5 authored by Alexis Girault's avatar Alexis Girault

ENH: Refactor VirtualCouplingObject in a controller

SceneObjectController: similar to cameraController, it
subclasses trackingController to update the transformation
of an object master geometry, instead of updating itself.

SceneObject: similar to camera, has an interface to easily
setup a controller using a device client.

CollidingObject: stores the force vector that is to be
applied to the device.

SceneManager: make the API calls from controller instead
of object
parent 7c272963
......@@ -38,11 +38,10 @@ CollisionHandling::make_collision_handling(const Type& type,
{
case Type::Penalty:
{
if (objA->getType() != SceneObject::Type::VirtualCoupling &&
objA->getType() == SceneObject::Type::Rigid)
if (objA->getType() == SceneObject::Type::Visual)
{
LOG(WARNING) << "CollisionHandling::make_collision_handling error: "
<< "penalty collision handling not yet implemented for non-rigid objects.";
<< "penalty collision handling only implemented for colliding objects.";
return nullptr;
}
return std::make_shared<PenaltyRigidCH>(side, colData, objA);
......
......@@ -22,7 +22,6 @@
#include "imstkPenaltyMeshToRigidCH.h"
#include "imstkCollidingObject.h"
#include "imstkVirtualCouplingObject.h"
#include "imstkCollisionData.h"
#include "imstkDeformableObject.h"
......
......@@ -22,7 +22,6 @@
#include "imstkPenaltyRigidCH.h"
#include "imstkCollidingObject.h"
#include "imstkVirtualCouplingObject.h"
#include "imstkCollisionData.h"
#include <g3log/g3log.hpp>
......@@ -32,7 +31,7 @@ namespace imstk {
void
PenaltyRigidCH::computeContactForces()
{
auto movableObj = std::dynamic_pointer_cast<VirtualCouplingObject>(m_obj);
auto movableObj = std::dynamic_pointer_cast<CollidingObject>(m_obj);
if(movableObj == nullptr)
{
LOG(WARNING) << "PenaltyRigidCH::computeContactForces error: "
......
......@@ -19,11 +19,12 @@
=========================================================================*/
#include <memory>
#include "imstkSceneObjectController.h"
#include "imstkVirtualCouplingObject.h"
#include "imstkCollidingObject.h"
#include "imstkGeometry.h"
#include "imstkGeometryMap.h"
#include <utility>
#include <g3log/g3log.hpp>
......@@ -31,52 +32,36 @@ namespace imstk
{
void
VirtualCouplingObject::initOffsets()
SceneObjectController::initOffsets()
{
m_translationOffset = m_collidingGeometry->getPosition();
m_rotationOffset = m_collidingGeometry->getOrientation();
m_translationOffset = m_sceneObject.getMasterGeometry()->getPosition();
m_rotationOffset = m_sceneObject.getMasterGeometry()->getOrientation();
}
void
VirtualCouplingObject::updateFromDevice()
SceneObjectController::updateFromDevice()
{
Vec3d p;
Quatd r;
if (!this->computeTrackingData(p, r))
{
LOG(WARNING) << "VirtualCouplingObject::updateFromDevice warning: could not update tracking info.";
LOG(WARNING) << "SceneObjectController::updateFromDevice warning: could not update tracking info.";
return;
}
// Update colliding geometry
m_collidingGeometry->setPosition(p);
m_collidingGeometry->setOrientation(r);
// Update visual geometry
if(m_collidingToVisualMap)
{
m_collidingToVisualMap->apply();
}
m_sceneObject.getMasterGeometry()->setPosition(p);
m_sceneObject.getMasterGeometry()->setOrientation(r);
}
void
VirtualCouplingObject::applyForces()
{
m_deviceClient->setForce(m_force);
this->setForce(Vec3d::Zero());
}
const Vec3d&
VirtualCouplingObject::getForce() const
SceneObjectController::applyForces()
{
return m_force;
}
void
VirtualCouplingObject::setForce(Vec3d force)
{
m_force = force;
if(auto collidingObject = dynamic_cast<CollidingObject*>(&m_sceneObject))
{
m_deviceClient->setForce(collidingObject->getForce());
}
}
} // imstk
......@@ -19,41 +19,38 @@
=========================================================================*/
#ifndef imstkVirtualCouplingObject_h
#define imstkVirtualCouplingObject_h
#ifndef imstkSceneObjectController_h
#define imstkSceneObjectController_h
#include "imstkCollidingObject.h"
#include "imstkTrackingController.h"
#include "imstkSceneObject.h"
#include <memory>
namespace imstk
{
class Geometry;
class GeometryMap;
///
/// \class VirtualCouplingObject
/// \class SceneObjectController
///
/// \brief
///
class VirtualCouplingObject : public CollidingObject, public TrackingController
class SceneObjectController : public TrackingController
{
public:
///
/// \brief Constructor
///
VirtualCouplingObject(std::string name,
SceneObjectController(SceneObject& sceneObject,
std::shared_ptr<DeviceClient> deviceClient) :
CollidingObject(name),
TrackingController(deviceClient)
{
m_type = Type::VirtualCoupling;
}
TrackingController(deviceClient),
m_sceneObject(sceneObject)
{}
///
/// \brief Destructor
///
~VirtualCouplingObject() = default;
~SceneObjectController() = default;
///
/// \brief Initialize offset based on object geometry
......@@ -70,22 +67,11 @@ public:
///
void applyForces();
///
/// \brief Get the force to apply to the device
///
const Vec3d& getForce() const;
///
/// \brief Set the force to apply to the device
///
void setForce(Vec3d force);
protected:
bool m_forceModified;
Vec3d m_force = Vec3d::Zero();
SceneObject& m_sceneObject; ///< SceneObject controlled by the external device
};
} // imstk
#endif // ifndef imstkVirtualCouplingObject_h
#endif // ifndef imstkSceneObjectController_h
......@@ -54,4 +54,16 @@ CollidingObject::setCollidingToVisualMap(std::shared_ptr<GeometryMap> map)
m_collidingToVisualMap = map;
}
} // imstk
\ No newline at end of file
const Vec3d&
CollidingObject::getForce() const
{
return m_force;
}
void
CollidingObject::setForce(Vec3d force)
{
m_force = force;
}
} // imstk
......@@ -25,6 +25,7 @@
#include <memory>
#include "imstkSceneObject.h"
#include "imstkMath.h"
namespace imstk
{
......@@ -65,10 +66,17 @@ public:
std::shared_ptr<GeometryMap> getCollidingToVisualMap() const;
void setCollidingToVisualMap(std::shared_ptr<GeometryMap> map);
///
/// \brief Set/Get the force to be applied to the object
///
const Vec3d& getForce() const;
void setForce(Vec3d force);
protected:
std::shared_ptr<Geometry> m_collidingGeometry; ///> Geometry for collisions
std::shared_ptr<GeometryMap> m_collidingToVisualMap; ///> Maps transformations to visual geometry
Vec3d m_force = Vec3d::Zero();
};
......
......@@ -21,6 +21,9 @@
#include "imstkSceneObject.h"
#include "imstkSceneObjectController.h"
#include "imstkDeviceClient.h"
namespace imstk
{
......@@ -66,4 +69,24 @@ SceneObject::setName(const std::string& name)
m_name = name;
}
std::shared_ptr<SceneObjectController>
SceneObject::getController() const
{
return m_controller;
}
std::shared_ptr<SceneObjectController>
SceneObject::setupController(std::shared_ptr<DeviceClient> deviceClient)
{
if(m_controller == nullptr)
{
m_controller = std::make_shared<SceneObjectController>(*this, deviceClient);
}
else
{
m_controller->setDeviceClient(deviceClient);
}
return m_controller;
}
} // imstk
......@@ -29,6 +29,8 @@ namespace imstk
{
class Geometry;
class SceneObjectController;
class DeviceClient;
///
/// \class SceneObject
......@@ -44,8 +46,7 @@ public:
Visual,
Colliding,
Rigid,
Deformable,
VirtualCoupling
Deformable
};
///
......@@ -83,6 +84,16 @@ public:
///
virtual std::shared_ptr<Geometry> getMasterGeometry() const;
///
/// \brief Get the object controller
///
std::shared_ptr<SceneObjectController> getController() const;
///
/// \brief Setup a controller for the object for a given device client
///
std::shared_ptr<SceneObjectController> setupController(std::shared_ptr<DeviceClient> deviceClient);
protected:
///
/// \brief Assigns the type of the object
......@@ -91,9 +102,9 @@ protected:
Type m_type; ///> Type of the scene object
std::string m_name; ///> Custom name of the scene object
std::shared_ptr<Geometry> m_visualGeometry; ///> Geometry for rendering
};
std::shared_ptr<Geometry> m_visualGeometry; ///> Geometry for rendering
std::shared_ptr<SceneObjectController> m_controller; ///> Object controller
};
using VisualObject = SceneObject;
......
......@@ -22,7 +22,7 @@
#ifndef imstkVirtualCouplingPBDObject_h
#define imstkVirtualCouplingPBDObject_h
#include "imstkVirtualCouplingObject.h"
#include "imstkTrackingController.h"
#include "imstkPbdRigidObject.h"
namespace imstk
{
......@@ -45,8 +45,7 @@ public:
std::shared_ptr<DeviceClient> deviceClient) :
TrackingController(deviceClient),
PbdRigidObject(name)
{
m_type = Type::VirtualCoupling;
{
}
///
......
......@@ -22,10 +22,10 @@
// imstk
#include "imstkSceneManager.h"
#include "imstkCameraController.h"
#include "imstkVirtualCouplingObject.h"
#include "imstkVirtualCouplingPBDObject.h"
#include "imstkSceneObjectController.h"
#include "imstkDynamicObject.h"
#include "imstkPbdObject.h"
#include "imstkVirtualCouplingPBDObject.h"
#include "imstkGeometryMap.h"
#include "g3log/g3log.hpp"
......@@ -51,9 +51,9 @@ SceneManager::initModule()
// Init virtual coupling objects offsets
for (auto obj : m_scene->getSceneObjects())
{
if (auto virtualCoupling = std::dynamic_pointer_cast<VirtualCouplingObject>(obj))
if (auto controller = obj->getController())
{
virtualCoupling->initOffsets();
controller->initOffsets();
}
if (auto virtualCouplingPBD = std::dynamic_pointer_cast<VirtualCouplingPBDObject>(obj))
{
......@@ -68,10 +68,14 @@ SceneManager::runModule()
// Update virtualCoupling objects based on devices
for (auto obj : m_scene->getSceneObjects())
{
if (auto virtualCoupling = std::dynamic_pointer_cast<VirtualCouplingObject>(obj))
if (auto controller = obj->getController())
{
virtualCoupling->updateFromDevice();
virtualCoupling->applyForces();
controller->updateFromDevice();
if (auto collidingObj = std::dynamic_pointer_cast<CollidingObject>(obj))
{
controller->applyForces();
collidingObj->setForce(Vec3d(0,0,0));
}
}
else if (auto virtualCouplingPBD = std::dynamic_pointer_cast<VirtualCouplingPBDObject>(obj))
{
......
......@@ -13,7 +13,6 @@
#include "imstkDeformableBodyModel.h"
#include "imstkDeformableObject.h"
#include "imstkSceneObject.h"
#include "imstkVirtualCouplingObject.h"
#include "imstkLight.h"
#include "imstkCamera.h"
......@@ -45,6 +44,7 @@
#include "imstkVRPNDeviceClient.h"
#include "imstkVRPNDeviceServer.h"
#include "imstkCameraController.h"
#include "imstkSceneObjectController.h"
// Collisions
#include "imstkInteractionPair.h"
......@@ -104,7 +104,7 @@ int main()
//testMultiTextures();
//testMeshCCD();
//testPenaltyRigidCollision();
testPenaltyRigidCollision();
//testTwoFalcons();
//testObjectController();
//testCameraController();
......@@ -120,7 +120,7 @@ int main()
//testDeformableBody();
//testVTKTexture();
//testMultiObjectWithTextures();
testTwoOmnis();
//testTwoOmnis();
//testVectorPlotters();
//testPbdVolume();
//testPbdCloth();
......@@ -353,20 +353,22 @@ void testPenaltyRigidCollision()
auto sphere0Geom = std::make_shared<Sphere>();
sphere0Geom->scale(0.5);
sphere0Geom->translate(Vec3d(1, 0.5, 0));
auto sphere0Obj = std::make_shared<imstk::VirtualCouplingObject>("Sphere0", client0);
sphere0Obj->setTranslationScaling(40);
auto sphere0Obj = std::make_shared<imstk::CollidingObject>("Sphere0");
sphere0Obj->setVisualGeometry(sphere0Geom);
sphere0Obj->setCollidingGeometry(sphere0Geom);
auto sphere0Controller = sphere0Obj->setupController(client0);
sphere0Controller->setTranslationScaling(40);
scene->addSceneObject(sphere0Obj);
// Sphere1
auto sphere1Geom = std::make_shared<Sphere>();
sphere1Geom->scale(0.5);
sphere1Geom->translate(Vec3d(-1, 0.5, 0));
auto sphere1Obj = std::make_shared<imstk::VirtualCouplingObject>("Sphere1", client1);
sphere1Obj->setTranslationScaling(40);
auto sphere1Obj = std::make_shared<imstk::CollidingObject>("Sphere1");
sphere1Obj->setVisualGeometry(sphere1Geom);
sphere1Obj->setCollidingGeometry(sphere1Geom);
auto sphere1Controller = sphere1Obj->setupController(client1);
sphere1Controller->setTranslationScaling(40);
scene->addSceneObject(sphere1Obj);
// Collisions
......@@ -424,20 +426,22 @@ void testTwoFalcons()
auto sphere0Geom = std::make_shared<imstk::Sphere>();
sphere0Geom->setPosition(imstk::Vec3d(16, 4.5, 0));
sphere0Geom->scale(1);
auto sphere0Obj = std::make_shared<imstk::VirtualCouplingObject>("Sphere0", falcon0);
sphere0Obj->setTranslationScaling(30);
auto sphere0Obj = std::make_shared<imstk::CollidingObject>("Sphere0");
sphere0Obj->setVisualGeometry(sphere0Geom);
sphere0Obj->setCollidingGeometry(sphere0Geom);
auto sphere0Controller = sphere0Obj->setupController(falcon0);
sphere0Controller->setTranslationScaling(30);
scene->addSceneObject(sphere0Obj);
// Sphere1
auto sphere1Geom = std::make_shared<imstk::Sphere>();
sphere1Geom->setPosition(imstk::Vec3d(-16, 4.5, 0));
sphere1Geom->scale(1);
auto sphere1Obj = std::make_shared<imstk::VirtualCouplingObject>("Sphere1", falcon1);
sphere1Obj->setTranslationScaling(30);
auto sphere1Obj = std::make_shared<imstk::CollidingObject>("Sphere1");
sphere1Obj->setVisualGeometry(sphere1Geom);
sphere1Obj->setCollidingGeometry(sphere1Geom);
auto sphere1Controller = sphere0Obj->setupController(falcon1);
sphere1Controller->setTranslationScaling(30);
scene->addSceneObject(sphere1Obj);
// Camera
......@@ -481,20 +485,22 @@ void testTwoOmnis(){
auto sphere0Geom = std::make_shared<imstk::Sphere>();
sphere0Geom->setPosition(imstk::Vec3d(2, 2.5, 0));
sphere0Geom->scale(1);
auto sphere0Obj = std::make_shared<imstk::VirtualCouplingObject>("Sphere0", client0);
sphere0Obj->setTranslationScaling(0.05);
auto sphere0Obj = std::make_shared<imstk::CollidingObject>("Sphere0");
sphere0Obj->setVisualGeometry(sphere0Geom);
sphere0Obj->setCollidingGeometry(sphere0Geom);
auto sphere0Controller = sphere0Obj->setupController(client0);
sphere0Controller->setTranslationScaling(0.05);
scene->addSceneObject(sphere0Obj);
// Sphere1
auto sphere1Geom = std::make_shared<imstk::Sphere>();
sphere1Geom->setPosition(imstk::Vec3d(-2, 2.5, 0));
sphere1Geom->scale(1);
auto sphere1Obj = std::make_shared<imstk::VirtualCouplingObject>("Sphere1", client1);
sphere1Obj->setTranslationScaling(0.05);
auto sphere1Obj = std::make_shared<imstk::CollidingObject>("Sphere1");
sphere1Obj->setVisualGeometry(sphere1Geom);
sphere1Obj->setCollidingGeometry(sphere1Geom);
auto sphere0Controller = sphere0Obj->setupController(client1);
sphere0Controller->setTranslationScaling(0.05);
scene->addSceneObject(sphere1Obj);
// Update Camera position
......@@ -527,10 +533,11 @@ void testObjectController()
auto geom = std::make_shared<imstk::Cube>();
geom->setPosition(imstk::UP_VECTOR);
geom->scale(2);
auto object = std::make_shared<imstk::VirtualCouplingObject>("VirtualObject", client);
object->setTranslationScaling(0.1);
auto object = std::make_shared<imstk::CollidingObject>("VirtualObject");
object->setVisualGeometry(geom);
object->setCollidingGeometry(geom);
auto controller = sphere0Obj->setupController(client);
controller->setTranslationScaling(0.1);
scene->addSceneObject(object);
// Update Camera position
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment