Implement virtual coupling object for Pbd

	-Create PbdRigidObject class for objects that are not deformable but need
	to interact with deformable pbd objects.
	-Create VirtualCouplingPbdObject class, which is inherted from
	PbdRigidObject and TrackingController. The physics and colliding
	geometry(mesh) need to be translate/rotate manually to interact
	with deformable objects, which is different from existing
	VirtualCouplingObject class.

Base/Constraint/imstkPbdCollisionConstraint.cpp

@@ -56,7 +56,7 @@ EdgeEdgeConstraint::solvePositionConstraint()
     }
     else
     {
-        LOG(WARNING) << "det is null";
+//        LOG(WARNING) << "det is null";
     }
 
     Vec3d P = x0 + t*(x1-x0);
@@ -155,7 +155,7 @@ PointTriangleConstraint::solvePositionConstraint()
 
     if (alpha < 0 || beta < 0 || alpha + beta > 1 )
     {
-        LOG(WARNING) << "Projection point not inside the triangle";
+        //LOG(WARNING) << "Projection point not inside the triangle";
         return false;
     }
 

Base/Rendering/RenderDelegate/imstkSurfaceMeshRenderDelegate.cpp

@@ -114,17 +114,17 @@ SurfaceMeshRenderDelegate::SurfaceMeshRenderDelegate(std::shared_ptr<SurfaceMesh
         unit++;
     }
 
-	// Update normals
-	auto normals = surfaceMesh->getPointDataArray("Normals");
-	auto vtkNormals = vtkSmartPointer<vtkFloatArray>::New();
-	vtkNormals->SetNumberOfComponents(3);
-	vtkNormals->SetName("Normals");
-	for (auto const normal : normals)
-	{
-		double triple[3] = { normal[0], normal[1], normal[2] };
-		vtkNormals->InsertNextTuple(triple);
-	}
-	polydata->GetPointData()->SetNormals(vtkNormals);
+	//// Update normals
+	//auto normals = surfaceMesh->getPointDataArray("Normals");
+	//auto vtkNormals = vtkSmartPointer<vtkFloatArray>::New();
+	//vtkNormals->SetNumberOfComponents(3);
+	//vtkNormals->SetName("Normals");
+	//for (auto const normal : normals)
+	//{
+	//	double triple[3] = { normal[0], normal[1], normal[2] };
+	//	vtkNormals->InsertNextTuple(triple);
+	//}
+	//polydata->GetPointData()->SetNormals(vtkNormals);
 
     // Actor
     m_actor->SetMapper(mapper);

Base/SceneElements/Objects/imstkPbdModel.cpp

@@ -339,4 +339,15 @@ PositionBasedModel::updatePhysicsGeometry()
     }
 }
 
+void
+PositionBasedModel::updatePbdStateFromPhysicsGeometry()
+{
+	Vec3d pos;
+	for (int i = 0; i < m_mesh->getNumVertices(); ++i)
+	{
+		pos = m_mesh->getVertexPosition(i);
+		m_state->setVertexPosition(i, pos);
+
+	}
+}
 } // imstk
\ No newline at end of file

Base/SceneElements/Objects/imstkPbdModel.h

@@ -165,6 +165,11 @@ public:
     ///
     void updatePhysicsGeometry();
 
+	///
+	/// \brief
+	///
+	void updatePbdStateFromPhysicsGeometry();
+
     ///
     /// \brief
     ///

Base/SceneElements/Objects/imstkPbdObject.cpp

@@ -146,7 +146,8 @@ PbdObject::init(int nCons, ...)
 
     state->setUniformMass(va_arg(args,double));
 
