diff --git a/Collision/MeshCollisionModel.cpp b/Collision/MeshCollisionModel.cpp
index 92c8442ef421e8d4663e2f2c6dd2540f31a56fe9..9a153636c0754da15789785420e5677c7172151a 100644
--- a/Collision/MeshCollisionModel.cpp
+++ b/Collision/MeshCollisionModel.cpp
@@ -38,7 +38,7 @@ void MeshCollisionModel::setMesh(std::shared_ptr<Mesh> modelMesh)
     this->aabbTree.reset();
     this->initAABBTree(1);
 }
-void MeshCollisionModel::loadTriangleMesh(const std::string& meshName, const BaseMesh::MeshFileType &type)
+void MeshCollisionModel::loadTriangleMesh(const std::string& meshName, const Core::BaseMesh::MeshFileType &type)
 {
     this->load(meshName,type);
 
diff --git a/Collision/MeshCollisionModel.h b/Collision/MeshCollisionModel.h
index 99384e47d94ad7fb3a2207f81bc54bd3b8435768..b7c27b6dd7a4dbd9caaac96145a92bd47856c140 100644
--- a/Collision/MeshCollisionModel.h
+++ b/Collision/MeshCollisionModel.h
@@ -68,7 +68,7 @@ public:
     ///
     /// @brief Loads a triangular mesh and stores it.
     ///
-    void loadTriangleMesh(const std::string &meshName, const BaseMesh::MeshFileType &type);
+    void loadTriangleMesh(const std::string &meshName, const Core::BaseMesh::MeshFileType &type);
 
     ///
     /// @brief Set internal AABB tree
diff --git a/Collision/OctreeCell.cpp b/Collision/OctreeCell.cpp
index bd8d28566db5e9572fe725a8bd604590b900d939..588fec1bf75d61f61a530ff25536c33c7575cfd4 100644
--- a/Collision/OctreeCell.cpp
+++ b/Collision/OctreeCell.cpp
@@ -79,12 +79,13 @@ void OctreeCell::expand( const double expandScale )
     cube.expand( expandScale );
 }
 
-bool OctreeCell::isCollidedWithTri( core::Vec3d &v0, core::Vec3d &v1, core::Vec3d &v2 )
+bool OctreeCell::isCollidedWithTri(const core::Vec3d &v0, const core::Vec3d &v1, const core::Vec3d &v2 ) const
 {
-    AABB tempAABB;
-    tempAABB.aabbMin = cube.leftMinCorner();
-    tempAABB.aabbMax = cube.rightMaxCorner();
-    return CollisionMoller::checkAABBTriangle( tempAABB, v0, v1, v2 );
+    Eigen::AlignedBox3d box;
+    box.min() = cube.leftMinCorner();
+    box.max() = cube.rightMaxCorner();
+
+    return (box.contains(v0) || box.contains(v1) || box.contains(v2));
 }
 
 bool OctreeCell::isCollidedWithPoint()
diff --git a/Collision/OctreeCell.h b/Collision/OctreeCell.h
index 57c8b4491bb414aa169299cf0e880e80ce0b54a6..1fe407319070fe348acac566a5f0ba0da9a29519 100644
--- a/Collision/OctreeCell.h
+++ b/Collision/OctreeCell.h
@@ -79,7 +79,7 @@ public:
     void setLength ( const double length );
 
     /// \brief check if a triangle is intersecting the octree cell
-    bool isCollidedWithTri ( core::Vec3d &v0, core::Vec3d &v1, core::Vec3d &v2 );
+    bool isCollidedWithTri ( const core::Vec3d &v0, const core::Vec3d &v1, const core::Vec3d &v2 ) const;
 
     /// \brief check if a point lies inside an octree cell
     bool isCollidedWithPoint ();
@@ -88,28 +88,28 @@ public:
     void subDivide ( const int divisionPerAxis,
                      std::array<OctreeCell,numberOfSubdivisions> &cells );
 
-    const AABB &getAabb() const
+    const Eigen::AlignedBox3d &getAabb() const
     {
         return aabb;
     }
 
-    void setAabb(const AABB &newAabb)
+    void setAabb(const Eigen::AlignedBox3d &newAabb)
     {
         this->aabb = newAabb;
     }
 
-    void getIntersections(const AABB &aabb, std::vector<size_t> &triangles)
+    void getIntersections(const Eigen::AlignedBox3d &aabb, std::vector<size_t> &triangles)
     {
         for(auto &i : data)
         {
-            if(i.first.overlaps(aabb))
+            if(!i.first.intersection(aabb).isEmpty())
             {
                 triangles.emplace_back(i.second);
             }
         }
     }
 
-    inline void addTriangleData(const AABB &aabb, size_t index)
+    inline void addTriangleData(const Eigen::AlignedBox3d &aabb, size_t index)
     {
         return data.emplace_back(aabb,index);
     }
@@ -142,19 +142,19 @@ public:
 
     void draw() const
     {
-        this->aabb.draw();
-        for(auto &child : childNodes)
-        {
-            if(child)
-            {
-                child->draw();
-            }
-        }
+//         this->aabb.draw();
+//         for(auto &child : childNodes)
+//         {
+//             if(child)
+//             {
+//                 child->draw();
+//             }
+//         }
     }
 
     void update()
     {
-        this->aabb.reset();
+        this->aabb.setEmpty();
         for(auto &box : data)
         {
             this->aabb.extend(box.first);
@@ -163,11 +163,11 @@ public:
 
 private:
     Cube cube; ///< cube
-    AABB aabb;
+    Eigen::AlignedBox3d aabb;
 
     std::array<std::shared_ptr<OctreeCell>,numberOfSubdivisions> childNodes;
     std::shared_ptr<OctreeCell> parentNode;
-    std::vector<std::pair<AABB,size_t>> data;
+    std::vector<std::pair<Eigen::AlignedBox3d,size_t>> data;
 
 };
 
diff --git a/Collision/SpatialHashCollision.cpp b/Collision/SpatialHashCollision.cpp
index 71b0993cdde7c50f978b2d7a5c76db0a613251e8..e94ec3321c6acca8dcd07e02ed5363117a75592c 100644
--- a/Collision/SpatialHashCollision.cpp
+++ b/Collision/SpatialHashCollision.cpp
@@ -23,6 +23,8 @@
 
 #include "SpatialHashCollision.h"
 
+#include <Eigen/Geometry>
+
 // SimMedTK includes
 #include "Core/CollisionConfig.h"
 #include "Collision/SurfaceTree.h"
@@ -78,11 +80,6 @@ void SpatialHashCollision::addMesh(std::shared_ptr<Mesh> mesh)
     mesh->allocateAABBTris();
 }
 