-    if (nCons > 0)
+ //   if (nCons > 0)
+	if (true)
     {
         char* gstring = va_arg(args,char*);
         float x,y,z;

Base/SceneElements/Objects/imstkPbdObject.h

@@ -38,22 +38,22 @@ public:
     ///
     /// \brief
     ///
-    void integratePosition();
+    virtual void integratePosition();
 
     ///
     /// \brief
     ///
-    void integrateVelocity();
+    virtual void integrateVelocity();
 
     ///
     /// \brief
     ///
-    void updateGeometry();
+    virtual void updateGeometry();
 
     ///
     /// \brief
     ///
-    void constraintProjection();
+    virtual void constraintProjection();
 
     ///
     /// \brief

Base/SceneElements/Objects/imstkPbdRigidObject.cpp

+/*=========================================================================
+
+   Library: iMSTK
+
+   Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+   & Imaging in Medicine, Rensselaer Polytechnic Institute.
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+      http://www.apache.org/licenses/LICENSE-2.0.txt
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
+
+   =========================================================================*/
+
+#include <memory>
+
+#include "imstkPbdRigidObject.h"
+#include "imstkGeometry.h"
+#include "imstkGeometryMap.h"
+
+#include <g3log/g3log.hpp>
+
+namespace imstk
+{
+	void PbdRigidObject::updateGeometry(){
+		m_pbdModel->updatePbdStateFromPhysicsGeometry();
+	}
+
+} //imstk

Base/SceneElements/Objects/imstkPbdRigidObject.h

+/*=========================================================================
+
+   Library: iMSTK
+
+   Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+   & Imaging in Medicine, Rensselaer Polytechnic Institute.
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+      http://www.apache.org/licenses/LICENSE-2.0.txt
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
+
+   =========================================================================*/
+
+#ifndef imstkPbdRigidObject_h
+#define imstkPbdRigidObject_h
+
+#include "imstkPbdObject.h"
+namespace imstk
+{
+
+class Geometry;
+class GeometryMap;
+
+///
+/// \class PbdRigidObject
+///
+/// \brief
+///
+class PbdRigidObject : public PbdObject
+{
+public:
+    ///
+    /// \brief Constructor
+    ///
+	PbdRigidObject(std::string name) : PbdObject(name)
+	{
+		m_type = Type::VirtualCoupling;
+		m_type = SceneObject::Type::Deformable;
+	}
+
+    ///
+    /// \brief Destructor
+    ///
+	~PbdRigidObject() = default;
+
+	///
+	/// \brief
+	///
+	virtual void integratePosition(){ return; }
+
+	///
+	/// \brief
+	///
+	virtual void integrateVelocity(){ return; }
+
+	///
+	/// \brief
+	///
+	virtual void updateGeometry();
+
+	///
+	/// \brief
+	///
+	virtual void constraintProjection(){ return; }
+
+protected:
+
+};
+
+} // imstk
+
+#endif // ifndef imstkVirtualCouplingObject_h

Base/SceneElements/Objects/imstkPbdState.h

@@ -93,11 +93,19 @@ public:
     ///
     /// \brief
     ///
-    inline Vec3d& getVertexPosition(const unsigned int& idx)
+	inline void setVertexPosition(const unsigned int& idx, Vec3d& pos)
     {
-        return m_pos.at(idx);
+		m_pos.at(idx) = pos;
     }
 
+	///
+	/// \brief
+	///
+	inline Vec3d& getVertexPosition(const unsigned int& idx)
+	{
+		return m_pos.at(idx);
+	}
+
     ///
     /// \brief
     ///

Base/SceneElements/Objects/imstkVirtualCouplingPBDObject.cpp

+/*=========================================================================
+
+   Library: iMSTK
+
+   Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+   & Imaging in Medicine, Rensselaer Polytechnic Institute.
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+      http://www.apache.org/licenses/LICENSE-2.0.txt
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
+
+   =========================================================================*/
+
+#include <memory>
+
+#include "imstkVirtualCouplingPBDObject.h"
+#include "imstkGeometry.h"
+#include "imstkGeometryMap.h"
+
+#include <g3log/g3log.hpp>
+
+namespace imstk
+{
+
+void
+VirtualCouplingPBDObject::initOffsets()
+{
+    m_translationOffset = m_collidingGeometry->getPosition();
+    m_rotationOffset = m_collidingGeometry->getOrientation();
+}
+
+void
+VirtualCouplingPBDObject::updateFromDevice()
+{
+    Vec3d p;
+    Quatd r;	
+	Eigen::Matrix4d t;	
+	
+    if (!this->computeTrackingData(p, r))
+    {
+        LOG(WARNING) << "VirtualCouplingPBDObject::updateFromDevice warning: could not update tracking info.";
+        return;
+    }
+
+    // Update colliding geometry
+  m_visualGeometry->setPosition(p);
+  m_visualGeometry->setOrientation(r);
+
+   computeTransform(p, r, t);
+
+   transform = t;
+  
+   Vec4d vertexPos;
+   vertexPos.w() = 1;
+   Vec3d vertexPos0;
+   auto collidingMesh = std::dynamic_pointer_cast<imstk::Mesh>(m_collidingGeometry);
+	for (int i = 0; i < collidingMesh->getNumVertices(); ++i)
+   {
+	   vertexPos0 = collidingMesh->getVertexPosition(i);
+	   vertexPos.x() = vertexPos0.x();
+	   vertexPos.y() = vertexPos0.y();
+	   vertexPos.z() = vertexPos0.z();
+	   vertexPos.applyOnTheLeft(transform);
+	   vertexPos0.x() = vertexPos.x();
+	   vertexPos0.y() = vertexPos.y();
+	   vertexPos0.z() = vertexPos.z();	  
+	   collidingMesh->setVerticePosition(i, vertexPos0);
+   }    
+}
+
+void 
+VirtualCouplingPBDObject::computeTransform(Vec3d& pos, Quatd& quat, Eigen::Matrix4d& t){
+	auto scaling = m_collidingGeometry->getScaling();
+	auto angleAxis = Rotd(quat);
+
+	t.setIdentity();
+	t(0, 0) = scaling;
+	t(1, 1) = scaling;
+	t(2, 2) = scaling;
+	
+	Eigen::Affine3d rot =
+		Eigen::Affine3d(Eigen::AngleAxisd(angleAxis.angle() * 180 / M_PI, 
+		Eigen::Vector3d(angleAxis.axis()[0],angleAxis.axis()[1],angleAxis.axis()[2])));
+	Eigen::Affine3d trans(Eigen::Translation3d(Eigen::Vector3d(pos[0], pos[1], pos[2])));
+	
+	
+	t *= trans.matrix();
+	t *= rot.matrix();
+	
+}
+
+void
+VirtualCouplingPBDObject::applyForces()
+{
+    m_deviceClient->setForce(m_force);
+    this->setForce(Vec3d::Zero());
+}
+
+const Vec3d&
+VirtualCouplingPBDObject::getForce() const
+{
+    return m_force;
+}
+
+void
+VirtualCouplingPBDObject::setForce(Vec3d force)
+{
+    m_force = force;
+}
+
+void
+VirtualCouplingPBDObject::resetCollidingGeo(){
+
+	auto collidingMesh = std::dynamic_pointer_cast<imstk::Mesh>(m_collidingGeometry);
+	collidingMesh->setVerticesPositions(collidingMesh->getInitialVerticesPositions());
+
+}
+
+} // imstk

Base/SceneElements/Objects/imstkVirtualCouplingPBDObject.h

+/*=========================================================================
+
+   Library: iMSTK
+
+   Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+   & Imaging in Medicine, Rensselaer Polytechnic Institute.
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+      http://www.apache.org/licenses/LICENSE-2.0.txt
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
+
+   =========================================================================*/
+
+#ifndef imstkVirtualCouplingPBDObject_h
+#define imstkVirtualCouplingPBDObject_h
+
+#include "imstkVirtualCouplingObject.h"
+#include "imstkPbdRigidObject.h"
+namespace imstk
+{
+
+class Geometry;
+class GeometryMap;
+
+///
+/// \class VirtualCouplingPBDObject
+///
+/// \brief
+///
+class VirtualCouplingPBDObject : public TrackingController, public PbdRigidObject
+{
+public:
+    ///
+    /// \brief Constructor
+    ///
+	VirtualCouplingPBDObject(std::string name,
+		std::shared_ptr<DeviceClient> deviceClient = nullptr,
+		double scaling = 1.0) :
+		TrackingController(deviceClient, scaling), PbdRigidObject(name)
+	{
+		m_type = Type::VirtualCoupling;
+		m_type = SceneObject::Type::Deformable;		
+	}
+
+    ///
+    /// \brief Destructor
+    ///
+    ~VirtualCouplingPBDObject() = default;
+
+	///
+	/// \brief Initialize offset based on object geometry
+	///
+	void initOffsets();
+
+	///
+	/// \brief Update geometries transformations
+	///
+	void updateFromDevice();
+
+	///
+	/// \brief Apply forces to the haptic device
+	///
+	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);
+
+	///
+	/// \brief Reset CollidingGeometry
+	///
+	void resetCollidingGeo();
+
+	///
+	/// \brief compute (offsetted)transform matrix
+	///
+	void computeTransform(Vec3d& p, Quatd& r, Eigen::Matrix4d& t);
+
+protected:
+
+	bool m_forceModified;
+	Vec3d m_force = Vec3d::Zero();
+	Eigen::Matrix4d transform = Eigen::Matrix4d();
+};
+
+
+} // imstk
+
+#endif // ifndef imstkVirtualCouplingObject_h

Base/SimulationManager/imstkSceneManager.cpp

@@ -23,6 +23,7 @@
 #include "imstkSceneManager.h"
 #include "imstkCameraController.h"
 #include "imstkVirtualCouplingObject.h"
+#include "imstkVirtualCouplingPBDObject.h"
 #include "imstkDynamicObject.h"
 #include "imstkPbdObject.h"
 
@@ -53,12 +54,32 @@ SceneManager::initModule()
         {
             virtualCoupling->initOffsets();
         }
+		if (auto virtualCouplingPBD = std::dynamic_pointer_cast<VirtualCouplingPBDObject>(obj))
+		{
+			virtualCouplingPBD->initOffsets();
+		}
     }
 }
 
 void
 SceneManager::runModule()
 {
+	// Update virtualCoupling objects based on devices
+	for (auto obj : m_scene->getSceneObjects())
+	{
+		if (auto virtualCoupling = std::dynamic_pointer_cast<VirtualCouplingObject>(obj))
+		{
+			virtualCoupling->updateFromDevice();
+			virtualCoupling->applyForces();
+		}
+		else if (auto virtualCouplingPBD = std::dynamic_pointer_cast<VirtualCouplingPBDObject>(obj))
+		{
+			virtualCouplingPBD->updateFromDevice();
+			virtualCouplingPBD->applyForces();
+			virtualCouplingPBD->updateGeometry();
+		}
+	}
+
     // Compute collision data per interaction pair
     for (auto intPair : m_scene->getCollisionGraph()->getInteractionPairList())
     {
@@ -84,7 +105,10 @@ SceneManager::runModule()
             //dynaObj->getPhysicsToCollidingMap()->apply();
             dynaObj->getPhysicsToVisualMap()->apply();
         }
-        if (auto pbdObj = std::dynamic_pointer_cast<PbdObject>(obj))
+		if (auto virtualCouplingPBD = std::dynamic_pointer_cast<VirtualCouplingPBDObject>(obj)){
+			continue;
+		}
+        else if (auto pbdObj = std::dynamic_pointer_cast<PbdObject>(obj))
         {
             pbdObj->integratePosition();
             pbdObj->constraintProjection();
@@ -104,7 +128,12 @@ SceneManager::runModule()
 
     for (auto obj : m_scene->getSceneObjects())
     {
-        if (auto pbdObj = std::dynamic_pointer_cast<PbdObject>(obj))
+		if (auto pbdRigidObj = std::dynamic_pointer_cast<VirtualCouplingPBDObject>(obj))
+		{
+			pbdRigidObj->resetCollidingGeo();
+			pbdRigidObj->getCollidingToVisualMap()->apply();
+		}
+        else if (auto pbdObj = std::dynamic_pointer_cast<PbdObject>(obj))
         {
             pbdObj->integrateVelocity();
             pbdObj->updateGeometry();
@@ -112,15 +141,6 @@ SceneManager::runModule()
         }
     }
 
-    // Update virtualCoupling objects based on devices
-    for (auto obj : m_scene->getSceneObjects())
-    {
-        if (auto virtualCoupling = std::dynamic_pointer_cast<VirtualCouplingObject>(obj))
-        {
-            virtualCoupling->updateFromDevice();
-            virtualCoupling->applyForces();
-        }
-    }
 }
 
 void

Examples/ETI/main.cpp

@@ -9,14 +9,26 @@
 
 #include "imstkMeshReader.h"
 #include "imstkSurfaceMesh.h"
+#include "imstkTetrahedralMesh.h"
 #include "g3log/g3log.hpp"
 
+
+// Maps
+#include "imstkTetraTriangleMap.h"
+#include "imstkIsometricMap.h"
+#include "imstkOneToOneMap.h"
+
 // Devices
 #include "imstkHDAPIDeviceClient.h"
-
+#include "imstkPlane.h"
 #include "imstkSphere.h"
 #include "imstkCube.h"
 #include "imstkVirtualCouplingObject.h"
+#include "imstkVirtualCouplingPBDObject.h"
+// Collisions
+#include "imstkInteractionPair.h"
+
+#include "imstkPbdObject.h"
 
 #include "ETI.h"
 
@@ -43,7 +55,7 @@ int main()
 {
 	auto sdk = std::make_shared<SimulationManager>();
 	auto scene = sdk->createNewScene("ETI simulator");
-	bool loadScene = true;
+	bool loadScene = false;
 	bool loadModel = false;
 	
 	// initialize text record
@@ -67,16 +79,6 @@ int main()
 	auto client0 = std::make_shared<imstk::HDAPIDeviceClient>("PHANToM 1");
 	sdk->addDeviceClient(client0);
 
-
-	//// Sphere0
-	//auto sphere0Geom = std::make_shared<imstk::Sphere>();
-	//sphere0Geom->setPosition(imstk::Vec3d(0, 0, 0));
-	//sphere0Geom->scale(1);
-	//auto sphere0Obj = std::make_shared<imstk::VirtualCouplingObject>("Sphere0", client0, 0.05);
-	//sphere0Obj->setVisualGeometry(sphere0Geom);
-	//sphere0Obj->setCollidingGeometry(sphere0Geom);
-	//scene->addSceneObject(sphere0Obj);
-
 	bool coarseMesh = true;
 
 	std::string path2obj;
@@ -87,14 +89,36 @@ int main()
 		path2obj = "resources/Tools/handle.obj";
 	
 	auto mesh = imstk::MeshReader::read(path2obj);
+	auto visualMesh = imstk::MeshReader::read(path2obj);
+	
+	auto handle = std::make_shared<imstk::VirtualCouplingPBDObject>("handle", client0, 0.5);
+
+	auto oneToOneHandle = std::make_shared<imstk::OneToOneMap>();
+	oneToOneHandle->setMaster(mesh);
+	oneToOneHandle->setSlave(mesh);
+	oneToOneHandle->compute();
+
+	auto C2VHandle = std::make_shared<imstk::OneToOneMap>();
+	C2VHandle->setMaster(mesh);
+	C2VHandle->setSlave(visualMesh);
+	C2VHandle->compute();
+
+	handle->setCollidingGeometry(mesh);
+	handle->setVisualGeometry(visualMesh);
+	handle->setPhysicsGeometry(mesh);
+	handle->setPhysicsToCollidingMap(oneToOneHandle);
+	handle->setCollidingToVisualMap(C2VHandle);
+	handle->setPhysicsToVisualMap(oneToOneHandle);
+	handle->init(/*Number of constraints*/0,
+		/*Mass*/0.0,
+		/*Gravity*/"0 0 0",
+		/*TimeStep*/0.001,
+		/*FixedPoint*/"",
+		/*NumberOfIterationInConstraintSolver*/5,
+		/*Proximity*/0.1,
+		/*Contact stiffness*/0.01);
+	scene->addSceneObject(handle);
 
-	auto meshObj = std::make_shared<imstk::VirtualCouplingObject>("mesh", client0, 0.05);
-
-	mesh->setPosition(imstk::Vec3d(0, 0, 0));	
-	mesh->scale(0.1);
-	meshObj->setVisualGeometry(mesh);
-	meshObj->setCollidingGeometry(mesh);
-	scene->addSceneObject(meshObj);
 
 	if (coarseMesh)
 		path2obj = "resources/Tools/blade2.obj";
@@ -102,48 +126,122 @@ int main()
 		path2obj = "resources/Tools/blade.obj";
 
 	auto mesh1 = imstk::MeshReader::read(path2obj);
-	auto meshObj1 = std::make_shared<imstk::VirtualCouplingObject>("mesh1", client0, 0.05);
-	meshObj1->setCollidingGeometry(mesh1);
-	mesh1->scale(0.1);
-//	mesh1->rotate(Vec3d(0.0, 0.0, 1.0), -PI / 2);
-	mesh1->setPosition(imstk::Vec3d(0, 0, 0));
-	meshObj1->setVisualGeometry(mesh1);
-	
-	scene->addSceneObject(meshObj1);
-
-	//_staticORObjectMesh.push_back(imstk::MeshReader::read("resources/OperatingRoom/bed1.obj"));
-	//_staticORObject.push_back(std::make_shared<imstk::VisualObject>("bed1"));
-	//textureName.push_back("resources/TextureOR/bed-1.jpg");
-
-	/*
-	auto mesh3 = imstk::MeshReader::read("resources/human/teeth.obj");
-	auto obj3 = std::make_shared<imstk::VisualObject>("obj3");
-	obj3->setVisualGeometry(mesh3);
-	obj3->setCollidingGeometry(mesh3);
-	scene->addSceneObject(obj3);
-
-	auto mesh4 = imstk::MeshReader::read("resources/human/trachea.obj");
-	auto obj4 = std::make_shared<imstk::VisualObject>("obj4");
-	obj4->setVisualGeometry(mesh4);
-	obj4->setCollidingGeometry(mesh4);
-	scene->addSceneObject(obj4);
-	*/
-	
-
-
-	/*
-	auto mesh6 = imstk::MeshReader::read("resources/human/dude_111.obj");
-	auto obj6 = std::make_shared<imstk::VisualObject>("obj6");
-	obj6->setVisualGeometry(mesh6);
-	obj6->setCollidingGeometry(mesh6);
-	scene->addSceneObject(obj6);
-	*/
-	//	auto mesh2 = imstk::MeshReader::read("/home/virtualfls/Projects/IMSTK/resources/Spheres/small_0.vtk");
-
+	auto viusalMesh1 = imstk::MeshReader::read(path2obj);
+	auto blade = std::make_shared<imstk::VirtualCouplingPBDObject>("blade", client0, 0.5);
+
+	auto oneToOneBlade = std::make_shared<imstk::OneToOneMap>();
+	oneToOneBlade->setMaster(mesh1);
+	oneToOneBlade->setSlave(mesh1);
+	oneToOneBlade->compute();
+
+	auto C2VBlade = std::make_shared<imstk::OneToOneMap>();
+	C2VBlade->setMaster(mesh1);
+	C2VBlade->setSlave(viusalMesh1);
+	C2VBlade->compute();
+
+	blade->setCollidingGeometry(mesh1);
+	blade->setVisualGeometry(viusalMesh1);
+	blade->setPhysicsGeometry(mesh1);
+	blade->setPhysicsToCollidingMap(oneToOneBlade);
+	blade->setCollidingToVisualMap(C2VBlade);
+	blade->setPhysicsToVisualMap(C2VBlade);
+	blade->init(/*Number of constraints*/0,
+		/*Mass*/0.0,
+		/*Gravity*/"0 0 0",
+		/*TimeStep*/0.001,
+		/*FixedPoint*/"",
+		/*NumberOfIterationInConstraintSolver*/5,
+		/*Proximity*/0.1,
+		/*Contact stiffness*/0.01);
+	scene->addSceneObject(blade);
+
+//	scene->addSceneObject(meshObj1);
+
+
+	// floor
+	auto floorMesh = std::make_shared<imstk::SurfaceMesh>();
+	std::vector<imstk::Vec3d> vertList;
+	double width = 60.0;
+	double height = 60.0;
+	int nRows = 20;
+	int nCols = 20;
+	int corner[4] = { 1, nRows, nRows*nCols - nCols+1,nRows*nCols };
+	char intStr[33];
+	std::string fixed_corner;
+	for (unsigned int i = 0; i < 4; i++){
+		itoa(corner[i], intStr, 10);
+		fixed_corner += std::string(intStr) + ' ';
+	}
+	vertList.resize(nRows*nCols);
+	const double dy = width / (double)(nCols - 1);
+	const double dx = height / (double)(nRows - 1);
+	for (int i = 0; i < nRows; i++)
+	{
+		for (int j = 0; j < nCols; j++)
+		{
+			const double y = (double)dy*j;
+			const double x = (double)dx*i;
+			vertList[i*nCols + j] = Vec3d(x - 30, -25, y-60);
 
+		}
+	}
+	floorMesh->setInitialVerticesPositions(vertList);
+	floorMesh->setVerticesPositions(vertList);
 
+	// c. Add connectivity data
+	std::vector<imstk::SurfaceMesh::TriangleArray> triangles;
+	for (int i = 0; i < nRows - 1; i++)
+	{
+		for (int j = 0; j < nCols - 1; j++)
+		{
+			imstk::SurfaceMesh::TriangleArray tri[2];
+			tri[0] = { { i*nCols + j, i*nCols + j + 1, (i + 1)*nCols + j } };
+			tri[1] = { { (i + 1)*nCols + j + 1, (i + 1)*nCols + j, i*nCols + j + 1 } };
+			triangles.push_back(tri[0]);
+			triangles.push_back(tri[1]);
+		}		
+	}
+	floorMesh->setTrianglesVertices(triangles);
+
+	auto oneToOneFloor = std::make_shared<imstk::OneToOneMap>();
+	oneToOneFloor->setMaster(floorMesh);
+	oneToOneFloor->setSlave(floorMesh);
+	oneToOneFloor->compute();
+
+	auto floor = std::make_shared<PbdObject>("Floor");
+	floor->setCollidingGeometry(floorMesh);
+	floor->setVisualGeometry(floorMesh);
+	floor->setPhysicsGeometry(floorMesh);
+	floor->setPhysicsToCollidingMap(oneToOneFloor);
+	floor->setPhysicsToVisualMap(oneToOneFloor);
+	floor->setCollidingToVisualMap(oneToOneFloor);
+	floor->init(/*Number of constraints*/2,
+		/*Constraint configuration*/"Distance 0.1",
+		/*Constraint configuration*/"Dihedral 0.001",
+		/*Mass*/0.5,
+		/*Gravity*/"0 -9.8 0",
+		/*TimeStep*/0.01,
+		/*FixedPoint*/fixed_corner.c_str(),		
+		/*NumberOfIterationInConstraintSolver*/5,
+		/*Proximity*/0.1,
+		/*Contact stiffness*/0.01);
+	scene->addSceneObject(floor);
+
+	// Collisions
+	auto colGraph = scene->getCollisionGraph();
+	auto pair1 = std::make_shared<PbdInteractionPair>(PbdInteractionPair(blade, floor));
+	auto pair2 = std::make_shared<PbdInteractionPair>(PbdInteractionPair(handle, floor));
+	pair1->setNumberOfInterations(5);
+	pair2->setNumberOfInterations(5);
+
+	colGraph->addInteractionPair(pair1);
+	colGraph->addInteractionPair(pair2);
+
+	Vec3d cameraPos = scene->getCamera()->getPosition();
+	std::cout << cameraPos << std::endl;
+	scene->getCamera()->setPosition(0,0,50);
 	sdk->setCurrentScene("ETI simulator");
-    sdk->startSimulation(false);
+    sdk->startSimulation(true);
 }
 
 void initializeText()