-void SpatialHashCollision::addMesh(std::shared_ptr<LineMesh> mesh)
-{
-    lineMeshes.push_back(mesh);
-}
-
 void SpatialHashCollision::removeMesh(std::shared_ptr<Mesh> mesh)
 {
     auto it = std::find(meshes.begin(),meshes.end(),mesh);
@@ -98,9 +95,11 @@ bool SpatialHashCollision::findCandidatePoints(std::shared_ptr<Mesh> mesh,
     tempAABB.aabbMax = colModel->root->getCube().rightMaxCorner();
 
     bool  found = false;
-    for (int i = 0; i < mesh->nbrVertices; i++)
+
+    auto &vertices = mesh->getVertices();
+    for (size_t i = 0, end = mesh->getNumberOfVertices(); i < end; ++i)
     {
-        if (CollisionMoller::checkAABBPoint(tempAABB, mesh->vertices[i]))
+        if (CollisionMoller::checkAABBPoint(tempAABB, vertices[i]))
         {
             addPoint(mesh, i, cellsForModelPoints);
             found = true;
@@ -111,9 +110,8 @@ bool SpatialHashCollision::findCandidatePoints(std::shared_ptr<Mesh> mesh,
 
 bool SpatialHashCollision::findCandidateTris(std::shared_ptr<Mesh> meshA, std::shared_ptr<Mesh> meshB)
 {
-    AABB aabboverlap;
-
-    if (CollisionMoller::checkOverlapAABBAABB(meshA->aabb, meshB->aabb, aabboverlap) == false)
+    auto intersection = meshA->getBoundingBox().intersection(meshB->getBoundingBox());
+    if (intersection.isEmpty())
     {
         return false;
     }
@@ -131,34 +129,6 @@ bool SpatialHashCollision::findCandidateTris(std::shared_ptr<Mesh> meshA, std::s
     return true;
 }
 
-bool SpatialHashCollision::findCandidateTrisLines(std::shared_ptr<Mesh> meshA, std::shared_ptr<LineMesh> meshB)
-{
-    AABB aabboverlap;
-
-    if (CollisionMoller::checkOverlapAABBAABB(meshA->aabb, meshB->aabb, aabboverlap) == false)
-    {
-        return false;
-    }
-
-    for (int i = 0; i < meshA->nbrTriangles; i++)
-    {
-        if (CollisionMoller::checkOverlapAABBAABB(aabboverlap, meshA->triAABBs[i]))
-        {
-            addTriangle(meshA, i, cellsForTri2Line);
-        }
-    }
-
-    for (int i = 0; i < meshB->nbrEdges; i++)
-    {
-        if (CollisionMoller::checkOverlapAABBAABB(aabboverlap, meshB->edgeAABBs[i]))
-        {
-            addLine(meshB, i, cellLines);
-        }
-    }
-
-    return true;
-}
-
 void SpatialHashCollision::computeCollisionTri2Tri()
 {
     HashIterator<CellTriangle> iterator;
@@ -179,7 +149,7 @@ void SpatialHashCollision::computeCollisionTri2Tri()
             while (cells.nextBucketItem(iterator1, triB))
             {
                 if (triA.meshID == triB.meshID ||
-                    !(meshes[0]->collisionGroup.isCollisionPermitted(meshes[1]->collisionGroup)))
+                    !(meshes[0]->getCollisionGroup()->isCollisionPermitted(meshes[1]->getCollisionGroup())))
                 {
                     continue;
                 }
@@ -227,7 +197,7 @@ void  SpatialHashCollision::computeCollisionLine2Tri()
             while (cellsForTri2Line.nextBucketItem(iteratorTri, tri))
             {
                 if (tri.meshID == line.meshID ||
-                    !(meshes[0]->collisionGroup.isCollisionPermitted(meshes[1]->collisionGroup)))
+                    !(meshes[0]->getCollisionGroup()->isCollisionPermitted(meshes[1]->getCollisionGroup())))
                 {
                     continue;
                 }
@@ -284,15 +254,19 @@ void SpatialHashCollision::computeCollisionModel2Points()
 
 void SpatialHashCollision::computeHash(std::shared_ptr<Mesh> mesh, const std::vector<int> &triangleIndexes)
 {
+    core::Vec3d cellSize(1.0/cellSizeX,1.0/cellSizeY,1.0/cellSizeZ);
     for(auto&& i : triangleIndexes)
     {
-        int xStartIndex = static_cast<int>(std::floor(mesh->triAABBs[i].aabbMin[0]/cellSizeX));
-        int yStartIndex = static_cast<int>(std::floor(mesh->triAABBs[i].aabbMin[1]/cellSizeY));
-        int zStartIndex = static_cast<int>(std::floor(mesh->triAABBs[i].aabbMin[2]/cellSizeZ));
+        auto startIndices = mesh->triAABBs[i].min().array()*cellSize.array();
+        auto endIndices = mesh->triAABBs[i].max().array()*cellSize.array();
+
+        int xStartIndex = static_cast<int>(std::floor(startIndices(0)));
+        int yStartIndex = static_cast<int>(std::floor(startIndices(1)));
+        int zStartIndex = static_cast<int>(std::floor(startIndices(2)));
 
-        int xEndIndex = static_cast<int>(std::floor(mesh->triAABBs[i].aabbMax[0]/cellSizeX));
-        int yEndIndex = static_cast<int>(std::floor(mesh->triAABBs[i].aabbMax[1]/cellSizeY));
-        int zEndIndex = static_cast<int>(std::floor(mesh->triAABBs[i].aabbMax[2]/cellSizeZ));
+        int xEndIndex = static_cast<int>(std::floor(endIndices(0)));
+        int yEndIndex = static_cast<int>(std::floor(endIndices(1)));
+        int zEndIndex = static_cast<int>(std::floor(endIndices(2)));
 
         for (int ix = xStartIndex; ix <= xEndIndex; ix++)
             for (int iy = yStartIndex; iy <= yEndIndex; iy++)
@@ -309,18 +283,23 @@ void SpatialHashCollision::addTriangle(std::shared_ptr<Mesh> mesh, int triangleI
     triangle.meshID = mesh->getUniqueId();
     triangle.primID = triangleId;
 
-    triangle.vert[0] = mesh->vertices[mesh->triangles[triangleId].vert[0]];
-    triangle.vert[1] = mesh->vertices[mesh->triangles[triangleId].vert[1]];
-    triangle.vert[2] = mesh->vertices[mesh->triangles[triangleId].vert[2]];
+    core::Vec3d cellSize(1.0/cellSizeX,1.0/cellSizeY,1.0/cellSizeZ);
 
-    int xStartIndex = static_cast<int>(std::floor(mesh->triAABBs[triangleId].aabbMin[0]/cellSizeX));
-    int yStartIndex = static_cast<int>(std::floor(mesh->triAABBs[triangleId].aabbMin[1]/cellSizeY));
-    int zStartIndex = static_cast<int>(std::floor(mesh->triAABBs[triangleId].aabbMin[2]/cellSizeZ));
+    auto startIndices = mesh->triAABBs[triangleId].min().array()*cellSize.array();
+    auto endIndices = mesh->triAABBs[triangleId].max().array()*cellSize.array();
 
-    int xEndIndex = static_cast<int>(std::floor(mesh->triAABBs[triangleId].aabbMax[0]/cellSizeX));
-    int yEndIndex = static_cast<int>(std::floor(mesh->triAABBs[triangleId].aabbMax[1]/cellSizeY));
-    int zEndIndex = static_cast<int>(std::floor(mesh->triAABBs[triangleId].aabbMax[2]/cellSizeZ));
+    int xStartIndex = static_cast<int>(std::floor(startIndices(0)));
+    int yStartIndex = static_cast<int>(std::floor(startIndices(1)));
+    int zStartIndex = static_cast<int>(std::floor(startIndices(2)));
 
+    int xEndIndex = static_cast<int>(std::floor(endIndices(0)));
+    int yEndIndex = static_cast<int>(std::floor(endIndices(1)));
+    int zEndIndex = static_cast<int>(std::floor(endIndices(2)));
+
+    auto &vertices = mesh->getVertices();
+    triangle.vert[0] = vertices[mesh->triangles[triangleId].vert[0]];
+    triangle.vert[1] = vertices[mesh->triangles[triangleId].vert[1]];
+    triangle.vert[2] = vertices[mesh->triangles[triangleId].vert[2]];
     for (int ix = xStartIndex; ix <= xEndIndex; ix++)
         for (int iy = yStartIndex; iy <= yEndIndex; iy++)
             for (int iz = zStartIndex; iz <= zEndIndex; iz++)
@@ -329,41 +308,20 @@ void SpatialHashCollision::addTriangle(std::shared_ptr<Mesh> mesh, int triangleI
             }
 }
 
-void SpatialHashCollision::addLine(std::shared_ptr<LineMesh> mesh,
-                                   int edgeId, Hash<CellLine> &cells)
-{
-    CellLine  line;
-    line.meshID = mesh->getUniqueId();
-    line.primID = edgeId;
-    line.vert[0] = mesh->vertices[mesh->edges[edgeId].vert[0]];
-    line.vert[1] = mesh->vertices[mesh->edges[edgeId].vert[1]];
-
-    int xStartIndex = static_cast<int>(std::floor(mesh->edgeAABBs[edgeId].aabbMin[0]/cellSizeX));
-    int yStartIndex = static_cast<int>(std::floor(mesh->edgeAABBs[edgeId].aabbMin[1]/cellSizeY));
-    int zStartIndex = static_cast<int>(std::floor(mesh->edgeAABBs[edgeId].aabbMin[2]/cellSizeZ));
-
-    int xEndIndex = static_cast<int>(std::floor(mesh->edgeAABBs[edgeId].aabbMax[0]/cellSizeX));
-    int yEndIndex = static_cast<int>(std::floor(mesh->edgeAABBs[edgeId].aabbMax[1]/cellSizeY));
-    int zEndIndex = static_cast<int>(std::floor(mesh->edgeAABBs[edgeId].aabbMax[2]/cellSizeZ));
-
-    for (int ix = xStartIndex; ix <= xEndIndex; ix++)
-        for (int iy = yStartIndex; iy <= yEndIndex; iy++)
-            for (int iz = zStartIndex; iz <= zEndIndex; iz++)
-            {
-                cells.checkAndInsert(line, hasher->getKey(cells.tableSize, ix, iy, iz));
-            }
-}
 
 void SpatialHashCollision::addPoint(std::shared_ptr<Mesh> mesh, int vertId, Hash<CellPoint> &cells)
 {
     CellPoint cellPoint;
     cellPoint.meshID = mesh->getUniqueId();
     cellPoint.primID = vertId;
-    cellPoint.vert = mesh->vertices[vertId];
 
-    int xStartIndex = static_cast<int>(std::floor(mesh->vertices[vertId][0]/cellSizeX));
-    int yStartIndex = static_cast<int>(std::floor(mesh->vertices[vertId][1]/cellSizeY));
-    int zStartIndex = static_cast<int>(std::floor(mesh->vertices[vertId][2]/cellSizeZ));
+    auto &vertices = mesh->getVertices();
+
+    cellPoint.vert = vertices[vertId];
+
+    int xStartIndex = static_cast<int>(std::floor(vertices[vertId][0]/cellSizeX));
+    int yStartIndex = static_cast<int>(std::floor(vertices[vertId][1]/cellSizeY));
+    int zStartIndex = static_cast<int>(std::floor(vertices[vertId][2]/cellSizeZ));
 
     cells.checkAndInsert(cellPoint, hasher->getKey(cells.tableSize, xStartIndex, yStartIndex, zStartIndex));
 }
@@ -411,7 +369,6 @@ void SpatialHashCollision::reset()
     cellLines.clearAll();
     cellsForTri2Line.clearAll();
     cellsForModelPoints.clearAll();
-    lineMeshes.clear();
     collidedLineTris.clear();
     collidedModelPoints.clear();
     collidedTriangles.clear();
@@ -419,18 +376,20 @@ void SpatialHashCollision::reset()
 bool SpatialHashCollision::findCandidates()
 {
     for(size_t i = 0; i < colModel.size(); i++)
+    {
         for(size_t i = 0; i < meshes.size(); i++)
         {
             findCandidatePoints(meshes[i], colModel[i]);
             addOctreeCell(colModel[i], cellsForModel);
         }
+    }
 
     ///Triangle-Triangle collision
     for(size_t i = 0; i < meshes.size(); i++)
     {
         for(size_t j = i + 1; j < meshes.size(); j++)
         {
-            if(meshes[i]->collisionGroup.isCollisionPermitted(meshes[j]->collisionGroup))
+            if(meshes[i]->getCollisionGroup()->isCollisionPermitted(meshes[j]->getCollisionGroup()))
             {
                 if(findCandidateTris(meshes[i], meshes[j]) == false)
                 {
@@ -440,18 +399,6 @@ bool SpatialHashCollision::findCandidates()
         }
     }
 
-    ///Triangle-line Collision
-    for(size_t i = 0; i < meshes.size(); i++)
-        for(size_t j = 0; j < lineMeshes.size(); j++)
-        {
-            if(meshes[i]->collisionGroup.isCollisionPermitted(lineMeshes[j]->collisionGroup))
-            {
-                if(findCandidateTrisLines(meshes[i], lineMeshes[j]) == false)
-                {
-                    continue;
-                }
-            }
-        }
     return 0;
 }
 void SpatialHashCollision::updateBVH()
@@ -460,11 +407,6 @@ void SpatialHashCollision::updateBVH()
     {
         meshes[i]->updateTriangleAABB();
     }
-
-    for(size_t i = 0; i < lineMeshes.size(); i++)
-    {
-        meshes[i]->upadateAABB();
-    }
 }
 const std::vector< std::shared_ptr< CollidedTriangles > >& SpatialHashCollision::getCollidedTriangles() const
 {
diff --git a/Collision/SpatialHashCollision.h b/Collision/SpatialHashCollision.h
index 7e80067556935877fbb58fcebd4ff2142f0f3177..b69072a5dd5a389b98b8373c37c3ba15fc9ad1c3 100644
--- a/Collision/SpatialHashCollision.h
+++ b/Collision/SpatialHashCollision.h
@@ -39,7 +39,6 @@ class CellTriangle;
 class CollidedLineTris;
 class CollidedModelPoints;
 class CollidedTriangles;
-class LineMesh;
 class Mesh;
 class OctreeCell;
 
@@ -69,9 +68,6 @@ public:
     /// \brief !!
     void addMesh(std::shared_ptr<Mesh> mesh);
 
-    /// \brief !!
-    void addMesh(std::shared_ptr<LineMesh> mesh);
-
     /// \brief !!
     void removeMesh(std::shared_ptr<Mesh> mesh);
 
@@ -84,9 +80,6 @@ public:
     /// \brief find the candidate triangle pairs for collision (broad phase collision)
     bool findCandidateTris(std::shared_ptr<Mesh> meshA, std::shared_ptr<Mesh> meshB);
 
-    /// \brief find the candidate line-triangle pairs for collision (broad phase collision)
-    bool findCandidateTrisLines(std::shared_ptr<Mesh> meshA, std::shared_ptr<LineMesh> meshB);
-
     /// \brief compute the collision between two triangles (narrow phase collision)
     void computeCollisionTri2Tri();
 
@@ -107,9 +100,6 @@ protected:
     /// \brief adds triangle to hash
     void addTriangle(std::shared_ptr<Mesh> mesh, int triangleId, Hash<CellTriangle> &cells);
 
-    /// \brief adds line to hash
-    void addLine(std::shared_ptr<LineMesh> mesh, int edgeId, Hash<CellLine> &cells);
-
     /// \brief adds point to hash
     void addPoint(std::shared_ptr<Mesh> mesh, int vertId, Hash<CellPoint> &cells);
 
@@ -143,7 +133,6 @@ private:
     Hash<CellModel> cellsForModel; // Candidate cells for collision model
     Hash<CellPoint> cellsForModelPoints; // Candidate for Collision model to point
     std::vector<std::shared_ptr<Mesh>> meshes; // Mesh models
-    std::vector<std::shared_ptr<LineMesh>> lineMeshes; // Line mehs models
     std::vector<std::shared_ptr<CollidedTriangles>> collidedTriangles; // List of collision pairs triangles
     std::vector<std::shared_ptr<CollidedLineTris>> collidedLineTris; // List of collision pairs triangles-lines
     std::vector<std::shared_ptr<CollidedModelPoints>> collidedModelPoints; // List of collision pairs models-points
diff --git a/Collision/SurfaceTree.h b/Collision/SurfaceTree.h
index 063f7c25c65fa6004197954414272ebba4b0b7b4..4c5da94143bcd3b1422464c2275dc1d26315c793 100644
--- a/Collision/SurfaceTree.h
+++ b/Collision/SurfaceTree.h
@@ -116,7 +116,7 @@ public:
                               const std::shared_ptr<CellType> right,
                               std::vector<std::pair<std::shared_ptr<CellType>,std::shared_ptr<CellType>>> &result )
     {
-        if(!left->getAabb().overlaps(right->getAabb()))
+        if(left->getAabb().intersection(right->getAabb()).isEmpty())
         {
             return;
         }
diff --git a/Collision/SurfaceTree.hpp b/Collision/SurfaceTree.hpp
index 8e8cdf9acb3e1c48221356d3cf2c68f76f319043..1ffabc0aa50a355786714a7c60324205e50aba93 100644
--- a/Collision/SurfaceTree.hpp
+++ b/Collision/SurfaceTree.hpp
@@ -35,7 +35,6 @@ template <typename CellType>
 void SurfaceTree<CellType>::initStructure()
 {
     core::Vec3d center;
-    double edge;
     std::vector<int> triangles;
 
     for (int i = 0; i < mesh->nbrTriangles; ++i)
@@ -44,13 +43,12 @@ void SurfaceTree<CellType>::initStructure()
     }
     root = std::make_shared<CellType>();
 
-    center = mesh->aabb.center();
-    edge = std::max(std::max(mesh->aabb.halfSizeX(), mesh->aabb.halfSizeY()),
-                        mesh->aabb.halfSizeZ());
+    auto &boundingBox = mesh->getBoundingBox();
+    center = boundingBox.center();
     root->setCenter(center);
-    root->setLength(2 * edge);
+    root->setLength(boundingBox.sizes().maxCoeff());
     root->setIsEmpty(false);
-    root->setAabb(mesh->aabb);
+    root->setAabb(mesh->getBoundingBox());
 
     treeAllLevels[0] = *root.get();
     this->createTree(root, triangles, 0);
@@ -190,6 +188,7 @@ bool SurfaceTree<CellType>::createTree(std::shared_ptr<CellType> Node,
     std::array<CellType, CellType::numberOfSubdivisions> subDividedNodes;
     std::array<std::vector<int>, CellType::numberOfSubdivisions> triangleMatrix;
 
+    auto const &vertices = mesh->getVertices();
     int level = Node->getLevel();
 
     if(level >= maxLevel)
@@ -213,7 +212,7 @@ bool SurfaceTree<CellType>::createTree(std::shared_ptr<CellType> Node,
 
         for(const auto &i : Node->getVerticesIndices())
         {
-            totalDistance += (Node->getCenter() - mesh->vertices[i]).norm();
+            totalDistance += (Node->getCenter() - vertices[i]).norm();
         }
 
         float weightSum = 0;
@@ -223,7 +222,7 @@ bool SurfaceTree<CellType>::createTree(std::shared_ptr<CellType> Node,
         for(const auto &i : Node->getVerticesIndices())
         {
             // TODO: make sure this is what is meant: 1-d^2/D^2 and not (1-d^2)/D^2
-            weight = 1-(Node->getCenter()-mesh->vertices[i]).squaredNorm() / totalDistance2;
+            weight = 1-(Node->getCenter()-vertices[i]).squaredNorm() / totalDistance2;
             weightSum += weight;
             Node->addWeight(weight);
         }
@@ -249,9 +248,9 @@ bool SurfaceTree<CellType>::createTree(std::shared_ptr<CellType> Node,
         for (int j = 0; j < CellType::numberOfSubdivisions; ++j)
         {
             if (subDividedNodes[j].isCollidedWithTri(
-                        mesh->vertices[mesh->triangles[triangles[i]].vert[0]],
-                        mesh->vertices[mesh->triangles[triangles[i]].vert[1]],
-                        mesh->vertices[mesh->triangles[triangles[i]].vert[2]]))
+                        vertices[mesh->triangles[triangles[i]].vert[0]],
+                        vertices[mesh->triangles[triangles[i]].vert[1]],
+                        vertices[mesh->triangles[triangles[i]].vert[2]]))
             {
                 triangleMatrix[j].push_back(triangles[i]);
             }
@@ -322,7 +321,8 @@ template <typename CellType>
 void SurfaceTree<CellType>::updateStructure()
 {
     CellType *current;
-
+    auto const &vertices = mesh->getVertices();
+    auto const &origVertices = mesh->getOrigVertices();
     for (int i = levelStartIndex[maxLevel-1][0]; i < levelStartIndex[maxLevel-1][1]; ++i)
     {
         current = &treeAllLevels[i];
@@ -333,7 +333,7 @@ void SurfaceTree<CellType>::updateStructure()
         {
             for(auto &i : current->getVerticesIndices())
             {
-                tempCenter = tempCenter + (mesh->vertices[i]-mesh->origVerts[i]) * current->getWeight(counter);
+                tempCenter = tempCenter + (vertices[i]-origVertices[i]) * current->getWeight(counter);
                 counter++;
             }
 
diff --git a/Collision/UnitTests/MeshCollisionModelSpec.cpp b/Collision/UnitTests/MeshCollisionModelSpec.cpp
index dca190123149399eb0ae5b9a9651f9c95126d1de..efc35a1f51edc7730c94f5cc953b08f650af9ef4 100644
--- a/Collision/UnitTests/MeshCollisionModelSpec.cpp
+++ b/Collision/UnitTests/MeshCollisionModelSpec.cpp
@@ -60,10 +60,11 @@ go_bandit([](){
             vertices.emplace_back(2.0,1.0,-1.0);
             vertices.emplace_back(3.0,2.0,1.0);
 
-            mesh->vertices[0] = vertices[0];
-            mesh->vertices[1] = vertices[1];
-            mesh->vertices[2] = vertices[2];
-            mesh->vertices[3] = vertices[3];
+            auto vertexArray = mesh->getVertices();
+            vertexArray[0] = vertices[0];
+            vertexArray[1] = vertices[1];
+            vertexArray[2] = vertices[2];
+            vertexArray[3] = vertices[3];
 
             mesh->triangles[0].vert[0] = 0;
             mesh->triangles[0].vert[1] = 1;
@@ -107,10 +108,11 @@ go_bandit([](){
             vertices.emplace_back(2.0,1.0,-1.0);
             vertices.emplace_back(3.0,2.0,1.0);
 
-            mesh->vertices[0] = vertices[0];
-            mesh->vertices[1] = vertices[1];
-            mesh->vertices[2] = vertices[2];
-            mesh->vertices[3] = vertices[3];
+            auto vertexArray = mesh->getVertices();
+            vertexArray[0] = vertices[0];
+            vertexArray[1] = vertices[1];
+            vertexArray[2] = vertices[2];
+            vertexArray[3] = vertices[3];
 
             mesh->triangles[0].vert[0] = 0;
             mesh->triangles[0].vert[1] = 1;
@@ -153,10 +155,11 @@ go_bandit([](){
             vertices.emplace_back(2.0,1.0,-1.0);
             vertices.emplace_back(3.0,2.0,1.0);
 
-            mesh->vertices.push_back(vertices[0]);
-            mesh->vertices.push_back(vertices[1]);
-            mesh->vertices.push_back(vertices[2]);
-            mesh->vertices.push_back(vertices[3]);
+            auto vertexArray = mesh->getVertices();
+            vertexArray.push_back(vertices[0]);
+            vertexArray.push_back(vertices[1]);
+            vertexArray.push_back(vertices[2]);
+            vertexArray.push_back(vertices[3]);
 
             mesh->triangles[0].vert[0] = 0;
             mesh->triangles[0].vert[1] = 1;
diff --git a/Collision/UnitTests/MeshToMeshCollisionSpec.cpp b/Collision/UnitTests/MeshToMeshCollisionSpec.cpp
index ffc56f6b09f95bceacddd40a05d559e83ac163b5..e0a8fd75e13cad78fd9c369b7240778772bc2e0a 100644
--- a/Collision/UnitTests/MeshToMeshCollisionSpec.cpp
+++ b/Collision/UnitTests/MeshToMeshCollisionSpec.cpp
@@ -40,9 +40,11 @@ std::shared_ptr<MeshCollisionModel> getModel(const std::vector<core::Vec3d> &ver
     mesh->initVertexArrays(3);
     mesh->initTriangleArrays(1);
 
-    mesh->vertices[0] = vertices[0];
-    mesh->vertices[1] = vertices[1];
-    mesh->vertices[2] = vertices[2];
+    auto vertexArray = mesh->getVertices();
+
+    vertexArray[0] = vertices[0];
+    vertexArray[1] = vertices[1];
+    vertexArray[2] = vertices[2];
 
     mesh->triangles[0].vert[0] = 0;
     mesh->triangles[0].vert[1] = 1;
diff --git a/Core/BaseMesh.cpp b/Core/BaseMesh.cpp
index 163bbc37354af797930529a8635079ca3b9d19bb..d41f73e47461f6e8545ede49d6fe0df58822eb17 100644
--- a/Core/BaseMesh.cpp
+++ b/Core/BaseMesh.cpp
@@ -46,9 +46,6 @@ struct BaseMesh::TextureAttachment
 
     // Texture filename
     std::string textureFileName;
-
-    // True if the texture co-ordinate is available
-    bool isTextureCoordAvailable;
 };
 
 BaseMesh::BaseMesh() : collisionGroup(nullptr), log(nullptr)
@@ -57,6 +54,7 @@ BaseMesh::BaseMesh() : collisionGroup(nullptr), log(nullptr)
 }
 void BaseMesh::assignTexture( const int textureId )
 {
+    // Texture enumeration starts at 1.
     if(textureId < 1)
     {
         return;
@@ -106,18 +104,30 @@ std::vector<core::Vec3d>& BaseMesh::getVertices()
 {
     return this->vertices;
 }
+const core::Vec3d& BaseMesh::getVertex ( const size_t i ) const
+{
+    return this->vertices[i];
+}
+core::Vec3d& BaseMesh::getVertex ( const size_t i )
+{
+    return this->vertices[i];
+}
+const std::vector<core::Vec3d>& BaseMesh::getOrigVertices() const
+{
+    return this->origVerts;
+}
+std::vector<core::Vec3d>& BaseMesh::getOrigVertices()
+{
+    return this->origVerts;
+}
 std::size_t BaseMesh::getNumberOfVertices() const
 {
     return this->vertices.size();
 }
-std::shared_ptr< CollisionGroup > BaseMesh::getCollisionGroup() const
+std::shared_ptr< CollisionGroup > &BaseMesh::getCollisionGroup()
 {
     return this->collisionGroup;
 }
-Eigen::AlignedBox3d BaseMesh::getBoundingBox() const
-{
-    return this->aabb;
-}
 std::size_t BaseMesh::getRenderingId() const
 {
     return this->renderingID;
@@ -126,12 +136,16 @@ std::string BaseMesh::getTextureFileName(const size_t i) const
 {
     return textures[i]->textureFileName;
 }
-const std::vector<core::Vec2f,
-            Eigen::aligned_allocator<core::Vec2f>> &
+const std::vector<core::Vec2f,Eigen::aligned_allocator<core::Vec2f>> &
 BaseMesh::getTextureCoordinates() const
 {
     return this->textureCoord;
 }
+std::vector<core::Vec2f,Eigen::aligned_allocator<core::Vec2f>> &
+BaseMesh::getTextureCoordinates()
+{
+    return this->textureCoord;
+}
 const std::vector< std::shared_ptr< BaseMesh::TextureAttachment > > &
 BaseMesh::getTextures() const
 {
@@ -141,5 +155,31 @@ const int& BaseMesh::getTextureId(size_t i) const
 {
     return this->textures[i]->textureId;
 }
+bool BaseMesh::BaseMesh::hasTextureCoordinates() const
+{
+    return this->textureCoord.size() > 0;
+}
+void BaseMesh::addTextureCoordinate ( const core::Vec2f& coord )
+{
+    this->textureCoord.push_back( coord );
+}
+void BaseMesh::addTextureCoordinate ( const float& x, const float& y )
+{
+    this->textureCoord.push_back( core::Vec2f(x,y) );
+}
+void BaseMesh::setBoundingBox ( const Eigen::AlignedBox3d& box )
+{
+    this->aabb = box;
+}
+const Eigen::AlignedBox3d& BaseMesh::getBoundingBox() const
+{
+    return this->aabb;
+}
+void BaseMesh::setRenderingId( size_t id )
+{
+    this->renderingID = id;
+}
+BaseMesh::~BaseMesh() {}
+
 
 }
diff --git a/Core/BaseMesh.h b/Core/BaseMesh.h
index 481155591656ce74d47bda0771270a91474e90ae..0ea0122a943e1f92bceffbe3ddf23ffca0f57121 100644
--- a/Core/BaseMesh.h
+++ b/Core/BaseMesh.h
@@ -29,6 +29,7 @@
 
 #include "Core/CoreClass.h"
 #include "Core/Vector.h"
+#include "Core/Quaternion.h"
 
 class CollisionGroup;
 class ErrorLog;
@@ -70,9 +71,10 @@ public:
     };
 
     ///
-    /// \brief constructor
+    /// \brief constructor/Destructor
     ///
     BaseMesh();
+    virtual ~BaseMesh();
     ///
     /// \brief Stores the texture locally. It assumes that the texture has been
     ///  already added to the \TextureManager
@@ -95,11 +97,23 @@ public:
     bool isMeshTextured() const;
 
     ///
-    /// \brief Returns vertices coordinates
+    /// \brief Returns vertex coordinates
     ///
     const std::vector<core::Vec3d> &getVertices() const;
     std::vector<core::Vec3d> &getVertices();
 
+    ///
+    /// \brief Returns vertex ith coordinate
+    ///
+    const core::Vec3d &getVertex(const size_t i) const;
+    core::Vec3d &getVertex(const size_t i);
+
+    ///
+    /// \brief Returns original vertex coordinates
+    ///
+    const std::vector<core::Vec3d> &getOrigVertices() const;
+    std::vector<core::Vec3d> &getOrigVertices();
+
     ///
     /// \brief Get the total number of vertices
     ///
@@ -108,17 +122,17 @@ public:
     ///
     /// \brief Return the collision group this mesh belongs to.
     ///
-    std::shared_ptr<CollisionGroup> getCollisionGroup() const;
+    std::shared_ptr<CollisionGroup> &getCollisionGroup();
 
     ///
-    /// \brief Returns the bounding box for this mesh.
+    /// \brief Set the rendering id
     ///
-    Eigen::AlignedBox3d getBoundingBox() const;
+    size_t getRenderingId() const;
 
     ///
-    /// \brief Returns the bounding box for this mesh.
+    /// \brief Set the rendering id
     ///
-    size_t getRenderingId() const;
+    void setRenderingId(size_t id);
 
     ///
     /// \brief Returns the the name of ith texture.
@@ -145,6 +159,46 @@ public:
     ///
     const int &getTextureId(size_t i) const;
 
+    ///
+    /// \brief Return true if it contains texture coordinates
+    ///
+    bool hasTextureCoordinates() const;
+
+    ///
+    /// \brief Add texture coordinates
+    ///
+    void addTextureCoordinate(const core::Vec2f &coord);
+    void addTextureCoordinate(const float &x, const float &y);
+
+    ///
+    /// \brief Set/get bounding box
+    ///
+    void setBoundingBox(const Eigen::AlignedBox3d &box);
+    const Eigen::AlignedBox3d &getBoundingBox() const;
+
+    ///
+    /// \brief Translate the mesh
+    ///
+    void translate(core::Vec3d offset);
+
+    ///
+    /// \brief Scale the mesh
+    ///
+    void scale(core::Vec3d factor);
+
+    ///
+    /// \brief Rotate the mesh
+    ///
+    void rotate(const Quaterniond &rot)
+    {
+        auto &vertices = this->getVertices();
+        auto &origVerts = this->getOrigVertices();
+        for (size_t i = 0, end = this->getNumberOfVertices(); i < end; ++i)
+        {
+            vertices[i] = R * vertices[i];
+            origVerts[i] = R * origVerts[i];
+        }
+    }
 private:
     // Data arrays - Vertices only
     // vertices co-ordinate data at time t
diff --git a/Core/CollisionConfig.cpp b/Core/CollisionConfig.cpp
index abcbdece78d58871d7fb712b1af2366738a0fe1f..0c5de3506f1ee0d9f5fe5efb553c3049cb95a41f 100644
--- a/Core/CollisionConfig.cpp
+++ b/Core/CollisionConfig.cpp
@@ -98,21 +98,21 @@ void CollisionGroup::setGroup( unsigned int p_groupId )
     groupMask = groupId;
 }
 
-void CollisionGroup::permitCollision( CollisionGroup &p_group )
+void CollisionGroup::permitCollision( std::shared_ptr<CollisionGroup> p_group )
 {
-    groupMask = groupMask | ( p_group.groupId );
-    p_group.groupMask = p_group.groupMask | ( groupId );
+    groupMask = groupMask | ( p_group->groupId );
+    p_group->groupMask = p_group->groupMask | ( groupId );
     std::cout << "Mask:" << groupMask << "\n";
     std::cout << "GroupId:" << groupId << "\n";
 }
 
-void CollisionGroup::forbidCollision( CollisionGroup &p_group )
+void CollisionGroup::forbidCollision( std::shared_ptr<CollisionGroup> p_group )
 {
-    groupMask = groupMask & ( ~p_group.groupId );
-    p_group.groupMask = p_group.groupMask & ( ~groupId );
+    groupMask = groupMask & ( ~p_group->groupId );
+    p_group->groupMask = p_group->groupMask & ( ~groupId );
 }
 
-bool CollisionGroup::isCollisionPermitted( CollisionGroup &p_group )
+bool CollisionGroup::isCollisionPermitted( std::shared_ptr<CollisionGroup> p_group ) const
 {
-    return ( ( groupId & p_group.groupId ) == 0 ? ( ( groupId & p_group.groupMask ) == 0 ? false : true ) : true );
+    return ( ( groupId & p_group->groupId ) == 0 ? ( ( groupId & p_group->groupMask ) == 0 ? false : true ) : true );
 }
diff --git a/Core/CollisionConfig.h b/Core/CollisionConfig.h
index ef0af5f83538ffb02ff045bb4d13761ada4c5651..9109885c019d3cd32dc32feb5568b7737b43ed11 100644
--- a/Core/CollisionConfig.h
+++ b/Core/CollisionConfig.h
@@ -140,14 +140,14 @@ public:
     void setGroup(unsigned int p_groupId);
 
     /// \brief !!
-    void permitCollision(CollisionGroup &p_group);
+    void permitCollision(std::shared_ptr<CollisionGroup> p_group);
 
     /// \brief !!
-    void forbidCollision(CollisionGroup &p_group);
+    void forbidCollision(std::shared_ptr<CollisionGroup> p_group);
 
     /// \brief if the group is different then the function checks the group mask..
     //  if they're in the same group collision enabled
-    bool isCollisionPermitted(CollisionGroup& p_group);
+    bool isCollisionPermitted(std::shared_ptr<CollisionGroup> p_group) const;
 
 };
 
diff --git a/Core/SDK.cpp b/Core/SDK.cpp
index b591efe828b24e0802dcd45cd1f68129a37a21d7..ae03532ad324770e7e6c3aa1f69b7ec78880155b 100644
--- a/Core/SDK.cpp
+++ b/Core/SDK.cpp
@@ -250,7 +250,7 @@ void SDK::terminateAll()
 }
 
 /// \brief register functions
-void SDK::registerMesh(std::shared_ptr<BaseMesh> newMesh)
+void SDK::registerMesh(std::shared_ptr<Core::BaseMesh> newMesh)
 {
     if(std::end(this->meshList) ==
         std::find(std::begin(this->meshList),std::end(this->meshList),newMesh) )
diff --git a/Core/SDK.h b/Core/SDK.h
index 572eb0c099a853613ee32d5f0aceeb481212ebfb..48353e5c35eeeb37c023850b6723388c084f8f15 100644
--- a/Core/SDK.h
+++ b/Core/SDK.h
@@ -114,7 +114,7 @@ public:
     void removeRef(std::shared_ptr<CoreClass> p_coreClass);
 
     /// \brief register functions
-    void registerMesh(std::shared_ptr<BaseMesh> p_mesh);
+    void registerMesh(std::shared_ptr<Core::BaseMesh> p_mesh);
 
     void registerModule(std::shared_ptr<Module> p_mod);
 
@@ -142,7 +142,7 @@ private:
     std::shared_ptr<Simulator> simulator; ///< Reference to the sdk simulator object
 
     ///holds the references to the entities in the framework
-    std::vector<std::shared_ptr<BaseMesh>> meshList;
+    std::vector<std::shared_ptr<Core::BaseMesh>> meshList;
     std::vector<std::shared_ptr<Module>> moduleList;
     std::vector<std::shared_ptr<ObjectSimulator>> simulatorList;
     std::vector<std::shared_ptr<CollisionDetection>> collisionDetectionList;
diff --git a/Examples/CollisionDetectionBVH/CollisionDetectionBVH.cpp b/Examples/CollisionDetectionBVH/CollisionDetectionBVH.cpp
index 10549ccc7542f9e47c697c7db09de71e80978240..4247b412ec0d9d009314d553a61501914da59c04 100644
--- a/Examples/CollisionDetectionBVH/CollisionDetectionBVH.cpp
+++ b/Examples/CollisionDetectionBVH/CollisionDetectionBVH.cpp
@@ -68,18 +68,18 @@ CollisionDetectionBVH::CollisionDetectionBVH()
 
     // Create collision models
     std::shared_ptr<MeshCollisionModel> collisionModelA = std::make_shared<MeshCollisionModel>();
-    collisionModelA->loadTriangleMesh("models/liverNormalized_SB2.3DS", BaseMesh::MeshFileType::ThreeDS);
-    collisionModelA->getMesh()->assignTexture("livertexture1");
+    collisionModelA->loadTriangleMesh("models/liverNormalized_SB2.3DS", Core::BaseMesh::MeshFileType::ThreeDS);
+    collisionModelA->getMesh()->assignTexture("","livertexture1");
     collisionModelA->getMesh()->getRenderDetail()->renderType = (SIMMEDTK_RENDER_FACES | SIMMEDTK_RENDER_WIREFRAME);
-    collisionModelA->getMesh()->translate(7, 3, 0);
+    collisionModelA->getMesh()->translate(core::Vec3d(7, 3, 0));
     collisionModelA->getMesh()->getRenderDetail()->lineSize = 2;
     collisionModelA->getMesh()->getRenderDetail()->pointSize = 5;
 
     std::shared_ptr<MeshCollisionModel> collisionModelB = std::make_shared<MeshCollisionModel>();
-    collisionModelB->loadTriangleMesh("models/liverNormalized_SB2.3DS", BaseMesh::MeshFileType::ThreeDS);
-    collisionModelB->getMesh()->assignTexture("livertexture2");
+    collisionModelB->loadTriangleMesh("models/liverNormalized_SB2.3DS", Core::BaseMesh::MeshFileType::ThreeDS);
+    collisionModelB->getMesh()->assignTexture("","livertexture2");
     collisionModelB->getMesh()->translate(core::Vec3d(2, 0, 0));
-    collisionModelB->getMesh()->assignTexture("livertexture2");
+    collisionModelB->getMesh()->assignTexture("","livertexture2");
     collisionModelB->getMesh()->getRenderDetail()->shadowColor.rgba[0] = 1.0;
     collisionModelB->getMesh()->getRenderDetail()->renderType = (SIMMEDTK_RENDER_FACES | SIMMEDTK_RENDER_WIREFRAME);
 
@@ -142,13 +142,13 @@ void CollisionDetectionBVH::simulateMain(const SimulationMainParam &/*p_param*/)
 {
     if ((10 > moveObj) && (moveObj > 0))
     {
-        modelB->getModel()->getMesh()->translate(1, 0, 0);
+        modelB->getModel()->getMesh()->translate(core::Vec3d(1, 0, 0));
         moveObj--;
     }
     else
     {
         moveObj = 9; // reset
-        modelB->getModel()->getMesh()->translate(-moveObj, 0, 0);
+        modelB->getModel()->getMesh()->translate(core::Vec3d(-moveObj, 0, 0));
     }
 
 	std::this_thread::sleep_for(std::chrono::milliseconds(50));
diff --git a/Examples/CollisionDetectionSpatialHashing/CollisionDetectionSpatialHashing.cpp b/Examples/CollisionDetectionSpatialHashing/CollisionDetectionSpatialHashing.cpp
index b3e8fe02bd35f93f21a925dba6bebc2aef0fc994..248394b7bd68d6a6d61bdbb647a59c328ef6f5de 100644
--- a/Examples/CollisionDetectionSpatialHashing/CollisionDetectionSpatialHashing.cpp
+++ b/Examples/CollisionDetectionSpatialHashing/CollisionDetectionSpatialHashing.cpp
@@ -70,18 +70,18 @@ CollisionDetectionSpatialHashing::CollisionDetectionSpatialHashing()
 
         // Create collision models
     std::shared_ptr<MeshCollisionModel> collisionModelA = std::make_shared<MeshCollisionModel>();
-    collisionModelA->loadTriangleMesh("models/liverNormalized_SB2.3DS", BaseMesh::MeshFileType::ThreeDS);
-    collisionModelA->getMesh()->assignTexture("livertexture1");
+    collisionModelA->loadTriangleMesh("models/liverNormalized_SB2.3DS", Core::BaseMesh::MeshFileType::ThreeDS);
+    collisionModelA->getMesh()->assignTexture("","livertexture1");
     collisionModelA->getMesh()->getRenderDetail()->renderType = (SIMMEDTK_RENDER_FACES | SIMMEDTK_RENDER_WIREFRAME);
-    collisionModelA->getMesh()->translate(7, 3, 0);
+    collisionModelA->getMesh()->translate(core::Vec3d(7, 3, 0));
     collisionModelA->getMesh()->getRenderDetail()->lineSize = 2;
     collisionModelA->getMesh()->getRenderDetail()->pointSize = 5;
 
     std::shared_ptr<MeshCollisionModel> collisionModelB = std::make_shared<MeshCollisionModel>();
-    collisionModelB->loadTriangleMesh("models/liverNormalized_SB2.3DS", BaseMesh::MeshFileType::ThreeDS);
-    collisionModelB->getMesh()->assignTexture("livertexture2");
+    collisionModelB->loadTriangleMesh("models/liverNormalized_SB2.3DS", Core::BaseMesh::MeshFileType::ThreeDS);
+    collisionModelB->getMesh()->assignTexture("","livertexture2");
     collisionModelB->getMesh()->translate(core::Vec3d(2, 0, 0));
-    collisionModelB->getMesh()->assignTexture("livertexture2");
+    collisionModelB->getMesh()->assignTexture("","livertexture2");
     collisionModelB->getMesh()->getRenderDetail()->shadowColor.rgba[0] = 1.0;
     collisionModelB->getMesh()->getRenderDetail()->renderType = (SIMMEDTK_RENDER_FACES | SIMMEDTK_RENDER_WIREFRAME);
 
@@ -141,13 +141,13 @@ void CollisionDetectionSpatialHashing::simulateMain(const SimulationMainParam &/
 {
     if ((10 > moveObj) && (moveObj > 0))
     {
-        modelB->getModel()->getMesh()->translate(1, 0, 0);
+        modelB->getModel()->getMesh()->translate(core::Vec3d(1, 0, 0));
         moveObj--;
     }
     else
     {
         moveObj = 9; // reset
-        modelB->getModel()->getMesh()->translate(-moveObj, 0, 0);
+        modelB->getModel()->getMesh()->translate(core::Vec3d(-moveObj, 0, 0));
     }
 
 	std::this_thread::sleep_for(std::chrono::milliseconds(50));
diff --git a/Examples/renderCubeToTexture/main.cpp b/Examples/renderCubeToTexture/main.cpp
index 463a7207f3e68bdf808643b13ebe6958f6a18947..1930d8f196919031b62c449a0a91ba19f3137510 100644
--- a/Examples/renderCubeToTexture/main.cpp
+++ b/Examples/renderCubeToTexture/main.cpp
@@ -74,8 +74,8 @@ int main()
     TextureManager::createDepthTexture("depthTex1", 64, 64);
 
     std::shared_ptr<MeshModel> squareModel = std::make_shared<MeshModel>();
-    squareModel->load("models/square.obj", BaseMesh::MeshFileType::Obj);
-    squareModel->getMesh()->assignTexture("colorTex1");
+    squareModel->load("models/square.obj", Core::BaseMesh::MeshFileType::Obj);
+    squareModel->getMesh()->assignTexture("","colorTex1");
     renderDetail= std::make_shared<RenderDetail>(SIMMEDTK_RENDER_FACES | SIMMEDTK_RENDER_TEXTURE);
     squareModel->setRenderDetail(renderDetail);
 
diff --git a/Geometry/MeshModel.cpp b/Geometry/MeshModel.cpp
index c947dc0f4782a13e0126531b0ff9002cb9e69b67..545597249b56e39f5acf78f1d45bba77c56ec181 100644
--- a/Geometry/MeshModel.cpp
+++ b/Geometry/MeshModel.cpp
@@ -26,25 +26,25 @@
 
 MeshModel::MeshModel() {}
 MeshModel::~MeshModel() {}
-void MeshModel::load(const std::string& meshName, const BaseMesh::MeshFileType& type)
+void MeshModel::load(const std::string& meshName, const Core::BaseMesh::MeshFileType& type)
 {
     this->mesh.reset();
 
     switch(type)
     {
-        case BaseMesh::MeshFileType::Obj:
+        case Core::BaseMesh::MeshFileType::Obj:
         {
             this->mesh = std::make_shared<SurfaceMesh>();
             break;
         }
 
-        case BaseMesh::MeshFileType::ThreeDS: // TODO: Is this a surface or volume mesh?
+        case Core::BaseMesh::MeshFileType::ThreeDS: // TODO: Is this a surface or volume mesh?
         {
             this->mesh = std::make_shared<SurfaceMesh>();
             break;
         }
 
-        case BaseMesh::MeshFileType::Volume:
+        case Core::BaseMesh::MeshFileType::Volume:
         {
             this->mesh = std::make_shared<VolumeMesh>();
             break;
@@ -65,9 +65,9 @@ const core::Vec3d& MeshModel::getNormal(size_t i) const
 std::array<core::Vec3d,3> MeshModel::getTrianglePositions(size_t i) const
 {
     std::array<core::Vec3d, 3> vertices;
-    vertices[0] = this->mesh->vertices[this->mesh->triangles[i].vert[0]];
-    vertices[1] = this->mesh->vertices[this->mesh->triangles[i].vert[1]];
-    vertices[2] = this->mesh->vertices[this->mesh->triangles[i].vert[2]];
+    vertices[0] = this->mesh->getVertex(this->mesh->triangles[i].vert[0]);
+    vertices[1] = this->mesh->getVertex(this->mesh->triangles[i].vert[1]);
+    vertices[2] = this->mesh->getVertex(this->mesh->triangles[i].vert[2]);
 
     return vertices;
 }
@@ -92,7 +92,7 @@ std::shared_ptr< Mesh > MeshModel::getMesh()
 }
 void MeshModel::load(const std::string& meshFileName, const std::string& textureFileName, const std::string& textureName)
 {
-    this->load(meshFileName, BaseMesh::MeshFileType::Obj);
+    this->load(meshFileName, Core::BaseMesh::MeshFileType::Obj);
 
     if(nullptr != this->mesh)
     {
@@ -101,7 +101,7 @@ void MeshModel::load(const std::string& meshFileName, const std::string& texture
 
         //Load in the texture for the model
         TextureManager::loadTexture(textureFileName, textureName);
-        this->mesh->assignTexture(textureName);
+        this->mesh->assignTexture(meshFileName,textureName);
     }
 }
 void MeshModel::setRenderDetail(std::shared_ptr< RenderDetail > renderDetail)
diff --git a/Geometry/MeshModel.h b/Geometry/MeshModel.h
index 1f718f27b16c0b8f43cfc427b7a1d5301e192fda..5f2b213ead5a3fd5ec1b97574f88c3f3c2ff1022 100644
--- a/Geometry/MeshModel.h
+++ b/Geometry/MeshModel.h
@@ -55,7 +55,7 @@ public:
     ///
     /// @brief Loads the mesh and stores it.
     ///
-    void load(const std::string& meshName, const BaseMesh::MeshFileType &type);
+    void load(const std::string& meshName, const Core::BaseMesh::MeshFileType &type);
 
     ///
     /// @brief Loads the mesh with texture and stores it. Only surface meshes allowed.
diff --git a/Geometry/UnitTests/MeshModelSpec.cpp b/Geometry/UnitTests/MeshModelSpec.cpp
index f09a0992586da0cbd0b76dd9ed01b864526c1d15..3d5888c00e3ad0be889e68c93e9d4e6fbfae79b9 100644
--- a/Geometry/UnitTests/MeshModelSpec.cpp
+++ b/Geometry/UnitTests/MeshModelSpec.cpp
@@ -37,10 +37,10 @@ std::shared_ptr<MeshModel> getModel(const std::vector<core::Vec3d> &vertices)
     // Add one triangle to the data structure
     mesh->initVertexArrays(3);
     mesh->initTriangleArrays(1);
-
-    mesh->vertices[0] = vertices[0];
-    mesh->vertices[1] = vertices[1];
-    mesh->vertices[2] = vertices[2];
+    auto &vertexArray = mesh->getVertices();
+    vertexArray[0] = vertices[0];
+    vertexArray[1] = vertices[1];
+    vertexArray[2] = vertices[2];
 
     mesh->triangles[0].vert[0] = 0;
     mesh->triangles[0].vert[1] = 1;
diff --git a/Mesh/Mesh.cpp b/Mesh/Mesh.cpp
index 45577dad23a6bdde1b57146324304e65e35545f3..15783a16aaad2b777c02038a5e84f09017b71233 100644
--- a/Mesh/Mesh.cpp
+++ b/Mesh/Mesh.cpp
@@ -36,16 +36,6 @@
 
 #include "VtkRendering/VtkRenderDelegate.h"
 
-BaseMesh::BaseMesh()
-{
-//     SDK::getInstance()->registerMesh(safeDownCast<BaseMesh>());
-}
-
-void BaseMesh::updateOriginalVertsWithCurrent()
-{
-    origVerts = vertices;
-}
-
 /// \brief constructor
 Mesh::Mesh()
 {
@@ -65,7 +55,6 @@ Mesh::Mesh()
 /// \brief destructor
 Mesh::~Mesh()
 {
-    delete [] texCoord;
     delete [] triNormals;
     delete [] vertNormals;
     delete [] triTangents;
@@ -155,7 +144,6 @@ void Mesh::calcTriangleTangents()
     auto texCoordArray = this->getTextureCoordinates();
     for (t = 0; t < nbrTriangles; t++)
     {
-        Triangle *tmpTri = &triangles[t];
         auto &v0 = vertexArray[triangles[t].vert[0]];
         auto &v1 = vertexArray[triangles[t].vert[1]];
         auto &v2 = vertexArray[triangles[t].vert[2]];
@@ -193,64 +181,53 @@ void Mesh::calcTriangleTangents()
 }
 
 /// \brief calucate the triangle tangent for rendering purposes
-core::Vec3d Mesh::calculateTangent(core::Vec3d& p1, core::Vec3d& p2, core::Vec3d& p3, TexCoord& t1, TexCoord& t2, TexCoord& t3)
+core::Vec3d Mesh::calculateTangent(const core::Vec3d& p1,
+                                   const core::Vec3d& p2,
+                                   const core::Vec3d& p3,
+                                   const core::Vec2f& t1,
+                                   const core::Vec2f& t2,
+                                   const core::Vec2f& t3)
 {
-    core::Vec3d tangent;
-    core::Vec3d v1;
-    core::Vec3d v2;
-
-    v1[0] = p2[0] - p1[0];
-    v1[1] = p2[1] - p1[1];
-    v1[2] = p2[2] - p1[2];
-
-    v2[0] = p3[0] - p1[0];
-    v2[1] = p3[1] - p1[1];
-    v2[2] = p3[2] - p1[2];
+    core::Vec3d v1 = p2-p1;
+    core::Vec3d v2 = p3-p1;
 
-    float bb1 = t2.v - t1.v;
-    float bb2 = t3.v - t1.v;
+    float bb1 = t2(1) - t1(1);
+    float bb2 = t3(1) - t1(1);
 
-    tangent[0] = bb2 * v1[0] - bb1 * v2[0];
-    tangent[1] = bb2 * v1[1] - bb1 * v2[1];
-    tangent[2] = bb2 * v1[2] - bb1 * v2[2];
+    core::Vec3d tangent = bb2 * v1 - bb1 * v2;
 
     return tangent.normalized();
 }
 
 /// \brief
-core::Vec3d Mesh::calculateTangent_test(core::Vec3d& p1, core::Vec3d& p2, core::Vec3d& p3, TexCoord& t1, TexCoord& t2, TexCoord& t3)
+core::Vec3d Mesh::calculateTangent_test(const core::Vec3d& p1,
+                                        const core::Vec3d& p2,
+                                        const core::Vec3d& p3,
+                                        const core::Vec2f& t1,
+                                        const core::Vec2f& t2,
+                                        const core::Vec2f& t3)
 {
+    core::Vec3d v1 = p2-p1;
+    core::Vec3d v2 = p3-p1;
 
-    core::Vec3d tangent;
-    core::Vec3d v1;
-    core::Vec3d v2;
+    float tt1 = t2(0)-t1(0);
+    float tt2 = t3(0)-t1(0);
 
-    v1[0] = p2[0] - p1[0];
-    v1[1] = p2[1] - p1[1];
-    v1[2] = p2[2] - p1[2];
+    float bb1 = t2(1)-t1(1);
+    float bb2 = t3(1)-t1(1);
+    float r = 1.0f/(tt1 * bb2 - tt2 * bb1);
 
-    v2[0] = p3[0] - p1[0];
-    v2[1] = p3[1] - p1[1];
-    v2[2] = p3[2] - p1[2];
+    core::Vec3d tangent = (bb2*v1 - bb1*v2)*r;
 
-    float tt1 = t2.u - t1.u;
-    float tt2 = t3.u - t1.u;
-
-    float bb1 = t2.v - t1.v;
-    float bb2 = t3.v - t1.v;
-    float r = 1.0F / (tt1 * bb2 - tt2 * bb1);
-    tangent = (bb2*v1 - bb1*v2)*r;
-
-    return tangent;
+    return tangent.normalized();
 }
 
 /// \brief calculates the normal of the vertex
 void Mesh::updateVertexNormals()
 {
-    core::Vec3d temp = core::Vec3d::Zero();
-
-    for (int i = 0; i < nbrVertices; i++)
+    for (size_t i = 0, end = this->getNumberOfVertices(); i < end; ++i)
     {
+        core::Vec3d temp = core::Vec3d::Zero();
         for (size_t j = 0; j < vertTriNeighbors[i].size(); j++)
         {
             temp += triNormals[vertTriNeighbors[i][j]];
@@ -258,7 +235,6 @@ void Mesh::updateVertexNormals()
 
         vertNormals[i] = temp;
         vertNormals[i].normalize();
-        temp = core::Vec3d::Zero();
     }
 }
 
@@ -275,15 +251,11 @@ void Mesh::updateTriangleNormals()
 /// \brief calculates the normal of a triangle
 core::Vec3d Mesh::calculateTriangleNormal(int triNbr)
 {
+    Triangle t = this->triangles[triNbr];
 
-    core::Vec3d v[3];
-    Triangle temp = this->triangles[triNbr];
+    const core::Vec3d &v = this->getVertex(t.vert[0]);
 
-    v[0] = this->vertices[temp.vert[0]];
-    v[1] = this->vertices[temp.vert[1]];
-    v[2] = this->vertices[temp.vert[2]];
-
-    return (v[1] - v[0]).cross(v[2] - v[0]).normalized();
+    return (this->getVertex(t.vert[1]) - v).cross(this->getVertex(t.vert[2]) - v).normalized();
 }
 
 /// \brief allocates vertices and related array
@@ -295,12 +267,12 @@ bool Mesh::initVertexArrays(int nbr)
         return false;
     }
 
-    this->nbrVertices = nbr;
-    this->vertices.resize(nbr);
-    this->origVerts.resize(nbr);
+    this->getVertices().resize(nbr);
+    this->getOrigVertices().resize(nbr);
+    this->getTextureCoordinates().resize(nbr);
+
     this->vertNormals = new core::Vec3d[nbr];
     this->vertTangents = new core::Vec3d[nbr];
-    this->texCoord = new TexCoord[nbr];
     return true;
 }
 
@@ -324,11 +296,9 @@ bool Mesh::initTriangleArrays(int nbr)
 /// \brief initializes the vertex neighbors
 void Mesh::initVertexNeighbors()
 {
+    vertTriNeighbors.resize(this->getNumberOfVertices());
 
-    int i;
-    vertTriNeighbors.resize(nbrVertices);
-
-    for (i = 0; i < nbrTriangles; i++)
+    for (int i = 0; i < nbrTriangles; ++i)
     {
         vertTriNeighbors[triangles[i].vert[0]].push_back(i);
         vertTriNeighbors[triangles[i].vert[1]].push_back(i);
@@ -339,33 +309,31 @@ void Mesh::initVertexNeighbors()
 /// \brief initializes the vertex neighbors
 void Mesh::calcNeighborsVertices()
 {
-
-    int i;
-    int triangleIndex;
     int candidate[3];
 
-    vertVertNeighbors.resize(nbrVertices);
+    vertVertNeighbors.resize(this->getNumberOfVertices());
 
-    for (i = 0; i < nbrVertices; i++)
+    for (size_t i = 0, end = vertVertNeighbors.size(); i < end; ++i)
     {
-        for (size_t j = 0; j < vertTriNeighbors[i].size(); j++)
+        int ii = int(i);
+        for (size_t j = 0, end_j = vertTriNeighbors[i].size(); j < end_j; ++j)
         {
-            triangleIndex = vertTriNeighbors[i][j];
+            auto triangleIndex = vertTriNeighbors[i][j];
             candidate[0] = triangles[triangleIndex].vert[0];
             candidate[1] = triangles[triangleIndex].vert[1];
             candidate[2] = triangles[triangleIndex].vert[2];
 
-            if (candidate[0] == i)
+            if (candidate[0] == ii)
             {
                 candidate[0] = -1;
             }
 
-            if (candidate[1] == i)
+            if (candidate[1] == ii)
             {
                 candidate[1] = -1;
             }
 
-            if (candidate[2] == i)
+            if (candidate[2] == ii)
             {
                 candidate[2] = -1;
             }
@@ -402,7 +370,6 @@ void Mesh::calcNeighborsVertices()
             {
                 vertVertNeighbors[i].push_back(candidate[2]);
             }
-
         }
     }
 }
@@ -410,31 +377,27 @@ void Mesh::calcNeighborsVertices()
 /// \brief
 void Mesh::upadateAABB()
 {
-    double minx = std::numeric_limits<double>::max();
-    double miny = std::numeric_limits<double>::max();
-    double minz = std::numeric_limits<double>::max();
-    double maxx = -std::numeric_limits<double>::max();
-    double maxy = -std::numeric_limits<double>::max();
-    double maxz = -std::numeric_limits<double>::max();
-
-    for (int i = 0; i < nbrVertices; i++)
-    {
-        minx = std::min(vertices[i][0], minx);
-        miny = std::min(vertices[i][1], miny);
-        minz = std::min(vertices[i][2], minz);
+    core::Vec3d minVector(
+        std::numeric_limits<double>::max(),
+        std::numeric_limits<double>::max(),
+        std::numeric_limits<double>::max());
+    core::Vec3d maxVector(
+        std::numeric_limits<double>::min(),
+        std::numeric_limits<double>::min(),
+        std::numeric_limits<double>::min());
 
-        maxx = std::max(vertices[i][0], maxx);
-        maxy = std::max(vertices[i][1], maxy);
-        maxz = std::max(vertices[i][2], maxz);
+    auto &vertices = this->getVertices();
+    for (size_t i = 0, end = this->getNumberOfVertices(); i < end; i++)
+    {
+        minVector = minVector.array().min(vertices[i].array());
+        maxVector = maxVector.array().max(vertices[i].array());
     }
 
-    aabb.aabbMin[0] = minx - (maxx - minx) * SIMMEDTK_MESH_AABBSKINFACTOR;
-    aabb.aabbMin[1] = miny - (maxy - miny) * SIMMEDTK_MESH_AABBSKINFACTOR;
-    aabb.aabbMin[2] = minz - (maxz - minz) * SIMMEDTK_MESH_AABBSKINFACTOR;
+    core::Vec3d skinOffset = (maxVector-minVector)*SIMMEDTK_MESH_AABBSKINFACTOR;
 
-    aabb.aabbMax[0] = maxx + (maxx - minx) * SIMMEDTK_MESH_AABBSKINFACTOR;
-    aabb.aabbMax[1] = maxy + (maxy - miny) * SIMMEDTK_MESH_AABBSKINFACTOR;
-    aabb.aabbMax[2] = maxz + (maxz - minz) * SIMMEDTK_MESH_AABBSKINFACTOR;
+    Eigen::AlignedBox3d bbox(minVector-skinOffset,
+                             maxVector+skinOffset);
+    this->setBoundingBox(bbox);
 }
 
 /// \brief
@@ -443,7 +406,7 @@ void Mesh::calcEdges()
     Edge edge;
     edges.reserve(SIMMEDTK_MESH_RESERVEDMAXEDGES);
 
-    for (int i = 0; i < nbrVertices; i++)
+    for (size_t i = 0, end = this->getNumberOfVertices(); i < end; i++)
     {
         for (size_t j = 0; j < vertVertNeighbors[i].size(); j++)
         {
@@ -457,29 +420,12 @@ void Mesh::calcEdges()
     }
 }
 
-/// \brief
-void Mesh::translate(float p_offsetX, float p_offsetY, float p_offsetZ)
-{
-
-    for (int i = 0; i < nbrVertices; i++)
-    {
-        vertices[i][0] = vertices[i][0] + p_offsetX;
-        vertices[i][1] = vertices[i][1] + p_offsetY;
-        vertices[i][2] = vertices[i][2] + p_offsetZ;
-
-        origVerts[i][0] = vertices[i][0] + p_offsetX;
-        origVerts[i][1] = vertices[i][1] + p_offsetY;
-        origVerts[i][2] = vertices[i][2] + p_offsetZ;
-    }
-
-    upadateAABB();
-}
-
 /// \brief
 void Mesh::translate(core::Vec3d p_offset)
 {
-
-    for (int i = 0; i < nbrVertices; i++)
+    auto &vertices = this->getVertices();
+    auto &origVerts = this->getOrigVertices();
+    for (size_t i = 0, end = this->getNumberOfVertices(); i < end; ++i)
     {
         vertices[i] = vertices[i] + p_offset;
         origVerts[i] = origVerts[i] + p_offset;
@@ -488,38 +434,30 @@ void Mesh::translate(core::Vec3d p_offset)
     upadateAABB();
 }
 
-/// \brief
 void Mesh::scale(core::Vec3d p_scaleFactors)
 {
-
-    for (int i = 0; i < nbrVertices; i++)
+    auto &vertices = this->getVertices();
+    auto &origVerts = this->getOrigVertices();
+    for (size_t i = 0, end = this->getNumberOfVertices(); i < end; ++i)
     {
-        vertices[i][0] = vertices[i][0] * p_scaleFactors[0];
-        vertices[i][1] = vertices[i][1] * p_scaleFactors[1];
-        vertices[i][2] = vertices[i][2] * p_scaleFactors[2];
-
-        origVerts[i][0] = origVerts[i][0] * p_scaleFactors[0];
-        origVerts[i][1] = origVerts[i][1] * p_scaleFactors[1];
-        origVerts[i][2] = origVerts[i][2] * p_scaleFactors[2];
+        vertices[i].array() *= p_scaleFactors.array();
+        origVerts[i].array() *= p_scaleFactors.array();
     }
 
     upadateAABB();
 }
 
-/// \brief
-void Mesh::rotate(const Matrix33d &p_rot)
+void Mesh::rotate(const Quaterniond &R)
 {
-
-    for (int i = 0; i < nbrVertices; i++)
+    BaseMesh::rotate(R);
+    for (size_t i = 0, end = this->getNumberOfVertices(); i < end; ++i)
     {
-        vertices[i] = p_rot * vertices[i];
-        origVerts[i] = p_rot * origVerts[i];
-        vertNormals[i] = p_rot * vertNormals[i];
+        vertNormals[i] = R * vertNormals[i];
     }
 
     for (int i = 0; i < nbrTriangles; i++)
     {
-        triNormals[i] = p_rot * triNormals[i];
+        triNormals[i] = R * triNormals[i];
     }
 
     calcTriangleTangents();
@@ -529,40 +467,22 @@ void Mesh::rotate(const Matrix33d &p_rot)
 /// \brief
 void Mesh::updateTriangleAABB()
 {
+    auto const &vertices = this->getVertices();
     for (int i = 0; i < nbrTriangles; i++)
     {
-        // min
-        triAABBs[i].aabbMin[0] = std::min(vertices[triangles[i].vert[0]][0], vertices[triangles[i].vert[1]][0]);
-        triAABBs[i].aabbMin[0] = std::min(triAABBs[i].aabbMin[0] ,   vertices[triangles[i].vert[2]][0]);
-
-        triAABBs[i].aabbMin[1] = std::min(vertices[triangles[i].vert[0]][1], vertices[triangles[i].vert[1]][1]);
-        triAABBs[i].aabbMin[1] = std::min(triAABBs[i].aabbMin[1] ,   vertices[triangles[i].vert[2]][1]);
-
-        triAABBs[i].aabbMin[2] = std::min(vertices[triangles[i].vert[0]][2], vertices[triangles[i].vert[1]][2]);
-        triAABBs[i].aabbMin[2] = std::min(triAABBs[i].aabbMin[2] ,   vertices[triangles[i].vert[2]][2]);
-
-        //max
-        triAABBs[i].aabbMax[0] = std::max(vertices[triangles[i].vert[0]][0], vertices[triangles[i].vert[1]][0]);
-        triAABBs[i].aabbMax[0] = std::max(triAABBs[i].aabbMax[0] ,   vertices[triangles[i].vert[2]][0]);
-
-        triAABBs[i].aabbMax[1] = std::max(vertices[triangles[i].vert[0]][1], vertices[triangles[i].vert[1]][1]);
-        triAABBs[i].aabbMax[1] = std::max(triAABBs[i].aabbMax[1] ,   vertices[triangles[i].vert[2]][1]);
-
-        triAABBs[i].aabbMax[2] = std::max(triAABBs[i].aabbMax[2],    vertices[triangles[i].vert[2]][2]);
+        triAABBs[i].setEmpty();
+        triAABBs[i].extend(vertices[triangles[i].vert[0]]);
+        triAABBs[i].extend(vertices[triangles[i].vert[1]]);
+        triAABBs[i].extend(vertices[triangles[i].vert[2]]);
     }
 }
 
 /// \brief
 void Mesh::checkCorrectWinding()
 {
-    int x[3];
-    int p[3];
-
     for (int i = 0; i < nbrTriangles; i++)
     {
-        x[0] = triangles[i].vert[0];
-        x[1] = triangles[i].vert[1];
-        x[2] = triangles[i].vert[2];
+        auto &x = triangles[i].vert;
 
         for (int j = 0; j < nbrTriangles; j++)
         {
@@ -571,9 +491,7 @@ void Mesh::checkCorrectWinding()
                 continue;
             }
 
-            p[0] = triangles[j].vert[0];
-            p[1] = triangles[j].vert[1];
-            p[2] = triangles[j].vert[2];
+            auto p = triangles[j].vert;
 
             if (x[0] == p[0] && x[1] == p[1])
             {
@@ -623,196 +541,17 @@ void Mesh::checkCorrectWinding()
     }
 }
 
-TextureAttachment::TextureAttachment()
-{
-}
-
-bool BaseMesh::isMeshTextured()
-{
-    return isTextureCoordAvailable;
-}
-
-void BaseMesh::assignTexture( int p_textureId )
-{
-    TextureAttachment attachment;
-    attachment.textureId = p_textureId;
-
-    if ( p_textureId > 0 )
-    {
-        this->textureIds.push_back( attachment );
-    }
-}
-void BaseMesh::assignTexture(const std::string& p_referenceName)
-{
-    int textureId;
-    TextureAttachment attachment;
-
-    if (TextureManager::findTextureId(p_referenceName, textureId) == SIMMEDTK_TEXTURE_OK)
-    {
-        attachment.textureId = textureId;
-        this->textureIds.push_back(attachment);
-    }
-}
-LineMesh::LineMesh( int p_nbrVertices ) : BaseMesh()
-{
-    nbrVertices = p_nbrVertices;
-    vertices.reserve( nbrVertices );
-    origVerts.reserve( nbrVertices );
-    edgeAABBs = new AABB[nbrVertices - 1];
-    texCoord = new TexCoord[nbrVertices];
-    edges = new Edge[nbrVertices - 1];
-    nbrEdges = nbrVertices - 1;
-    isTextureCoordAvailable = false;
-    createAutoEdges();
-}
-LineMesh::LineMesh( int p_nbrVertices, bool autoEdge ) : BaseMesh()
-{
-    nbrVertices = p_nbrVertices;
-    vertices.reserve( nbrVertices );
-    origVerts.reserve( nbrVertices );
-    texCoord = new TexCoord[nbrVertices];
-
-    /// Edge AABB should be assigned by the instance
-    edgeAABBs = nullptr;
-
-    /// Edges should be assigned by the instance
-    edges = nullptr;
-
-    /// Number of edges should be assigned by the instance
-    nbrEdges = 0;
-
-    isTextureCoordAvailable = false;
-
-    if ( autoEdge )
-    {
-        createAutoEdges();
-    }
-}
-void LineMesh::createAutoEdges()
-{
-    for ( int i = 0; i < nbrEdges; i++ )
-    {
-        edges[i].vert[0] = i;
-        edges[i].vert[1] = i + 1;
-    }
-}
-void LineMesh::updateAABB()
-{
-    AABB tempAABB;
-    core::Vec3d minOffset( -2.0, -2.0, -2.0 );
-    core::Vec3d maxOffset( 1.0, 1.0, 1.0 );
-    core::Vec3d minEdgeOffset( -0.1, -0.1, -0.1 );
-    core::Vec3d maxEdgeOffset( 0.1, 0.1, 0.1 );
-
-    tempAABB.aabbMin[0] = std::numeric_limits<double>::max();
-    tempAABB.aabbMin[1] = std::numeric_limits<double>::max();
-    tempAABB.aabbMin[2] = std::numeric_limits<double>::max();
-
-    tempAABB.aabbMax[0] = -std::numeric_limits<double>::max();
-    tempAABB.aabbMax[1] = -std::numeric_limits<double>::max();
-    tempAABB.aabbMax[2] = -std::numeric_limits<double>::max();
-
-    for ( int i = 0; i < nbrEdges; i++ )
-    {
-        ///min
-        edgeAABBs[i].aabbMin[0] = std::min( vertices[edges[i].vert[0]][0], vertices[edges[i].vert[1]][0] );
-        edgeAABBs[i].aabbMin[1] = std::min( vertices[edges[i].vert[0]][1], vertices[edges[i].vert[1]][1] );
-        edgeAABBs[i].aabbMin[2] = std::min( vertices[edges[i].vert[0]][2], vertices[edges[i].vert[1]][2] );
-        edgeAABBs[i].aabbMin += minEdgeOffset;
-        tempAABB.aabbMin[0] = std::min( tempAABB.aabbMin[0], edgeAABBs[i].aabbMin[0] );
-        tempAABB.aabbMin[1] = std::min( tempAABB.aabbMin[1], edgeAABBs[i].aabbMin[1] );
-        tempAABB.aabbMin[2] = std::min( tempAABB.aabbMin[2], edgeAABBs[i].aabbMin[2] );
-
-        ///max
-        edgeAABBs[i].aabbMax[0] = std::max( vertices[edges[i].vert[0]][0], vertices[edges[i].vert[1]][0] );
-        edgeAABBs[i].aabbMax[1] = std::max( vertices[edges[i].vert[0]][1], vertices[edges[i].vert[1]][1] );
-        edgeAABBs[i].aabbMax[2] = std::max( vertices[edges[i].vert[0]][2], vertices[edges[i].vert[1]][2] );
-        edgeAABBs[i].aabbMax += maxEdgeOffset;
-        tempAABB.aabbMax[0] = std::max( tempAABB.aabbMax[0], edgeAABBs[i].aabbMax[0] );
-        tempAABB.aabbMax[1] = std::max( tempAABB.aabbMax[1], edgeAABBs[i].aabbMax[1] );
-        tempAABB.aabbMax[2] = std::max( tempAABB.aabbMax[2], edgeAABBs[i].aabbMax[2] );
-    }
-
-    tempAABB.aabbMin += minOffset;
-    tempAABB.aabbMax += maxOffset;
-    aabb = tempAABB;
-}
-void LineMesh::translate( float p_offsetX, float p_offsetY, float p_offsetZ )
-{
-
-    for ( int i = 0; i < nbrVertices; i++ )
-    {
-        vertices[i][0] = vertices[i][0] + p_offsetX;
-        vertices[i][1] = vertices[i][1] + p_offsetY;
-        vertices[i][2] = vertices[i][2] + p_offsetZ;
-    }
-
-    updateAABB();
-}
-void LineMesh::translate( core::Vec3d p_offset )
-{
-
-    for ( int i = 0; i < nbrVertices; i++ )
-    {
-        vertices[i] = vertices[i] + p_offset;
-        origVerts[i] = origVerts[i] + p_offset;
-    }
-
-    updateAABB();
-}
-void LineMesh::rotate( Matrix33d p_rot )
-{
-
-    for ( int i = 0; i < nbrVertices; i++ )
-    {
-        vertices[i] = p_rot * vertices[i];
-        origVerts[i] = p_rot * origVerts[i];
-    }
-
-    updateAABB();
-}
-void LineMesh::scale( core::Vec3d p_scaleFactors )
-{
-
-    for ( int i = 0; i < nbrVertices; i++ )
-    {
-        vertices[i][0] = vertices[i][0] * p_scaleFactors[0];
-        vertices[i][1] = vertices[i][1] * p_scaleFactors[1];
-        vertices[i][2] = vertices[i][2] * p_scaleFactors[2];
-
-        origVerts[i][0] = origVerts[i][0] * p_scaleFactors[0];
-        origVerts[i][1] = origVerts[i][1] * p_scaleFactors[1];
-        origVerts[i][2] = origVerts[i][2] * p_scaleFactors[2];
-    }
-
-    updateAABB();
-}
-
-bool LineMesh::isMeshTextured()
-{
-    return isTextureCoordAvailable;
-}
-
-int Mesh::getNumTriangles() const
-{
-    return this->nbrTriangles;
-}
-
-int Mesh::getNumEdges() const
-{
-    return this->edges.size();
-}
-
 void Mesh::updateSurfaceMeshFromVegaFormat(std::shared_ptr<ObjMesh> vegaSurfaceMesh)
 {
     Vec3d p;
     //copy the vertex co-ordinates
-    for(int i=0; i<this->nbrVertices ; i++)
+    auto &vertices = this->getVertices();
+    for (size_t i = 0, end = this->getNumberOfVertices(); i < end; ++i)
     {
        p = vegaSurfaceMesh->getPosition(i);
-       this->vertices[i][0] = p[0];
-       this->vertices[i][1] = p[1];
-       this->vertices[i][2] = p[2];
+       vertices[i][0] = p[0];
+       vertices[i][1] = p[1];
+       vertices[i][2] = p[2];
     }
 }
 
@@ -823,61 +562,51 @@ bool Mesh::importSurfaceMeshFromVegaFormat(std::shared_ptr<ObjMesh> vegaSurfaceM
 
     if(!vegaSurfaceMesh->isTriangularMesh())
     {
-        if (this->log != nullptr)
-        {
-            this->log->addError("Error : SimMedTK supports only triangular surface mesh. Vega mesh is not a triangle mesh!");
+//         if (this->log != nullptr)
+//         {
+//             this->log->addError("Error : SimMedTK supports only triangular surface mesh. Vega mesh is not a triangle mesh!");
             return false;
-        }
+//         }
     }
 
-    int i, threeI;
 
     // temporary arrays
     int numVertices(0);
-    double* vertices;
     int numTriangles(0);
-    int* triangles;
-    //Int * numGroups;
-	//Int ** triangleGroups;
 
-    vertices = nullptr;
-    triangles = nullptr;
-    vegaSurfaceMesh->exportGeometry(&numVertices, &vertices, &numTriangles , &triangles, nullptr, nullptr);
+    double* vegaVertices = nullptr;
+    int* vegaTriangles = nullptr;
+    vegaSurfaceMesh->exportGeometry(&numVertices, &vegaVertices, &numTriangles , &vegaTriangles, nullptr, nullptr);
 
-    this->nbrVertices = numVertices;
     this->nbrTriangles = numTriangles;
 
     initVertexArrays(numVertices);
     initTriangleArrays(numTriangles);
 
-    /*delete this->triangles;
-    this->triangles = new Triangle[this->nbrTriangles];*/
-
     //copy the triangle connectivity information
-    for(i=0; i<this->nbrTriangles ; i++)
+    for(int i = 0, fastIndex = 0; i < this->nbrTriangles; ++i, fastIndex+=3)
     {
-        threeI = 3*i;
-        this->triangles[i].vert[0] = triangles[threeI+0];
-        this->triangles[i].vert[1] = triangles[threeI+1];
-        this->triangles[i].vert[2] = triangles[threeI+2];
+        this->triangles[i].vert[0] = vegaTriangles[fastIndex+0];
+        this->triangles[i].vert[1] = vegaTriangles[fastIndex+1];
+        this->triangles[i].vert[2] = vegaTriangles[fastIndex+2];
     }
 
-    //this->vertices.resize(this->nbrVertices);
-    //copy the vertex co-ordinates
-    for(i=0; i<this->nbrVertices ; i++)
+    auto &vertexArray = this->getVertices();
+    for(int i = 0, fastIndex = 0; i < numVertices; ++i, fastIndex+=3)
     {
-        this->vertices[i][0] = vertices[3 * i + 0];
-        this->vertices[i][1] = vertices[3 * i + 1];
-        this->vertices[i][2] = vertices[3 * i + 2];
+        vertexArray[i][0] = vegaVertices[fastIndex+0];
+        vertexArray[i][1] = vegaVertices[fastIndex+1];
+        vertexArray[i][2] = vegaVertices[fastIndex+2];
     }
 
-    if(perProcessingStage){
+    if(perProcessingStage)
+    {
         updateOriginalVertsWithCurrent();
     }
 
     //deallocate temporary arrays
-    delete [] triangles;
-    delete [] vertices;
+    delete [] vegaTriangles;
+    delete [] vegaVertices;
 
     return 1;
 
diff --git a/Mesh/Mesh.h b/Mesh/Mesh.h
index ec0ce1d2b7af2aa18563b7ccc7dc78ca3b04d2a4..a96c62fbfc8a23f68964a548f8a06fb854494fdf 100644
--- a/Mesh/Mesh.h
+++ b/Mesh/Mesh.h
@@ -48,79 +48,6 @@ struct Triangle;
 struct Tetrahedra;
 struct Edge;
 
-
-/// \brief !!
-struct TextureAttachment
-{
-    TextureAttachment();
-    int textureId;
-};
-
-/// \brief base class for the mesh
-class BaseMesh: public CoreClass
-{
-public:
-    /// \brief designates what purpose/scenario the mesh is used for
-    enum class MeshType
-    {
-        Deformable,
-        DeformableCutable,
-        RigidCutable,
-        Rigid
-    };
-
-    /// \brief designates input mesh file type
-    enum class MeshFileType
-    {
-        None,
-        Obj,
-        ThreeDS,
-        Volume,
-    };
-
-    /// \brief constructor
-    BaseMesh();
-
-    /// \brief query if the mesh has textures available for rendering
-    bool isMeshTextured();
-
-    /// \brief assign the texture
-    void assignTexture(int p_textureId);
-
-    /// \brief assign the texture
-    void assignTexture(const std::string& p_referenceName);
-
-    /// \brief update the original texture vertices with the current
-    void updateOriginalVertsWithCurrent();
-
-    const std::vector<core::Vec3d> &getVertices() const
-    {
-        return this->vertices;
-    }
-
-    std::vector<core::Vec3d> &getVertices()
-    {
-        return this->vertices;
-    }
-
-    int getNumVertices()
-    {
-        return nbrVertices;
-    }
-
-public:
-    CollisionGroup collisionGroup; ///< !!
-    GLint renderingID; ///< !!
-    std::shared_ptr<ErrorLog> log; ///< record the log
-    std::vector<core::Vec3d> vertices; ///< vertices co-ordinate data at time t
-    std::vector<core::Vec3d> origVerts; ///< vertices co-ordinate data at time t=0
-    int  nbrVertices; ///< number of vertices
-    AABB aabb; ///< Axis aligned bounding box
-    bool isTextureCoordAvailable; ///< true if the texture co-ordinate is available
-    TexCoord *texCoord; ///< texture co-ordinates
-    std::vector<TextureAttachment> textureIds; ///< !!
-};
-
 /// \brief: this is a generic Mesh class from which surface and volume meshes are inherited
 /// Note: this class cannot exist on its own
 class Mesh: public Core::BaseMesh
@@ -159,20 +86,30 @@ public:
     /// \brief update the normals of vertices after they moved
     void updateVertexNormals();
 
-    /// \brief update AABB after the mesh moved
+    /// \brief update Eigen::AlignedBox3d after the mesh moved
     void upadateAABB();
 
-    /// \brief update AABB of each triangle after mesh moved
+    /// \brief update Eigen::AlignedBox3d of each triangle after mesh moved
     void updateTriangleAABB();
 
     /// \brief compute triangle tangents
     void calcTriangleTangents();
 
     /// \brief compute the tangent give the three vertices
-    void calculateTangent(core::Vec3d& p1, core::Vec3d& p2, core::Vec3d& p3, TexCoord& t1, TexCoord& t2, TexCoord& t3, core::Vec3d& t);
+    core::Vec3d calculateTangent(const core::Vec3d& p1,
+                                 const core::Vec3d& p2,
+                                 const core::Vec3d& p3,
+                                 const core::Vec2f& t1,
+                                 const core::Vec2f& t2,
+                                 const core::Vec2f& t3);
 
     /// \brief !!
-    void calculateTangent_test(core::Vec3d& p1, core::Vec3d& p2, core::Vec3d& p3, TexCoord& t1, TexCoord& t2, TexCoord& t3, core::Vec3d& t);
+    core::Vec3d calculateTangent_test(const core::Vec3d& p1,
+                                      const core::Vec3d& p2,
+                                      const core::Vec3d& p3,
+                                      const core::Vec2f& t1,
+                                      const core::Vec2f& t2,
+                                      const core::Vec2f& t3);
 
     /// \brief find the neighbors of all vertices of mesh
     void calcNeighborsVertices();
@@ -180,9 +117,6 @@ public:
     /// \brief find all the edges of the mesh
     void calcEdges();
 
-    /// \brief translate the mesh
-    void translate(float, float, float);
-
     /// \brief translate the mesh
     void translate(core::Vec3d p_offset);
 
@@ -190,7 +124,7 @@ public:
     void scale(core::Vec3d p_scaleFactors);
 
     /// \brief rotate the mesh
-    void rotate(const Matrix33d &p_rot);
+    void rotate(const Quaterniond &p_rot);
 
     /// \brief check if there is a consistent orientation of triangle vertices
     /// across the entire surface mesh
@@ -212,10 +146,16 @@ public:
     void updateSurfaceMeshFromVegaFormat(std::shared_ptr<ObjMesh> vegaSurfaceMesh);
 
     /// \brief get number of triangles
-    int getNumTriangles()  const;
+    int getNumTriangles()  const
+    {
+        return this->nbrTriangles;
+    }
 
     /// \brief get number of edges
-    int getNumEdges()  const;
+    size_t getNumEdges() const
+    {
+        return this->edges.size();
+    }
 
 public:
     int  nbrTriangles; ///< number of triangles
@@ -234,7 +174,7 @@ public:
     ///AABBB of the mesh.
     ///This value is allocated and computed by only collision detection module
     ///Therefore it is initially nullptr
-    std::vector<AABB> triAABBs;
+    std::vector<Eigen::AlignedBox3d> triAABBs;
 
     MeshType meshType; ///< type of mesh (rigid, deformable etc.)
     MeshFileType meshFileType; ///< type of input mesh
@@ -264,53 +204,4 @@ struct Edge
     std::array<unsigned int,2> vert;
 };
 
-/// \brief !!
-class LineMesh: public BaseMesh
-{
-public:
-    /// \brief destructor
-    ~LineMesh()
-    {
-        delete[]edgeAABBs;
-        delete[]texCoord;
-        delete[]edges;
-    }
-
-    /// \brief constructor
-    LineMesh(int p_nbrVertices);
-
-    /// \brief constructor
-    LineMesh(int p_nbrVertices, bool autoEdge);
-
-    /// \brief !!
-    void createAutoEdges();
-
-    /// \brief !!
-    virtual void createCustomEdges() {};
-
-    /// \brief updat AABB when the mesh moves
-    void updateAABB();
-
-    /// \brief translate the vertices of mesh
-    void translate(float p_offsetX, float p_offsetY, float p_offsetZ);
-
-    /// \brief translate the vertices of mesh
-    void translate(core::Vec3d p_offset);
-
-    /// \brief scale the mesh
-    void scale(core::Vec3d p_scaleFactors);
-
-    /// \brief rotate the mesh
-    void rotate(Matrix33d p_rot);
-
-    /// \brief query if the mesh is textured
-    bool isMeshTextured();
-
-public:
-    AABB *edgeAABBs;///< AABBs for the edges in the mesh
-    Edge *edges;///< edges of the line mesh
-    int nbrEdges;///< number of edges of the line mesh
-
-};
-
 #endif
diff --git a/Mesh/SurfaceMesh.cpp b/Mesh/SurfaceMesh.cpp
index 9ff9d84a2e9d161e7b7755f6a4f1509a89a3e732..3782cc2be86924e557249403d6ccc543c9cf0369 100644
--- a/Mesh/SurfaceMesh.cpp
+++ b/Mesh/SurfaceMesh.cpp
@@ -198,29 +198,21 @@ bool SurfaceMesh::LoadMeshAssimp(const std::string& fileName)
     //extract the information from the aiScene's mesh objects
     aiMesh *mesh = scene->mMeshes[0]; //Guarenteed to have atleast one mesh
 
-    if (mesh->HasTextureCoords(0))
-    {
-        this->isTextureCoordAvailable = 1;
-    }
-    else
-    {
-        this->isTextureCoordAvailable = 0;
-    }
-
     initVertexArrays(mesh->mNumVertices);
     initTriangleArrays(mesh->mNumFaces);
 
     //Get indexed vertex data
+    auto &vertices = this->getVertices();
     for (size_t i = 0; i < mesh->mNumVertices; i++)
     {
-        this->vertices[i] = core::Vec3d(mesh->mVertices[i][0],
-                                    mesh->mVertices[i][1],
-                                    mesh->mVertices[i][2]);
+        vertices[i] = core::Vec3d(mesh->mVertices[i][0],
+                                  mesh->mVertices[i][1],
+                                  mesh->mVertices[i][2]);
     }
-    this->origVerts = this->vertices;
+    this->updateOriginalVertsWithCurrent();
 
     //Get indexed texture coordinate data
-    if (isTextureCoordAvailable)
+    if (mesh->HasTextureCoords(0))
     {
         //Assimp supports 3D texture coords, but we only support 2D
         if (mesh->mNumUVComponents[0] != 2)
@@ -230,15 +222,14 @@ bool SurfaceMesh::LoadMeshAssimp(const std::string& fileName)
                 log_SF->addError("Error: Error loading mesh, non-two dimensional texture coordinate found.");
             }
 
-            this->isTextureCoordAvailable = 0;
             return false;
         }
 
         //Extract the texture data
         for (unsigned int i = 0; i < mesh->mNumVertices; i++)
         {
-            this->texCoord[i].u = mesh->mTextureCoords[0][i][0];
-            this->texCoord[i].v = mesh->mTextureCoords[0][i][1];
+            this->addTextureCoordinate(mesh->mTextureCoords[0][i][0],
+                                       mesh->mTextureCoords[0][i][1]);
         }
     }
 
@@ -287,6 +278,7 @@ bool SurfaceMesh::Load3dsMesh(const std::string& fileName)
         fread(&l_chunk_id, 2, 1, l_file);  //Read the chunk header
         fread(&l_chunk_lenght, 4, 1, l_file);  //Read the lenght of the chunk
 
+        auto vertexArray = this->getVertices();
         switch (l_chunk_id)
         {
         //----------------- MAIN3DS -----------------
@@ -340,18 +332,15 @@ bool SurfaceMesh::Load3dsMesh(const std::string& fileName)
         //-------------------------------------------
         case 0x4110:
             fread(&l_qty, sizeof(unsigned short), 1, l_file);
-            this->nbrVertices = l_qty;
-            this->vertices.reserve(l_qty);
-            this->origVerts.reserve(l_qty);
+            vertexArray.reserve(l_qty);
             this->vertNormals = new core::Vec3d[l_qty];
             this->vertTangents = new core::Vec3d[l_qty];
-            this->texCoord = new TexCoord[l_qty];
 
-            for (int fpt = 0; fpt < this->nbrVertices; fpt++)
+            for (int fpt = 0; fpt < l_qty; fpt++)
             {
                 float fTemp[3];
                 fread(fTemp, sizeof(float), 3, l_file);
-                this->vertices.emplace_back(fTemp[0], fTemp[1], fTemp[2]);
+                vertexArray.emplace_back(fTemp[0], fTemp[1], fTemp[2]);
             }
 
             break;
@@ -399,12 +388,9 @@ bool SurfaceMesh::Load3dsMesh(const std::string& fileName)
             {
                 float fTemp[2];
                 fread(fTemp, sizeof(float), 2, l_file);
-                this->texCoord[tpt].u = fTemp[0];
-                this->texCoord[tpt].v = fTemp[1];
+                this->addTextureCoordinate(fTemp[0],fTemp[1]);
             }
 
-            isTextureCoordAvailable = true;
-
             break;
 
         //----------- Skip unknow chunks ------------
@@ -416,7 +402,7 @@ bool SurfaceMesh::Load3dsMesh(const std::string& fileName)
             fseek(l_file, l_chunk_lenght - 6, SEEK_CUR);
         }
     }
-    this->origVerts = this->vertices;
+    this->updateOriginalVertsWithCurrent();
     fclose(l_file);  // Closes the file stream
 
     return 1; // Returns ok
@@ -432,7 +418,7 @@ void SurfaceMesh::printPrimitiveDetails()
 {
     std::cout << "----------------------------\n";
     std::cout << "Mesh Info for   : " << this->getName() <<"\n\t";
-    std::cout << "Num. vertices   : " << this->getNumVertices() <<"\n\t";
+    std::cout << "Num. vertices   : " << this->getNumberOfVertices() <<"\n\t";
     std::cout << "Num. triangles  : " << this->getNumTriangles() << "\n\t";
     std::cout << "Num. edges      : " << this->getNumEdges() << "\n\t";
     std::cout << "Is mesh textured: " << this->isMeshTextured() << "\n";
diff --git a/Mesh/UnitTests/VegaVolumetricMeshSpec.cpp.in b/Mesh/UnitTests/VegaVolumetricMeshSpec.cpp.in
index 996765010ba43cd2d89e90efee69c2ff02b3bb8b..3645b2c8ca31694b84f1fa96d40f7163445d0820 100644
--- a/Mesh/UnitTests/VegaVolumetricMeshSpec.cpp.in
+++ b/Mesh/UnitTests/VegaVolumetricMeshSpec.cpp.in
@@ -62,9 +62,10 @@ go_bandit([](){
             std::shared_ptr<SurfaceMesh> surfaceMesh = std::make_shared<SurfaceMesh>();
 
             // These vertices coincide with vertices on the volume mesh. So the weights take a special form.
-            surfaceMesh->vertices.emplace_back(-2.44627, -0.903874999999999, -1.711465);
-            surfaceMesh->vertices.emplace_back(-2.008655, -0.762779999999999, -1.63081);
-            surfaceMesh->vertices.emplace_back(-2.248035, -0.599385, -1.41836);
+            auto &vertices = surfaceMesh->getVertices();
+            vertices.emplace_back(-2.44627, -0.903874999999999, -1.711465);
+            vertices.emplace_back(-2.008655, -0.762779999999999, -1.63081);
+            vertices.emplace_back(-2.248035, -0.599385, -1.41836);
 
             vegaMesh->attachSurfaceMesh(surfaceMesh,2.0);
 
diff --git a/Mesh/VolumeMesh.cpp b/Mesh/VolumeMesh.cpp
index 4116a43a4182f97f59f2024c9000d5cbe74d1f3e..c3e02f0813057bdf164f9065cf49a7b30218bfb3 100644
--- a/Mesh/VolumeMesh.cpp
+++ b/Mesh/VolumeMesh.cpp
@@ -253,7 +253,7 @@ bool VolumeMesh::getSurface(const std::string& fileName)
         triangles[i].vert[2] = temp[triangles[i].vert[2] - 1];
     }
 
-    surfaceNodeIndex.resize(nbrVertices);
+    surfaceNodeIndex.resize(this->getNumberOfVertices());
 
     count = 0;
 
@@ -304,10 +304,8 @@ bool VolumeMesh::readBC(const std::string& fileName)
 /// \brief copies the updated co-ordinates of the surface vertices only
 void VolumeMesh::copySurface()
 {
-
-    int i;
-
-    for (i = 0; i < nbrVertices; i++)
+    auto &vertices = this->getVertices();
+    for (size_t i = 0, end = this->getNumberOfVertices(); i < end; ++i)
     {
         vertices[i][0] = nodes[surfaceNodeIndex[i]][0];
         vertices[i][1] = nodes[surfaceNodeIndex[i]][1];
@@ -321,9 +319,9 @@ void VolumeMesh::copySurface()
 /// \brief copies the updated co-ordinates of the surface vertices only
 void VolumeMesh::initSurface()
 {
-    int i;
-
-    for (i = 0; i < nbrVertices; i++)
+    auto &vertices = this->getVertices();
+    auto &origVerts = this->getOrigVertices();
+    for (size_t i = 0, end = this->getNumberOfVertices(); i < end; ++i)
     {
         vertices[i][0] = nodes[surfaceNodeIndex[i]][0];
         vertices[i][1] = nodes[surfaceNodeIndex[i]][1];
@@ -355,7 +353,7 @@ void VolumeMesh::updateVolumeMeshFromVegaFormat(const std::shared_ptr<const Volu
 
 void VolumeMesh::importVolumeMeshFromVegaFormat(const std::shared_ptr<const VolumetricMesh> vega3dMesh, const bool preProcessingStage)
 {
-    int i, threeI, j;
+    int threeI, j;
 
     //temporary arrays
     int numNodes(0);
@@ -371,7 +369,7 @@ void VolumeMesh::importVolumeMeshFromVegaFormat(const std::shared_ptr<const Volu
 
     this->tetra.resize(this->nbrTetra);
     //copy the element connectivity information
-    for(i=0; i<this->nbrTetra ; i++)
+    for(int i=0; i<this->nbrTetra ; i++)
     {
         threeI = numVertsPerEle * i;
         for(j=0; j<numVertsPerEle ; j++)
@@ -382,7 +380,7 @@ void VolumeMesh::importVolumeMeshFromVegaFormat(const std::shared_ptr<const Volu
 
 	this->nodes.resize(this->nbrNodes);
     //copy the nodal co-ordinates
-    for(i=0; i<this->nbrVertices ; i++)
+    for(size_t i=0, end = this->getNumberOfVertices(); i < end; i++)
     {
         threeI = 3*i;
         this->nodes[i][0] = nodes[threeI];
diff --git a/RenderDelegates/StylusRenderDelegate.cpp b/RenderDelegates/StylusRenderDelegate.cpp
index 51920a50b3484dcaee602ffa66f708ad5a324124..a5499886f75ad9bb4230ce48da3d8ffb467d7e75 100644
--- a/RenderDelegates/StylusRenderDelegate.cpp
+++ b/RenderDelegates/StylusRenderDelegate.cpp
@@ -49,7 +49,7 @@ void StylusRenderDelegate::initDraw()
     glNewList(newList + listCounter, GL_COMPILE);
     iter.node->data->mesh->draw();
     glEndList();
-    iter.node->data->mesh->renderingID = (newList + listCounter);
+    iter.node->data->mesh->setRenderingId(newList + listCounter);
     listCounter++;
     iter++;
     }
@@ -77,7 +77,7 @@ void StylusRenderDelegate::draw() const
       }
 
     glMultMatrixd(viewMatrix.data());
-    glCallList(iter.node->data->mesh->renderingID);
+    glCallList(iter.node->data->mesh->getRenderingId());
     glPopMatrix();
     iter++;
 
@@ -95,7 +95,7 @@ void StylusRenderDelegate::draw() const
         }
 
       glMultMatrixd(viewMatrix.data());
-      glCallList(iter.node->data->mesh->renderingID);
+      glCallList(iter.node->data->mesh->getRenderingId());
       glPopMatrix();
       iter++;
       }
diff --git a/Simulators/DummySimulator.cpp b/Simulators/DummySimulator.cpp
index 297ef3e8309e089580122da5acb9880db0fbd1c3..d525425488fc0dca2c3cbea554860028cdb068bd 100644
--- a/Simulators/DummySimulator.cpp
+++ b/Simulators/DummySimulator.cpp
@@ -54,9 +54,9 @@ void DummySimulator::initCustom()
                 }
                 std::shared_ptr<Mesh> mesh = model->getMesh();
 
-                object->getLocalVertices().reserve( mesh->nbrVertices );
+                object->getLocalVertices().reserve( mesh->getNumberOfVertices() );
                 // WARNING:  Copy!!?
-                object->getLocalVertices() = mesh->vertices;
+                object->getLocalVertices() = mesh->getVertices();
                 object->getFlags().isSimulatorInit = true;
                 break;
             }
@@ -85,7 +85,7 @@ void DummySimulator::run()
             }
             std::shared_ptr<Mesh> mesh = model->getMesh();
 
-            for ( int vertIndex = 0; vertIndex < mesh->nbrVertices; vertIndex++ )
+            for ( int vertIndex = 0; vertIndex < mesh->getNumberOfVertices(); vertIndex++ )
             {
                 staticSceneObject->getLocalVertices()[vertIndex][1] = staticSceneObject->getLocalVertices()[vertIndex][1] + 0.000001;
             }
@@ -115,7 +115,7 @@ void DummySimulator::syncBuffers()
             }
             std::shared_ptr<Mesh> mesh = model->getMesh();
             // WARNING: Copy??!
-            mesh->vertices = staticSceneObject->getLocalVertices();
+            mesh->getVertices() = staticSceneObject->getLocalVertices();
         }
     }
 }
diff --git a/Simulators/MyStylus.cpp b/Simulators/MyStylus.cpp
index d277f5f28b49de289e1fdaf9756ac2a7527c2e32..b610155f43e05a93bbba1504fa0e7a0b94a94ee4 100644
--- a/Simulators/MyStylus.cpp
+++ b/Simulators/MyStylus.cpp
@@ -38,21 +38,21 @@ MyStylus::MyStylus(const std::string& p_shaft, const std::string& p_lower, const
     angle = 0;
     Matrix33d rot = Eigen::AngleAxisd(-M_PI_2, core::Vec3d::UnitX()).matrix();
 
-    SurfaceMesh *mesh = new SurfaceMesh(BaseMesh::MeshType::Rigid, nullptr);
-    mesh->loadMesh(p_shaft, BaseMesh::MeshFileType::ThreeDS);
-    mesh->assignTexture("hookCautery");
+    SurfaceMesh *mesh = new SurfaceMesh(Core::BaseMesh::MeshType::Rigid, nullptr);
+    mesh->loadMesh(p_shaft, Core::BaseMesh::MeshFileType::ThreeDS);
+    mesh->assignTexture("","hookCautery");
     mesh->scale(core::Vec3d(0.2, 0.2, 0.2));
     mesh->rotate(rot);
 
-    SurfaceMesh *lowerMesh = new SurfaceMesh(BaseMesh::MeshType::Rigid, nullptr);
-    lowerMesh->loadMesh(p_lower, BaseMesh::MeshFileType::ThreeDS);
-    lowerMesh->assignTexture("metal");
+    SurfaceMesh *lowerMesh = new SurfaceMesh(Core::BaseMesh::MeshType::Rigid, nullptr);
+    lowerMesh->loadMesh(p_lower, Core::BaseMesh::MeshFileType::ThreeDS);
+    lowerMesh->assignTexture("","metal");
     lowerMesh->scale(core::Vec3d(0.2, 0.2, 0.2));
     lowerMesh->rotate(rot);
 
-    SurfaceMesh *upperMesh = new SurfaceMesh(BaseMesh::MeshType::Rigid, nullptr);
-    upperMesh->loadMesh(p_upper, BaseMesh::MeshFileType::ThreeDS);
-    upperMesh->assignTexture("metal");
+    SurfaceMesh *upperMesh = new SurfaceMesh(Core::BaseMesh::MeshType::Rigid, nullptr);
+    upperMesh->loadMesh(p_upper, Core::BaseMesh::MeshFileType::ThreeDS);
+    upperMesh->assignTexture("","metal");
     upperMesh->scale(core::Vec3d(0.2, 0.2, 0.2));
     upperMesh->rotate(rot);
 
@@ -186,9 +186,9 @@ HookCautery::HookCautery(const std::string& p_pivot)
 {
     Matrix33d rot = Eigen::AngleAxisd(-M_PI_2, core::Vec3d::UnitX()).matrix();
 
-    SurfaceMesh *mesh = new SurfaceMesh(BaseMesh::MeshType::Rigid, nullptr);
-    mesh->loadMesh(p_pivot, BaseMesh::MeshFileType::ThreeDS);
-    mesh->assignTexture("metal");
+    SurfaceMesh *mesh = new SurfaceMesh(Core::BaseMesh::MeshType::Rigid, nullptr);
+    mesh->loadMesh(p_pivot, Core::BaseMesh::MeshFileType::ThreeDS);
+    mesh->assignTexture("","metal");
     mesh->scale(core::Vec3d(0.2, 0.2, 0.2));
     mesh->rotate(rot);
 
diff --git a/Simulators/PBDObjectSimulator.cpp b/Simulators/PBDObjectSimulator.cpp
index 79a391052f1bb09d37ccc814a107423262f40178..8a166ffd4f0223257472a18f7df5b760dcc779fd 100644
--- a/Simulators/PBDObjectSimulator.cpp
+++ b/Simulators/PBDObjectSimulator.cpp
@@ -48,10 +48,10 @@ void PBDObjectSimulator::initObject( std::shared_ptr<PBDSurfaceSceneObject> p_ob
         return;
     }
 
-    p_object->getLocalVertices().reserve( p_object->mesh->nbrVertices );
+    p_object->getLocalVertices().reserve( p_object->mesh->getNumberOfVertices() );
 
     // WARNING: Copying vertices??!!
-    p_object->getLocalVertices() = p_object->mesh->vertices;
+    p_object->getLocalVertices() = p_object->mesh->getVertices();
     p_object->getFlags().isSimulatorInit = true;
 }
 void PBDObjectSimulator::initCustom()
diff --git a/Simulators/PBDSceneObject.cpp b/Simulators/PBDSceneObject.cpp
index 283acfbe7db9e637777db4479f554631dd841a5d..3cdeee488495d6af2928e8e661b83cf599d15e3f 100644
--- a/Simulators/PBDSceneObject.cpp
+++ b/Simulators/PBDSceneObject.cpp
@@ -27,10 +27,11 @@ void PBDSurfaceSceneObject::findFixedMassWrtSphere(core::Vec3d p_center, float p
 {
     float dist = 0;
 
-    for (int i = 0; i < mesh->nbrVertices; i++)
+    auto &vertices = mesh->getVertices();
+    for (int i = 0; i < mesh->getNumberOfVertices(); i++)
     {
 
-        dist = (p_center - mesh->vertices[i]).norm();
+        dist = (p_center - vertices[i]).norm();
 
         if (dist < p_radius)
         {
@@ -89,7 +90,8 @@ void PBDSurfaceSceneObject::initMeshStructure()
 void PBDSurfaceSceneObject::InitSurfaceObject()
 {
     //surface mesh
-    nbrMass = mesh->nbrVertices;
+    nbrMass = mesh->getNumberOfVertices();
+    auto &vertices = mesh->getVertices();
 
     P = new core::Vec3d[nbrMass];
     V = new core::Vec3d[nbrMass];
@@ -103,7 +105,7 @@ void PBDSurfaceSceneObject::InitSurfaceObject()
 
     for ( int i = 0; i < nbrMass; i++ )
     {
-        P[i] = mesh->vertices[i];
+        P[i] = vertices[i];
     }
 
     nbrSpr = mesh->edges.size();
@@ -111,7 +113,7 @@ void PBDSurfaceSceneObject::InitSurfaceObject()
 
     for ( int i = 0; i < nbrSpr; i++ )
     {
-        L0[i] = ( mesh->vertices[mesh->edges[i].vert[0]] - mesh->vertices[mesh->edges[i].vert[1]] ).norm();
+        L0[i] = ( vertices[mesh->edges[i].vert[0]] - vertices[mesh->edges[i].vert[1]] ).norm();
     }
 
     mesh->allocateAABBTris();
@@ -145,7 +147,7 @@ PBDSurfaceSceneObject::~PBDSurfaceSceneObject()
 }
 void PBDSurfaceSceneObject::findFixedCorners()
 {
-
+    auto &vertices = mesh->getVertices();
     nbrFixedMass = 2;
     listFixedMass = new int[nbrFixedMass];
     core::Vec3d corner[2];
@@ -160,9 +162,9 @@ void PBDSurfaceSceneObject::findFixedCorners()
     {
         minmin = std::numeric_limits<float>::max();
 
-        for ( j = 0; j < mesh->nbrVertices; j++ )
+        for ( j = 0; j < mesh->getNumberOfVertices(); j++ )
         {
-            dist = ( corner[i] - mesh->vertices[j] ).norm();
+            dist = ( corner[i] - vertices[j] ).norm();
 
             if ( dist < minmin )
             {
diff --git a/Simulators/VegaFemSceneObject.h b/Simulators/VegaFemSceneObject.h
index c114244b0ba10993a9c4b092d6c7997d03d3e964..2c2514fa5bf5b5801629e59ed859a267758b6071 100644
--- a/Simulators/VegaFemSceneObject.h
+++ b/Simulators/VegaFemSceneObject.h
@@ -227,8 +227,8 @@ private:
 
     // Interpolation between primary and secondary surface mesh
     int numInterpolationElementVerts;
-    std::vector<int> interpolationVertices;
-    std::vector<double> interpolationWeights;
+    int *interpolationVertices;
+    double *interpolationWeights;
 
     std::shared_ptr<LinearSolver> linearSolver;
 
diff --git a/Testing/TestMesh.h b/Testing/TestMesh.h
index de809185d83cccbd2c84bfdbda5eba45235bcfab..a46520901e3e82cecf04749d1f1d9fa44987db96 100644
--- a/Testing/TestMesh.h
+++ b/Testing/TestMesh.h
@@ -57,7 +57,7 @@ std::shared_ptr<Mesh> makeSurfaceMesh()
     };
 
     std::shared_ptr<SurfaceMesh> mesh = std::make_shared<SurfaceMesh>();
-    mesh->vertices = vertices;
+    mesh->getVertices() = vertices;
     mesh->triangles = triangles;
 
     return mesh;
diff --git a/VirtualTools/CurvedGrasper.cpp b/VirtualTools/CurvedGrasper.cpp
index 52a198fca47ae5559bd5e9610535229e17ed4add..4cd96e4c20ee70023a190af0b44c4565f9cca393 100644
--- a/VirtualTools/CurvedGrasper.cpp
+++ b/VirtualTools/CurvedGrasper.cpp
@@ -42,24 +42,24 @@ CurvedGrasper::CurvedGrasper(size_t p_PhantomID,
     this->phantomID = p_PhantomID;
 
     Matrix33d rot;
-    mesh_pivot = new SurfaceMesh(BaseMesh::MeshType::Rigid, nullptr);
-    mesh_pivot->loadMesh(p_pivotModelFileName, BaseMesh::MeshFileType::ThreeDS);
+    mesh_pivot = new SurfaceMesh(Core::BaseMesh::MeshType::Rigid, nullptr);
+    mesh_pivot->loadMesh(p_pivotModelFileName, Core::BaseMesh::MeshFileType::ThreeDS);
     mesh_pivot->scale(core::Vec3d(0.5, 0.5, 0.5));
     rot = Eigen::AngleAxisd(-M_PI_2, core::Vec3d::UnitX()).matrix();
     mesh_pivot->rotate(rot);
     rot = Eigen::AngleAxisd(-M_PI_2, core::Vec3d::UnitZ()).matrix();
     mesh_pivot->rotate(rot);
 
-    mesh_upperJaw = new SurfaceMesh(BaseMesh::MeshType::Rigid, nullptr);
-    mesh_upperJaw->loadMesh(p_upperModelFileName, BaseMesh::MeshFileType::ThreeDS);
+    mesh_upperJaw = new SurfaceMesh(Core::BaseMesh::MeshType::Rigid, nullptr);
+    mesh_upperJaw->loadMesh(p_upperModelFileName, Core::BaseMesh::MeshFileType::ThreeDS);
     mesh_upperJaw->scale(core::Vec3d(0.5, 0.5, 0.5));
     rot = Eigen::AngleAxisd(-M_PI_2, core::Vec3d::UnitY()).matrix();
     mesh_upperJaw->rotate(rot);
     rot = Eigen::AngleAxisd(-M_PI_2, core::Vec3d::UnitZ()).matrix();
     mesh_upperJaw->rotate(rot);
 
-    mesh_lowerJaw = new SurfaceMesh(BaseMesh::MeshType::Rigid, nullptr);
-    mesh_lowerJaw->loadMesh(p_lowerModelFileName, BaseMesh::MeshFileType::ThreeDS);
+    mesh_lowerJaw = new SurfaceMesh(Core::BaseMesh::MeshType::Rigid, nullptr);
+    mesh_lowerJaw->loadMesh(p_lowerModelFileName, Core::BaseMesh::MeshFileType::ThreeDS);
     mesh_lowerJaw->scale(core::Vec3d(0.5, 0.5, 0.5));
     rot = Eigen::AngleAxisd(-M_PI_2, core::Vec3d::UnitY()).matrix();
     mesh_lowerJaw->rotate(rot);
diff --git a/VtkRendering/UnitTests/VtkViewerSpec.cpp b/VtkRendering/UnitTests/VtkViewerSpec.cpp
index 530c4d697722106048ea24c81983d9df977fd80f..5ad5ecc07e9841d3eca97fd575df17016d4c414d 100644
--- a/VtkRendering/UnitTests/VtkViewerSpec.cpp
+++ b/VtkRendering/UnitTests/VtkViewerSpec.cpp
@@ -66,7 +66,9 @@ go_bandit([](){
 //             //Add the cube to the scene to be rendered
 //             scene->addSceneObject(cube);
 
+            mesh->assignTexture("/home/rortiz/tmp/CollisionHash_resources/textures/brick.jpg","cubeTex");
             viewer->addObject(mesh);
+
             viewer->exec();
         });
 
diff --git a/VtkRendering/VtkMeshRenderDelegate.cpp b/VtkRendering/VtkMeshRenderDelegate.cpp
index 2e35c4d7dd58371c80d04039a263cc4dcf82504f..1d15b590a8f21936bddaf078ee326f570a4cbed2 100644
--- a/VtkRendering/VtkMeshRenderDelegate.cpp
+++ b/VtkRendering/VtkMeshRenderDelegate.cpp
@@ -108,13 +108,13 @@ void MeshRenderDelegate::initDraw()
         textureCoordinates->SetNumberOfComponents(3);
         textureCoordinates->SetName("TextureCoordinates");
 
-        auto texCoords = mesh->getTextureCoordinates(0);
+        auto texCoords = mesh->getTextureCoordinates();
         for(auto &coord : texCoords)
         {
             float tuple[3] = {coord[0],coord[1],0.0};
             textureCoordinates->InsertNextTuple(tuple);
         }
-        unstructuredMesh->GetPointData()->SetTCoords(textureCoordinates);
+        unstructuredMesh->GetPointData()->SetTCoords(textureCoordinates.GetPointer());
     }
 
     if (mesh->getRenderDetail()->renderType & SIMMEDTK_RENDER_NORMALS)