MutableVertexMesh.cpp

/*

Copyright (c) 2005-2012, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#include "MutableVertexMesh.hpp"
#include "RandomNumberGenerator.hpp"
#include "UblasCustomFunctions.hpp"
#include "Warnings.hpp"
#include "LogFile.hpp"

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::MutableVertexMesh(std::vector<Node<SPACE_DIM>*> nodes,
                                               std::vector<VertexElement<ELEMENT_DIM,SPACE_DIM>*> vertexElements,
                                               double cellRearrangementThreshold,
                                               double t2Threshold,
                                               double cellRearrangementRatio)
    : mCellRearrangementThreshold(cellRearrangementThreshold),
      mCellRearrangementRatio(cellRearrangementRatio),
      mT2Threshold(t2Threshold),
      mCheckForInternalIntersections(false)
{
    assert(cellRearrangementThreshold > 0.0);
    assert(t2Threshold > 0.0);

    // Reset member variables and clear mNodes and mElements
    Clear();

    // Populate mNodes and mElements
    for (unsigned node_index=0; node_index<nodes.size(); node_index++)
    {
        Node<SPACE_DIM>* p_temp_node = nodes[node_index];
        this->mNodes.push_back(p_temp_node);
    }
    for (unsigned elem_index=0; elem_index<vertexElements.size(); elem_index++)
    {
        VertexElement<ELEMENT_DIM,SPACE_DIM>* p_temp_vertex_element = vertexElements[elem_index];
        this->mElements.push_back(p_temp_vertex_element);
    }
    // In 3D, populate mFaces
    if (SPACE_DIM == 3)
    {
        // Use a std::set to keep track of which faces have been added to mFaces
        std::set<unsigned> faces_counted;

        // Loop over mElements
        for (unsigned elem_index=0; elem_index<this->mElements.size(); elem_index++)
        {
            // Loop over faces of this element
            for (unsigned face_index=0; face_index<this->mElements[elem_index]->GetNumFaces(); face_index++)
            {
                VertexElement<ELEMENT_DIM-1, SPACE_DIM>* p_face = this->mElements[elem_index]->GetFace(face_index);

                // If this face is not already contained in mFaces, add it, and update faces_counted
                if (faces_counted.find(p_face->GetIndex()) == faces_counted.end())
                {
                    this->mFaces.push_back(p_face);
                    faces_counted.insert(p_face->GetIndex());
                }
            }
        }
    }

    // Register elements with nodes
    for (unsigned index=0; index<this->mElements.size(); index++)
    {
        VertexElement<ELEMENT_DIM,SPACE_DIM>* p_temp_vertex_element = this->mElements[index];
        for (unsigned node_index=0; node_index<p_temp_vertex_element->GetNumNodes(); node_index++)
        {
            Node<SPACE_DIM>* p_temp_node = p_temp_vertex_element->GetNode(node_index);
            p_temp_node->AddElement(p_temp_vertex_element->GetIndex());
        }
    }

    this->mMeshChangesDuringSimulation = true;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::MutableVertexMesh()
    : mCellRearrangementThreshold(0.01), // Overwritten as soon as archiving is complete
      mCellRearrangementRatio(1.5), // Overwritten as soon as archiving is complete
      mT2Threshold(0.001), // Overwritten as soon as archiving is complete
      mCheckForInternalIntersections(false) // Overwritten as soon as archiving is complete
{
    this->mMeshChangesDuringSimulation = true;
    Clear();
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::~MutableVertexMesh()
{
    Clear();
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
double MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::GetCellRearrangementThreshold() const
{
    return mCellRearrangementThreshold;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
double MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::GetT2Threshold() const
{
    return mT2Threshold;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
double MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::GetCellRearrangementRatio() const
{
    return mCellRearrangementRatio;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
bool MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::GetCheckForInternalIntersections() const
{
    return mCheckForInternalIntersections;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::SetCellRearrangementThreshold(double cellRearrangementThreshold)
{
    mCellRearrangementThreshold = cellRearrangementThreshold;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::SetT2Threshold(double t2Threshold)
{
    mT2Threshold = t2Threshold;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::SetCellRearrangementRatio(double cellRearrangementRatio)
{
    mCellRearrangementRatio = cellRearrangementRatio;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::SetCheckForInternalIntersections(bool checkForInternalIntersections)
{
    mCheckForInternalIntersections=checkForInternalIntersections;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::Clear()
{
    mDeletedNodeIndices.clear();
    mDeletedElementIndices.clear();

    VertexMesh<ELEMENT_DIM, SPACE_DIM>::Clear();
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
unsigned MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::GetNumNodes() const
{
    return this->mNodes.size() - mDeletedNodeIndices.size();
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
unsigned MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::GetNumElements() const
{
    return this->mElements.size() - mDeletedElementIndices.size();
}

//\todo deal with boundary elements in vertex meshes #1392.
//template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
//unsigned AbstractTetrahedralMesh<ELEMENT_DIM, SPACE_DIM>::GetNumBoundaryElements() const
//{
//    return mBoundaryElements.size();
//}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
std::vector< c_vector<double, SPACE_DIM> > MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::GetLocationsOfT1Swaps()
{
    return mLocationsOfT1Swaps;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
std::vector< c_vector<double, SPACE_DIM> > MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::GetLocationsOfT3Swaps()
{
    return mLocationsOfT3Swaps;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::ClearLocationsOfT1Swaps()
{
    mLocationsOfT1Swaps.clear();
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::ClearLocationsOfT3Swaps()
{
    mLocationsOfT3Swaps.clear();
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
unsigned MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::AddNode(Node<SPACE_DIM>* pNewNode)
{
    if (mDeletedNodeIndices.empty())
    {
        pNewNode->SetIndex(this->mNodes.size());
        this->mNodes.push_back(pNewNode);
    }
    else
    {
        unsigned index = mDeletedNodeIndices.back();
        pNewNode->SetIndex(index);
        mDeletedNodeIndices.pop_back();
        delete this->mNodes[index];
        this->mNodes[index] = pNewNode;
    }
    return pNewNode->GetIndex();
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
unsigned MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::AddElement(VertexElement<ELEMENT_DIM,SPACE_DIM>* pNewElement)
{
    unsigned new_element_index = pNewElement->GetIndex();

    if (new_element_index == this->mElements.size())
    {
        this->mElements.push_back(pNewElement);
    }
    else
    {
        this->mElements[new_element_index] = pNewElement;
    }
    pNewElement->RegisterWithNodes();
    return pNewElement->GetIndex();
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::SetNode(unsigned nodeIndex, ChastePoint<SPACE_DIM> point)
{
    this->mNodes[nodeIndex]->SetPoint(point);
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
unsigned MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::DivideElementAlongGivenAxis(VertexElement<ELEMENT_DIM,SPACE_DIM>* pElement,
                                                                                c_vector<double, SPACE_DIM> axisOfDivision,
                                                                                bool placeOriginalElementBelow)
{
    // Make sure that we are in the correct dimension - this code will be eliminated at compile time
    assert(SPACE_DIM == 2); // only works in 2D at present
    assert(ELEMENT_DIM == SPACE_DIM);

    // Get the centroid of the element
    c_vector<double, SPACE_DIM> centroid = this->GetCentroidOfElement(pElement->GetIndex());

    // Create a vector perpendicular to the axis of division
    c_vector<double, SPACE_DIM> perp_axis;
    perp_axis(0) = -axisOfDivision(1);
    perp_axis(1) = axisOfDivision(0);

    /*
     * Find which edges the axis of division crosses by finding any node
     * that lies on the opposite side of the axis of division to its next
     * neighbour.
     */
    unsigned num_nodes = pElement->GetNumNodes();
    std::vector<unsigned> intersecting_nodes;
    for (unsigned i=0; i<num_nodes; i++)
    {
        bool is_current_node_on_left = (inner_prod(this->GetVectorFromAtoB(pElement->GetNodeLocation(i), centroid), perp_axis) >= 0);
        bool is_next_node_on_left = (inner_prod(this->GetVectorFromAtoB(pElement->GetNodeLocation((i+1)%num_nodes), centroid), perp_axis) >= 0);

        if (is_current_node_on_left != is_next_node_on_left)
        {
            intersecting_nodes.push_back(i);
        }
    }

    // If the axis of division does not cross two edges then we cannot proceed
    if (intersecting_nodes.size() != 2)
    {
        EXCEPTION("Cannot proceed with element division: the given axis of division does not cross two edges of the element");
    }

    std::vector<unsigned> division_node_global_indices;
    unsigned nodes_added = 0;

    // Find the intersections between the axis of division and the element edges
    for (unsigned i=0; i<intersecting_nodes.size(); i++)
    {
        /*
         * Get pointers to the nodes forming the edge into which one new node will be inserted.
         *
         * Note that when we use the first entry of intersecting_nodes to add a node,
         * we change the local index of the second entry of intersecting_nodes in
         * pElement, so must account for this by moving one entry further on.
         */
        Node<SPACE_DIM>* p_node_A = pElement->GetNode((intersecting_nodes[i]+nodes_added)%pElement->GetNumNodes());
        Node<SPACE_DIM>* p_node_B = pElement->GetNode((intersecting_nodes[i]+nodes_added+1)%pElement->GetNumNodes());

        // Find the indices of the elements owned by each node on the edge into which one new node will be inserted
        std::set<unsigned> elems_containing_node_A = p_node_A->rGetContainingElementIndices();
        std::set<unsigned> elems_containing_node_B = p_node_B->rGetContainingElementIndices();

        c_vector<double, SPACE_DIM> position_a = p_node_A->rGetLocation();
        c_vector<double, SPACE_DIM> position_b = p_node_B->rGetLocation();
        c_vector<double, SPACE_DIM> a_to_b = this->GetVectorFromAtoB(position_a, position_b);

        c_vector<double, SPACE_DIM> intersection;

        if (norm_2(a_to_b) < 2.0*mCellRearrangementRatio*mCellRearrangementThreshold)
        {
            WARNING("Edge is too small for normal division, putting node in the middle of a and b, there may be T1Swaps straight away.");
            intersection = position_a + 0.5*a_to_b;
        }
        else
        {
            // Find the location of the intersection
            double determinant = a_to_b[0]*axisOfDivision[1] - a_to_b[1]*axisOfDivision[0];

            c_vector<double, SPACE_DIM> moved_centroid = position_a + this->GetVectorFromAtoB(position_a, centroid); // allow for periodicity and other metrics

            double alpha = (moved_centroid[0]*a_to_b[1] - position_a[0]*a_to_b[1]
                            -moved_centroid[1]*a_to_b[0] + position_a[1]*a_to_b[0])/determinant;

            intersection = moved_centroid + alpha*axisOfDivision;

            /*
             * If then new node is too close to one of the edge nodes, then reposition it
             * a distance mCellRearrangementRatio*mCellRearrangementThreshold further along the edge.
             */
            c_vector<double, SPACE_DIM> a_to_intersection = this->GetVectorFromAtoB(position_a, intersection);
            if (norm_2(a_to_intersection) < mCellRearrangementThreshold)
            {
                intersection = position_a + mCellRearrangementRatio*mCellRearrangementThreshold*a_to_b/norm_2(a_to_b);
            }

            c_vector<double, SPACE_DIM> b_to_intersection = this->GetVectorFromAtoB(position_b, intersection);
            if (norm_2(b_to_intersection) < mCellRearrangementThreshold)
            {
                assert(norm_2(a_to_intersection) > mCellRearrangementThreshold); // to prevent moving intersection back to original position

                intersection = position_b - mCellRearrangementRatio*mCellRearrangementThreshold*a_to_b/norm_2(a_to_b);
            }
        }

        /*
         * The new node is boundary node if the 2 nodes are boundary nodes and the elements don't look like
         *   ___A___
         *  |   |   |
         *  |___|___|
         *      B
         */

        bool is_boundary = false;
        if (p_node_A->IsBoundaryNode() && p_node_B->IsBoundaryNode())
        {
            if (elems_containing_node_A.size() !=2 ||
                elems_containing_node_B.size() !=2 ||
                elems_containing_node_A != elems_containing_node_B)
            {
                is_boundary = true;
            }
        }

        // Add a new node to the mesh at the location of the intersection
        unsigned new_node_global_index = this->AddNode(new Node<SPACE_DIM>(0, is_boundary, intersection[0], intersection[1]));
        nodes_added++;

        // Now make sure node is added to neighbouring elements

        // Find common elements
        std::set<unsigned> shared_elements;
        std::set_intersection(elems_containing_node_A.begin(),
                              elems_containing_node_A.end(),
                              elems_containing_node_B.begin(),
                              elems_containing_node_B.end(),
                              std::inserter(shared_elements, shared_elements.begin()));

        // Iterate over common elements
        for (std::set<unsigned>::iterator iter = shared_elements.begin();
             iter != shared_elements.end();
             ++iter)
        {
            // Find which node has the lower local index in this element
            unsigned local_indexA = this->GetElement(*iter)->GetNodeLocalIndex(p_node_A->GetIndex());
            unsigned local_indexB = this->GetElement(*iter)->GetNodeLocalIndex(p_node_B->GetIndex());

            unsigned index = local_indexB;
            if (local_indexB > local_indexA)
            {
                index = local_indexA;
            }
            if ((local_indexA == 0) && (local_indexB == this->GetElement(*iter)->GetNumNodes()-1))
            {
                index = local_indexB;
            }
            if ((local_indexB == 0) && (local_indexA == this->GetElement(*iter)->GetNumNodes()-1))
            {
                index = local_indexA;
            }
            // Add new node to this element
            this->GetElement(*iter)->AddNode(this->GetNode(new_node_global_index), index);
        }
        // Store index of new node
        division_node_global_indices.push_back(new_node_global_index);
    }

    // Now call DivideElement() to divide the element using the new nodes
    unsigned new_element_index = DivideElement(pElement,
                                               pElement->GetNodeLocalIndex(division_node_global_indices[0]),
                                               pElement->GetNodeLocalIndex(division_node_global_indices[1]),
                                               placeOriginalElementBelow);
    return new_element_index;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
unsigned MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::DivideElementAlongShortAxis(VertexElement<ELEMENT_DIM,SPACE_DIM>* pElement,
                                                                                bool placeOriginalElementBelow)
{
    // Make sure that we are in the correct dimension - this code will be eliminated at compile time
    assert(SPACE_DIM == 2); // only works in 2D at present
    assert(ELEMENT_DIM == SPACE_DIM);

    // Find the short axis of the element
    c_vector<double, SPACE_DIM> short_axis = this->GetShortAxisOfElement(pElement->GetIndex());

    unsigned new_element_index = DivideElementAlongGivenAxis(pElement, short_axis, placeOriginalElementBelow);
    return new_element_index;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
unsigned MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::DivideElement(VertexElement<ELEMENT_DIM,SPACE_DIM>* pElement,
                                                                  unsigned nodeAIndex,
                                                                  unsigned nodeBIndex,
                                                                  bool placeOriginalElementBelow)
{
    // Make sure that we are in the correct dimension - this code will be eliminated at compile time
    assert(SPACE_DIM == 2); // only works in 2D at present
    assert(ELEMENT_DIM == SPACE_DIM);

    // Sort nodeA and nodeB such that nodeBIndex > nodeAindex
    assert(nodeBIndex != nodeAIndex);

    unsigned node1_index = (nodeAIndex < nodeBIndex) ? nodeAIndex : nodeBIndex; // low index
    unsigned node2_index = (nodeAIndex < nodeBIndex) ? nodeBIndex : nodeAIndex; // high index

    // Store the number of nodes in the element (this changes when nodes are deleted from the element)
    unsigned num_nodes = pElement->GetNumNodes();

    // Copy the nodes in this element
    std::vector<Node<SPACE_DIM>*> nodes_elem;
    for (unsigned i=0; i<num_nodes; i++)
    {
        nodes_elem.push_back(pElement->GetNode(i));
    }

    // Get the index of the new element
    unsigned new_element_index;
    if (mDeletedElementIndices.empty())
    {
        new_element_index = this->mElements.size();
    }
    else
    {
        new_element_index = mDeletedElementIndices.back();
        mDeletedElementIndices.pop_back();
        delete this->mElements[new_element_index];
    }

    // Add the new element to the mesh
    AddElement(new VertexElement<ELEMENT_DIM,SPACE_DIM>(new_element_index, nodes_elem));

    // Find lowest element
    double height_midpoint_1 = 0.0;
    double height_midpoint_2 = 0.0;
    unsigned counter_1 = 0;
    unsigned counter_2 = 0;

    for (unsigned i=0; i<num_nodes; i++)
    {
        if (i>=node1_index && i<=node2_index)
        {
            height_midpoint_1 += pElement->GetNode(i)->rGetLocation()[1];
            counter_1++;
        }
        if (i<=node1_index || i>=node2_index)
        {
            height_midpoint_2 += pElement->GetNode(i)->rGetLocation()[1];
            counter_2++;
        }
    }
    height_midpoint_1 /= (double)counter_1;
    height_midpoint_2 /= (double)counter_2;

    for (unsigned i=num_nodes; i>0; i--)
    {
        if (i-1 < node1_index || i-1 > node2_index)
        {
            if (height_midpoint_1 < height_midpoint_2)
            {
                if (placeOriginalElementBelow)
                {
                    pElement->DeleteNode(i-1);
                }
                else
                {
                    this->mElements[new_element_index]->DeleteNode(i-1);
                }
            }
            else
            {
                if (placeOriginalElementBelow)
                {
                    this->mElements[new_element_index]->DeleteNode(i-1);
                }
                else
                {
                    pElement->DeleteNode(i-1);
                }
            }
        }
        else if (i-1 > node1_index && i-1 < node2_index)
        {
            if (height_midpoint_1 < height_midpoint_2)
            {
                if (placeOriginalElementBelow)
                {
                    this->mElements[new_element_index]->DeleteNode(i-1);
                }
                else
                {
                    pElement->DeleteNode(i-1);
                }
            }
            else
            {
                if (placeOriginalElementBelow)
                {
                    pElement->DeleteNode(i-1);
                }
                else
                {
                    this->mElements[new_element_index]->DeleteNode(i-1);
                }
            }
        }
    }

    return new_element_index;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::DeleteElementPriorToReMesh(unsigned index)
{
    assert(SPACE_DIM == 2);

    // Mark any nodes that are contained only in this element as deleted
    for (unsigned i=0; i<this->mElements[index]->GetNumNodes(); i++)
    {
        Node<SPACE_DIM>* p_node = this->mElements[index]->GetNode(i);

        if (p_node->rGetContainingElementIndices().size()==1)
        {
            DeleteNodePriorToReMesh(p_node->GetIndex());
        }

        // Mark all the nodes contained in the removed element as boundary nodes
        p_node->SetAsBoundaryNode(true);
    }

    // Mark this element as deleted
    this->mElements[index]->MarkAsDeleted();
    mDeletedElementIndices.push_back(index);
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::DeleteNodePriorToReMesh(unsigned index)
{
    this->mNodes[index]->MarkAsDeleted();
    mDeletedNodeIndices.push_back(index);
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::DivideEdge(Node<SPACE_DIM>* pNodeA, Node<SPACE_DIM>* pNodeB)
{
    // Find the indices of the elements owned by each node
    std::set<unsigned> elements_containing_nodeA = pNodeA->rGetContainingElementIndices();
    std::set<unsigned> elements_containing_nodeB = pNodeB->rGetContainingElementIndices();

    // Find common elements
    std::set<unsigned> shared_elements;
    std::set_intersection(elements_containing_nodeA.begin(),
                          elements_containing_nodeA.end(),
                          elements_containing_nodeB.begin(),
                          elements_containing_nodeB.end(),
                          std::inserter(shared_elements, shared_elements.begin()));

    // Check that the nodes have a common edge and not more than 2
    assert(!shared_elements.empty());
    assert(!shared_elements.size()<=2);

    // Specifiy if its a boundary node
    bool is_boundary_node = false;
    if (shared_elements.size()==1)
    {
        // If only one shared element then must be on the boundary.
        assert((pNodeA->IsBoundaryNode())&&(pNodeB->IsBoundaryNode()));
        is_boundary_node = true;
    }

    // Create a new node (position is not important as it will be changed)
    Node<SPACE_DIM>* p_new_node = new Node<SPACE_DIM>(GetNumNodes(), is_boundary_node, 0.0, 0.0);

    // Update the node location
    c_vector<double, SPACE_DIM> new_node_position = pNodeA->rGetLocation() + 0.5*this->GetVectorFromAtoB(pNodeA->rGetLocation(), pNodeB->rGetLocation());
    ChastePoint<SPACE_DIM> point(new_node_position);
    p_new_node->SetPoint(new_node_position);

    // Add node to mesh
    this->mNodes.push_back(p_new_node);

    // Iterate over common elements
    for (std::set<unsigned>::iterator iter = shared_elements.begin();
         iter != shared_elements.end();
         ++iter)
    {
        // Find which node has the lower local index in this element
        unsigned local_indexA = this->GetElement(*iter)->GetNodeLocalIndex(pNodeA->GetIndex());
        unsigned local_indexB = this->GetElement(*iter)->GetNodeLocalIndex(pNodeB->GetIndex());

        unsigned index = local_indexB;
        if ( local_indexB > local_indexA )
        {
            index = local_indexA;
        }
        if ( (local_indexA == 0) && (local_indexB == this->GetElement(*iter)->GetNumNodes()-1))
        {
            index = local_indexB;
        }
        if ( (local_indexB == 0) && (local_indexA == this->GetElement(*iter)->GetNumNodes()-1))
        {
            index = local_indexA;
        }
        // Add new node to this element
        this->GetElement(*iter)->AddNode(p_new_node, index);
    }
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::RemoveDeletedNodesAndElements(VertexElementMap& rElementMap)
{
    // Make sure that we are in the correct dimension - this code will be eliminated at compile time
    assert(SPACE_DIM==2 || SPACE_DIM==3);
    assert(ELEMENT_DIM == SPACE_DIM);

    // Make sure the map is big enough
    rElementMap.Resize(this->GetNumAllElements());

    // Remove deleted elements
    std::vector<VertexElement<ELEMENT_DIM, SPACE_DIM>*> live_elements;
    for (unsigned i=0; i< this->mElements.size(); i++)
    {
        if (this->mElements[i]->IsDeleted())
        {
            delete this->mElements[i];
            rElementMap.SetDeleted(i);
        }
        else
        {
            live_elements.push_back(this->mElements[i]);
            rElementMap.SetNewIndex(i, (unsigned)(live_elements.size()-1));
        }
    }

    assert(mDeletedElementIndices.size() == this->mElements.size() - live_elements.size());
    mDeletedElementIndices.clear();
    this->mElements = live_elements;

    for (unsigned i=0; i<this->mElements.size(); i++)
    {
        this->mElements[i]->ResetIndex(i);
    }

    // Remove deleted nodes
    RemoveDeletedNodes();
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::RemoveDeletedNodes()
{
    std::vector<Node<SPACE_DIM>*> live_nodes;
    for (unsigned i=0; i<this->mNodes.size(); i++)
    {
        if (this->mNodes[i]->IsDeleted())
        {
            delete this->mNodes[i];
        }
        else
        {
            live_nodes.push_back(this->mNodes[i]);
        }
    }

    assert(mDeletedNodeIndices.size() == this->mNodes.size() - live_nodes.size());
    this->mNodes = live_nodes;
    mDeletedNodeIndices.clear();

    for (unsigned i=0; i<this->mNodes.size(); i++)
    {
        this->mNodes[i]->SetIndex(i);
    }
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::ReMesh(VertexElementMap& rElementMap)
{
    // Make sure that we are in the correct dimension - this code will be eliminated at compile time
    assert(SPACE_DIM==2 || SPACE_DIM==3);
    assert(ELEMENT_DIM == SPACE_DIM);

    if (SPACE_DIM==2)
    {
        /*
         * We do not need to call Clear() and remove all current data, since
         * cell birth, rearrangement and death result only in local remeshing
         * of a vertex-based mesh.
         */

        // Remove deleted nodes and elements
        RemoveDeletedNodesAndElements(rElementMap);

        // Check for element rearrangements
        bool recheck_mesh = true;
        while (recheck_mesh == true)
        {
            // Separate loops as need to check for T2Swaps first
            recheck_mesh = CheckForT2Swaps(rElementMap);

            if (recheck_mesh == false)
            {
                recheck_mesh = CheckForT1Swaps(rElementMap);
            }
        }

        //Check for element intersections.
        recheck_mesh = true;
        while (recheck_mesh == true)
        {
            //Check mesh for intersections, and perform T3Swaps where required
            recheck_mesh = CheckForIntersections();
        }

        RemoveDeletedNodes(); // to remove any nodes if they are deleted
    }
    else // 3D
    {
        #define COVERAGE_IGNORE
        EXCEPTION("Remeshing has not been implemented in 3D (see #827 and #860)\n");
        #undef COVERAGE_IGNORE

    }
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::ReMesh()
{
    VertexElementMap map(GetNumElements());
    ReMesh(map);
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
bool MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::CheckForT1Swaps(VertexElementMap& rElementMap)
{
    // Loop over elements to check for T1Swaps
    for (typename VertexMesh<ELEMENT_DIM, SPACE_DIM>::VertexElementIterator elem_iter = this->GetElementIteratorBegin();
         elem_iter != this->GetElementIteratorEnd();
         ++elem_iter)
    {
        unsigned num_nodes = elem_iter->GetNumNodes();
        assert(num_nodes > 0); // if not element should be deleted

        unsigned new_num_nodes = num_nodes;

        /*
         * Perform T1 swaps and merges where necessary
         * Check there are > 3 nodes in both elements that contain the pair of nodes
         * and the edges are small enough
         */

        // Loop over element vertices
        for (unsigned local_index=0; local_index<num_nodes; local_index++)
        {
            // Find locations of current node and anticlockwise node
            Node<SPACE_DIM>* p_current_node = elem_iter->GetNode(local_index);
            unsigned local_index_plus_one = (local_index+1)%new_num_nodes; 
            Node<SPACE_DIM>* p_anticlockwise_node = elem_iter->GetNode(local_index_plus_one);

            // Find distance between nodes
            double distance_between_nodes = this->GetDistanceBetweenNodes(p_current_node->GetIndex(), p_anticlockwise_node->GetIndex());

            std::set<unsigned> elements_of_node_a = p_current_node->rGetContainingElementIndices();
            std::set<unsigned> elements_of_node_b = p_anticlockwise_node->rGetContainingElementIndices();

            std::set<unsigned> all_elements;
            std::set_union(elements_of_node_a.begin(), elements_of_node_a.end(),
                           elements_of_node_b.begin(), elements_of_node_b.end(),
                           std::inserter(all_elements, all_elements.begin()));

            // Track if either node is in a triangular element.
            bool triangular_element = false;

            for (std::set<unsigned>::const_iterator it = all_elements.begin();
                 it != all_elements.end();
                 ++it)
            {
                if (this->GetElement(*it)->GetNumNodes() <= 3)
                {
                    triangular_element = true;
                }
            }

            // If the nodes are too close together and we don't have any triangular elements connected to the nodes, perform a swap
            if ((!triangular_element) && (distance_between_nodes < mCellRearrangementThreshold))
            {
                // Identify the type of node swap/merge needed then call method to perform swap/merge
                IdentifySwapType(p_current_node, p_anticlockwise_node, rElementMap);
                return true;
            }
        }
    }
    return false;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
bool MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::CheckForT2Swaps(VertexElementMap& rElementMap)
{

    // Loop over elements to check for T2Swaps
    for (typename VertexMesh<ELEMENT_DIM, SPACE_DIM>::VertexElementIterator elem_iter = this->GetElementIteratorBegin();
         elem_iter != this->GetElementIteratorEnd();
         ++elem_iter)
    {
        if (elem_iter->GetNumNodes() == 3u)
        {
            /*
             * Perform T2 swaps where necessary
             * Check there are only 3 nodes and the element is small enough
             */
            if (this->GetVolumeOfElement(elem_iter->GetIndex()) < GetT2Threshold())
            {
                PerformT2Swap(*elem_iter);

                // Now remove the deleted nodes (if we don't do this then the search for T1Swap causes errors)
                RemoveDeletedNodesAndElements(rElementMap);

                return true;
            }
        }
    }
    return false;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
bool MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::CheckForIntersections()
{
    // Check that no boundary nodes have overlapped elements on the boundary
    for (typename AbstractMesh<ELEMENT_DIM,SPACE_DIM>::NodeIterator node_iter = this->GetNodeIteratorBegin();
         node_iter != this->GetNodeIteratorEnd();
         ++node_iter)
    {
        if (node_iter->IsBoundaryNode())
        {
            assert(!(node_iter->IsDeleted()));

            for (typename VertexMesh<ELEMENT_DIM, SPACE_DIM>::VertexElementIterator elem_iter = this->GetElementIteratorBegin();
                 elem_iter != this->GetElementIteratorEnd();
                 ++elem_iter)
            {
                if (elem_iter->IsElementOnBoundary())
                {

                    unsigned elem_index = elem_iter->GetIndex();

                    // Check that the node is not part of this element.
                    if (node_iter->rGetContainingElementIndices().count(elem_index) == 0)
                    {
                        if (this->ElementIncludesPoint(node_iter->rGetLocation(), elem_index))
                        {
                            PerformT3Swap(&(*node_iter), elem_index);
                            return true;
                        }
                    }
                }
            }
        }
    }

    if (mCheckForInternalIntersections)
    {
        // Check that no nodes have overlapped elements inside the mesh  // Change to only loop over neighboring elements
        for (typename AbstractMesh<ELEMENT_DIM,SPACE_DIM>::NodeIterator node_iter = this->GetNodeIteratorBegin();
             node_iter != this->GetNodeIteratorEnd();
             ++node_iter)
        {
             assert(!(node_iter->IsDeleted()));

            for (typename VertexMesh<ELEMENT_DIM, SPACE_DIM>::VertexElementIterator elem_iter = this->GetElementIteratorBegin();
                 elem_iter != this->GetElementIteratorEnd();
                 ++elem_iter)
            {
                unsigned elem_index = elem_iter->GetIndex();

                // Check that the node is not part of this element.
                if (node_iter->rGetContainingElementIndices().count(elem_index) == 0)
                {
                    if (this->ElementIncludesPoint(node_iter->rGetLocation(), elem_index))
                    {
                        PerformIntersectionSwap(&(*node_iter), elem_index);
                        return true;
                    }
                }
            }
        }
    }

    return false;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::IdentifySwapType(Node<SPACE_DIM>* pNodeA, Node<SPACE_DIM>* pNodeB, VertexElementMap& rElementMap)
{
    // Make sure that we are in the correct dimension - this code will be eliminated at compile time
    assert(SPACE_DIM == 2); // this method only works in 2D at present
    assert(ELEMENT_DIM == SPACE_DIM);

    // Find the sets of elements containing nodes A and B
    std::set<unsigned> nodeA_elem_indices = pNodeA->rGetContainingElementIndices();

    std::set<unsigned> nodeB_elem_indices = pNodeB->rGetContainingElementIndices();

    // Form the set union
    std::set<unsigned> all_indices, temp_set;
    std::set_union(nodeA_elem_indices.begin(), nodeA_elem_indices.end(),
                   nodeB_elem_indices.begin(), nodeB_elem_indices.end(),
                   std::inserter(temp_set, temp_set.begin()));
    all_indices.swap(temp_set); // temp_set will be deleted

    if ((nodeA_elem_indices.size()>3) || (nodeB_elem_indices.size()>3))
    {
        /*
         * Looks like
         *
         *  \
         *   \ A   B
         * ---o---o---
         *   /
         *  /
         *
         */
        EXCEPTION("A node is contained in more than three elements"); // the code can't handle this case
    }
    else // each node is contained in at most three elements
    {
        switch (all_indices.size())
        {
            case 1:
            {
                /*
                 * In this case, each node is contained in a single element, so the nodes
                 * lie on the boundary of the mesh:
                 *
                 *    A   B
                 * ---o---o---
                 *
                 * We merge the nodes.
                 */

                // Check nodes A and B are on the boundary
                assert(pNodeA->IsBoundaryNode());
                assert(pNodeB->IsBoundaryNode());

                PerformNodeMerge(pNodeA, pNodeB);

                // Remove the deleted node and re-index
                RemoveDeletedNodes();
                break;
            }
            case 2:
            {
                if (nodeA_elem_indices.size()==2 && nodeB_elem_indices.size()==2)
                {
                    if (pNodeA->IsBoundaryNode() && pNodeB->IsBoundaryNode())
                    {
                        /*
                         * In this case, the node configuration looks like:
                         *
                         *   \   /
                         *    \ / Node A
                         * (1) |   (2)      (element number in brackets)
                         *    / \ Node B
                         *   /   \
                         *
                         * With voids on top and bottom
                         *
                         * We perform a Type 1 swap and separate the elements in this case.
                         */
                         PerformT1Swap(pNodeA, pNodeB,all_indices);
                    }
                    else if (pNodeA->IsBoundaryNode() || pNodeB->IsBoundaryNode())
                    {
                        /*
                         * In this case, the node configuration looks like:
                         *
                         *   \   /
                         *    \ / Node A
                         * (1) |   (2)      (element number in brackets)
                         *     x Node B
                         *     |
                         *
                         * With a void on top.
                         *
                         * We should not be able to get here when running normal vertex code
                         * as there should only be nodes on 3 way junctions or boundaries.
                         *
                         */

                        EXCEPTION("There is a non boundary node contained only in 2 elements something has gone wrong.");
                    }
                    else
                    {
                        /*
                         * In this case, each node is contained in two elements, so the nodes
                         * lie on an internal edge:
                         *
                         *    A   B
                         * ---o---o---
                         *
                         * We should not be able to get here when running normal vertex code
                         * as there should only be nodes on 3 way junctions or boundaries.
                         */

                        EXCEPTION("There are non-boundary nodes contained in only in 2 elements something has gone wrong.");
                    }
                }
                else
                {
                    /*
                     * In this case, the node configuration looks like:
                     *
                     * Outside
                     *         /
                     *   --o--o (2)
                     *     (1) \
                     *
                     * We merge the nodes in this case. ///\todo this should be a T1 Swap see #1263
                     */

                    PerformNodeMerge(pNodeA, pNodeB);

                    // Remove the deleted node and re-index
                    RemoveDeletedNodes();
                }
                break;
            }
            case 3:
            {
                if (nodeA_elem_indices.size()==1 || nodeB_elem_indices.size()==1)
                {
                    /*
                     * In this case, one node is contained in one element and
                     * the other node is contained in three elements:
                     *
                     *    A   B
                     *
                     *  empty   /
                     *         / (3)
                     * ---o---o-----   (element number in brackets)
                     *  (1)    \ (2)
                     *          \
                     *
                     * We should not be able to get here when running normal vertex code
                     * as a boundary node is contained in at most 2 elements.
                     */

                    // Check nodes A and B are on the boundary
                    assert(pNodeA->IsBoundaryNode());
                    assert(pNodeB->IsBoundaryNode());

                    EXCEPTION("There is a boundary node contained in three elements something has gone wrong.");
                }
                else if (nodeA_elem_indices.size()==2 && nodeB_elem_indices.size()==2)
                {
                    // element in nodeA_elem_indices which is not in nodeB_elem_indices contains a shared node with the element in nodeA_elem_indices which is not in nodeB_elem_indices.

                    std::set<unsigned> element_A_not_B, temp_set;
                    std::set_difference(all_indices.begin(), all_indices.end(),
                                        nodeB_elem_indices.begin(), nodeB_elem_indices.end(),
                                        std::inserter(temp_set, temp_set.begin()));
                    element_A_not_B.swap(temp_set);

                    // Should only be one such element
                    assert(element_A_not_B.size()==1);

                    std::set<unsigned> element_B_not_A;
                    std::set_difference(all_indices.begin(), all_indices.end(),
                                        nodeA_elem_indices.begin(), nodeA_elem_indices.end(),
                                        std::inserter(temp_set, temp_set.begin()));
                    element_B_not_A.swap(temp_set);

                    // Should only be one such element
                    assert(element_B_not_A.size()==1);

                    VertexElement<ELEMENT_DIM, SPACE_DIM>* p_element_A_not_B = this->mElements[*element_A_not_B.begin()];
                    VertexElement<ELEMENT_DIM, SPACE_DIM>* p_element_B_not_A = this->mElements[*element_B_not_A.begin()];

                    unsigned local_index_1 = p_element_A_not_B->GetNodeLocalIndex(pNodeA->GetIndex());
                    unsigned next_node_1 = p_element_A_not_B->GetNodeGlobalIndex((local_index_1 + 1)%(p_element_A_not_B->GetNumNodes()));
                    unsigned previous_node_1 = p_element_A_not_B->GetNodeGlobalIndex((local_index_1 + p_element_A_not_B->GetNumNodes() - 1)%(p_element_A_not_B->GetNumNodes()));
                    unsigned local_index_2 = p_element_B_not_A->GetNodeLocalIndex(pNodeB->GetIndex());
                    unsigned next_node_2 = p_element_B_not_A->GetNodeGlobalIndex((local_index_2 + 1)%(p_element_B_not_A->GetNumNodes()));
                    unsigned previous_node_2 = p_element_B_not_A->GetNodeGlobalIndex((local_index_2 + p_element_B_not_A->GetNumNodes() - 1)%(p_element_B_not_A->GetNumNodes()));

                    if (next_node_1 == previous_node_2 || next_node_2 == previous_node_1)
                     {
                        /*
                         * In this case, the node configuration looks like:
                         *
                         *    A  C  B                A      B
                         *      /\                 \        /
                         *     /v \                 \  (1) /
                         * (3)o----o (1)  or     (2) o----o (3)    (element number in brackets, v is a void)
                         *   /  (2) \                 \v /
                         *  /        \                 \/
                         *                             C
                         *
                         * We perform a T3 way merge, removing the void.
                         */

                        // Check nodes A and B are on the boundary
                        assert(pNodeA->IsBoundaryNode());
                        assert(pNodeB->IsBoundaryNode());

                        // Get the other node in the triangular void
                        unsigned nodeC_index;
                        if (next_node_1 == previous_node_2 && next_node_2 != previous_node_1)
                        {
                            nodeC_index = next_node_1;
                        }
                        else if (next_node_2 == previous_node_1 && next_node_1 != previous_node_2)
                        {
                            nodeC_index = next_node_2;
                        }
                        else
                        {
                             assert(next_node_1 == previous_node_2 && next_node_2 == previous_node_1);
                             EXCEPTION("Triangular element next to triangular void, not implemented yet.");
                        }

                        PerformVoidRemoval(pNodeA, pNodeB, this->mNodes[nodeC_index]);
                    }
                    else
                    {
                        /*
                         * In this case, the node configuration looks like:
                         *
                         *     A  B                  A  B
                         *   \ empty/              \      /
                         *    \    /                \(1) /
                         * (3) o--o (1)  or      (2) o--o (3)    (element number in brackets)
                         *    / (2)\                /    \
                         *   /      \              /empty \
                         *
                         * We perform a T1 Swap in this case.
                         */

                        // Check nodes A and B are on the boundary
                        assert(pNodeA->IsBoundaryNode());
                        assert(pNodeB->IsBoundaryNode());

                        PerformT1Swap(pNodeA, pNodeB, all_indices);
                    }
                }
                else
                {
                    /*
                     * In this case, one of the nodes is contained in two elements
                     * and the other node is contained in three elements.
                     */
                    assert (   (nodeA_elem_indices.size()==2 && nodeB_elem_indices.size()==3)
                            || (nodeA_elem_indices.size()==3 && nodeB_elem_indices.size()==2) );

                    // They can't both be boundary nodes
                    assert(!(pNodeA->IsBoundaryNode() && pNodeB->IsBoundaryNode()));

                    if (pNodeA->IsBoundaryNode() || pNodeB->IsBoundaryNode())
                    {
                        /*
                         * In this case, the node configuration looks like:
                         *
                         *     A  B                      A  B
                         *   \      /                  \      /
                         *    \ (1)/                    \(1) /
                         * (3) o--o (empty)  or  (empty) o--o (3)    (element number in brackets)
                         *    / (2)\                    /(2) \
                         *   /      \                  /      \
                         *
                         * We perform a Type 1 swap in this case.
                         */
                        PerformT1Swap(pNodeA, pNodeB, all_indices);
                    }
                    else
                    {
                        /*
                         * In this case, the node configuration looks like:
                         *
                         *     A  B             A  B
                         *   \                       /
                         *    \  (1)           (1)  /
                         * (3) o--o---   or  ---o--o (3)    (element number in brackets)
                         *    /  (2)           (2)  \
                         *   /                       \
                         *
                          * We should not be able to get here when running normal vertex code
                         * as there should only be nodes on 3 way junctions or boundaries.
                         */

                        EXCEPTION("There are non-boundary nodes contained only in 2 elements something has gone wrong.");
                    }
                }
                break;
            }
            case 4:
            {
                /*
                 * In this case, the node configuration looks like:
                 *
                 *   \(1)/
                 *    \ / Node A
                 * (2) |   (4)      (element number in brackets)
                 *    / \ Node B
                 *   /(3)\
                 *
                 * We perform a Type 1 swap in this case.
                 */
                PerformT1Swap(pNodeA, pNodeB, all_indices);
                break;
            }
            default:
                // This can't happen
                NEVER_REACHED;
        }
    }
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::PerformNodeMerge(Node<SPACE_DIM>* pNodeA, Node<SPACE_DIM>* pNodeB)
{
    // Sort the nodes by index
    unsigned nodeA_index = pNodeA->GetIndex();
    unsigned nodeB_index = pNodeB->GetIndex();

    unsigned lo_node_index = (nodeA_index < nodeB_index) ? nodeA_index : nodeB_index; // low index
    unsigned hi_node_index = (nodeA_index < nodeB_index) ? nodeB_index : nodeA_index; // high index

    // Get pointers to the nodes, sorted by index
    Node<SPACE_DIM>* p_lo_node = this->GetNode(lo_node_index);
    Node<SPACE_DIM>* p_hi_node = this->GetNode(hi_node_index);

    // Find the sets of elements containing each of the nodes, sorted by index
    std::set<unsigned> lo_node_elem_indices = p_lo_node->rGetContainingElementIndices();
    std::set<unsigned> hi_node_elem_indices = p_hi_node->rGetContainingElementIndices();

    // Move the low-index node to the mid-point
    c_vector<double, SPACE_DIM> node_midpoint = p_lo_node->rGetLocation() + 0.5*this->GetVectorFromAtoB(p_lo_node->rGetLocation(), p_hi_node->rGetLocation());
    c_vector<double, SPACE_DIM>& r_lo_node_location = p_lo_node->rGetModifiableLocation();
    r_lo_node_location = node_midpoint;

    // Update the elements previously containing the high-index node to contain the low-index node
    for (std::set<unsigned>::const_iterator it = hi_node_elem_indices.begin();
         it != hi_node_elem_indices.end();
         ++it)
    {
        // Find the local index of the high-index node in this element
        unsigned hi_node_local_index = this->mElements[*it]->GetNodeLocalIndex(hi_node_index);
        assert(hi_node_local_index < UINT_MAX); // this element should contain the high-index node

        /*
         * If this element already contains the low-index node, then just remove the high-index node.
         * Otherwise replace it with the low-index node in the element and remove it from mNodes.
         */
        if (lo_node_elem_indices.count(*it) > 0)
        {
            this->mElements[*it]->DeleteNode(hi_node_local_index); // think this method removes the high-index node from mNodes
        }
        else
        {
            // Replace the high-index node with the low-index node in this element
            this->mElements[*it]->UpdateNode(hi_node_local_index, p_lo_node);
        }
    }
    assert(!(this->mNodes[hi_node_index]->IsDeleted()));
    this->mNodes[hi_node_index]->MarkAsDeleted();
    mDeletedNodeIndices.push_back(hi_node_index);
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::PerformT1Swap(Node<SPACE_DIM>* pNodeA,
                                                              Node<SPACE_DIM>* pNodeB,
                                                              std::set<unsigned>& rElementsContainingNodes)
{
    // Make sure that we are in the correct dimension - this code will be eliminated at compile time
    assert(SPACE_DIM == 2); // only works in 2D at present
    assert(ELEMENT_DIM == SPACE_DIM);

    /*
     * Restructure elements - remember to update nodes and elements.
     *
     * We need to implement the following changes:
     *
     * The element whose index was in nodeA_elem_indices but not nodeB_elem_indices,
     * and the element whose index was in nodeB_elem_indices but not nodeA_elem_indices,
     * should now both contain nodes A and B.
     *
     * The element whose index was in nodeA_elem_indices and nodeB_elem_indices, and which
     * node C lies inside, should now only contain node A.
     *
     * The element whose index was in nodeA_elem_indices and nodeB_elem_indices, and which
     * node D lies inside, should now only contain node B.
     *
     * Iterate over all elements involved and identify which element they are
     * in the diagram then update the nodes as necessary.
     *
     *   \(1)/
     *    \ / Node A
     * (2) |   (4)     elements in brackets
     *    / \ Node B
     *   /(3)\
     *
     */

    /*
     * Compute the locations of two new nodes C, D, placed on either side of the
     * edge E_old formed by nodes current_node and anticlockwise_node, such
     * that the edge E_new formed by the new nodes is the perpendicular bisector
     * of E_old, with |E_new| 'just larger' (mCellRearrangementRatio) than mThresholdDistance.
     */

    // Find the sets of elements containing nodes A and B
    std::set<unsigned> nodeA_elem_indices = pNodeA->rGetContainingElementIndices();
    std::set<unsigned> nodeB_elem_indices = pNodeB->rGetContainingElementIndices();

    double distance_between_nodes_CD = mCellRearrangementRatio*mCellRearrangementThreshold;

    c_vector<double, SPACE_DIM> nodeA_location = pNodeA->rGetLocation();
    c_vector<double, SPACE_DIM> nodeB_location = pNodeB->rGetLocation();

    c_vector<double, SPACE_DIM> a_to_b = this->GetVectorFromAtoB(nodeA_location, nodeB_location);

    // Store the location of the T1Swap, the mid point of the moving nodes,
    mLocationsOfT1Swaps.push_back(nodeA_location + 0.5*a_to_b);

    c_vector<double, SPACE_DIM> perpendicular_vector;
    perpendicular_vector(0) = -a_to_b(1);
    perpendicular_vector(1) = a_to_b(0);

    if (norm_2(a_to_b) < 1e-10)
    {
        EXCEPTION("Nodes are too close together, this shouldn't happen");
    }

    c_vector<double, SPACE_DIM> c_to_d = distance_between_nodes_CD / norm_2(a_to_b) * perpendicular_vector;
    c_vector<double, SPACE_DIM> nodeC_location = nodeA_location + 0.5*a_to_b - 0.5*c_to_d;
    c_vector<double, SPACE_DIM> nodeD_location = nodeC_location + c_to_d;

    /*
     * Move node A to C and node B to D
     */

    c_vector<double, SPACE_DIM>& r_nodeA_location = pNodeA->rGetModifiableLocation();
    r_nodeA_location = nodeC_location;

    c_vector<double, SPACE_DIM>& r_nodeB_location = pNodeB->rGetModifiableLocation();
    r_nodeB_location = nodeD_location;

    for (std::set<unsigned>::const_iterator it = rElementsContainingNodes.begin();
         it != rElementsContainingNodes.end();
         ++it)
    {
        if (nodeA_elem_indices.find(*it) == nodeA_elem_indices.end()) // not in nodeA_elem_indices so element 3
        {
            /*
             * In this case the element index was not in
             * nodeA_elem_indices, so this element
             * does not contain node A. Therefore we must add node A
             * (which has been moved to node C) to this element.
             *
             * Locate local index of node B in element then add node A after
             * in anticlockwise direction.
             */

            unsigned nodeB_local_index = this->mElements[*it]->GetNodeLocalIndex(pNodeB->GetIndex());
            assert(nodeB_local_index < UINT_MAX); // this element should contain node B

            this->mElements[*it]->AddNode(pNodeA, nodeB_local_index);
        }
        else if (nodeB_elem_indices.find(*it) == nodeB_elem_indices.end()) // not in nodeB_elem_indices so element 1
        {
            /*
             * In this case the element index was not in
             * nodeB_elem_indices, so this element
             * does not contain node B. Therefore we must add node B
             * (which has been moved to node D) to this element.
             *
             * Locate local index of node A in element then add node B after
             * in anticlockwise direction.
             */
            unsigned nodeA_local_index = this->mElements[*it]->GetNodeLocalIndex(pNodeA->GetIndex());
            assert(nodeA_local_index < UINT_MAX); // this element should contain node A
            this->mElements[*it]->AddNode(pNodeB, nodeA_local_index);
        }
        else
        {
            /*
             * In this case the element index was in both nodeB_elem_indices and nodeB_elem_indices
             * so is element 2 or 4
             */

            /*
             * Locate local index of nodeA and nodeB and use the ordering to
             * identify the element, if nodeB_index > nodeA_index then element 4
             * and if nodeA_index > nodeB_index then element 2
             */
            unsigned nodeA_local_index = this->mElements[*it]->GetNodeLocalIndex(pNodeA->GetIndex());
            assert(nodeA_local_index < UINT_MAX); // this element should contain node A

            unsigned nodeB_local_index = this->mElements[*it]->GetNodeLocalIndex(pNodeB->GetIndex());
            assert(nodeB_local_index < UINT_MAX); // this element should contain node B

            unsigned nodeB_local_index_plus_one = (nodeB_local_index + 1)%(this->mElements[*it]->GetNumNodes());

            if (nodeA_local_index == nodeB_local_index_plus_one)
            {
                /*
                 * In this case the local index of nodeA is the local index of
                 * nodeB plus one so we are in element 2 so we remove nodeB
                 */
                this->mElements[*it]->DeleteNode(nodeB_local_index);
            }
            else
            {
                assert(nodeB_local_index == (nodeA_local_index + 1)%(this->mElements[*it]->GetNumNodes())); // as A and B are next to each other
                /*
                 * In this case the local index of nodeA is the local index of
                 * nodeB minus one so we are in element 4 so we remove nodeA
                 */
                this->mElements[*it]->DeleteNode(nodeA_local_index);
            }
        }
    }

    // Sort out boundary nodes
    if (pNodeA->IsBoundaryNode() || pNodeB->IsBoundaryNode())
    {
        if (pNodeA->GetNumContainingElements()==3)
        {
            pNodeA->SetAsBoundaryNode(false);
        }
        else
        {
            pNodeA->SetAsBoundaryNode(true);
        }
        if (pNodeB->GetNumContainingElements()==3)
        {
            pNodeB->SetAsBoundaryNode(false);
        }
        else
        {
            pNodeB->SetAsBoundaryNode(true);
        }
    }
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::PerformIntersectionSwap(Node<SPACE_DIM>* pNode, unsigned elementIndex)
{
    // Make sure that we are in the correct dimension - this code will be eliminated at compile time
    assert(SPACE_DIM == 2); // only works in 2D at present
    assert(ELEMENT_DIM == SPACE_DIM);

    VertexElement<ELEMENT_DIM, SPACE_DIM>* p_element = this->GetElement(elementIndex);
    unsigned num_nodes = p_element->GetNumNodes();

    std::set<unsigned> elements_containing_intersecting_node;

    for (unsigned node_local_index=0; node_local_index<num_nodes; node_local_index++)
    {
        unsigned node_global_index = p_element->GetNodeGlobalIndex(node_local_index);

        std::set<unsigned> node_elem_indices = this->GetNode(node_global_index)->rGetContainingElementIndices();

        for (std::set<unsigned>::const_iterator elem_iter = node_elem_indices.begin();
             elem_iter != node_elem_indices.end();
             ++elem_iter)
        {
            VertexElement<ELEMENT_DIM, SPACE_DIM>* p_neighboring_element = this->GetElement(*elem_iter);

            // Check if element contains the intersecting node
            for (unsigned node_index_2 = 0;  node_index_2 < p_neighboring_element->GetNumNodes(); node_index_2++)
            {
                if(p_neighboring_element->GetNodeGlobalIndex(node_index_2)==pNode->GetIndex())
                {
                    elements_containing_intersecting_node.insert(p_neighboring_element->GetIndex());
                }
            }
        }
    }
    /*
     * If there are not 2 elements containing the intersecting node then the node is coming from the other side of the element
     * and there is no way to fix it unles you want to make 2 new elements.
     */
    assert(elements_containing_intersecting_node.size()==2);

    std::set<unsigned> all_elements_containing_intersecting_node = pNode->rGetContainingElementIndices();

    std::set<unsigned> intersecting_element;

    std::set_difference( all_elements_containing_intersecting_node.begin(), all_elements_containing_intersecting_node.end(),
                         elements_containing_intersecting_node.begin(), elements_containing_intersecting_node.end(),
                         std::inserter(intersecting_element, intersecting_element.begin()));

    /*
     * Identify nodes and Elements to perform switch on
     * Intersecting node is node A
     * Other node is node B
     *
     * Element 1 only contains node A
     * Element 2 has nodes B and A (in that order)
     * Element 3 only contains node B
     * Element 4 has nodes A and B (in that order)
     */
    unsigned node_A_index = pNode->GetIndex();
    unsigned node_B_index;
    unsigned element_1_index =  *(intersecting_element.begin());
    unsigned element_2_index;
    unsigned element_3_index =  elementIndex;
    unsigned element_4_index;

    std::set<unsigned>::iterator iter = elements_containing_intersecting_node.begin();
    unsigned element_a_index = *(iter);
    iter++;
    unsigned element_b_index = *(iter);

    VertexElement<ELEMENT_DIM, SPACE_DIM>* p_element_a = this->GetElement(element_a_index);
    VertexElement<ELEMENT_DIM, SPACE_DIM>* p_element_b = this->GetElement(element_b_index);

    std::set<unsigned> element_a_nodes;
    for (unsigned node_index = 0;  node_index < p_element_a->GetNumNodes(); node_index++)
    {
        element_a_nodes.insert(p_element_a->GetNodeGlobalIndex(node_index));
    }

    std::set<unsigned> element_b_nodes;
    for (unsigned node_index = 0;  node_index < p_element_b->GetNumNodes(); node_index++)
    {
        element_b_nodes.insert(p_element_b->GetNodeGlobalIndex(node_index));
    }

    std::set<unsigned> switching_nodes;
    std::set_intersection(element_a_nodes.begin(), element_a_nodes.end(),
                          element_b_nodes.begin(), element_b_nodes.end(),
                          std::inserter(switching_nodes, switching_nodes.begin()));

    assert(switching_nodes.size() == 2);

    // Check intersecting node is this set
    assert(switching_nodes.find(node_A_index) != switching_nodes.end());
    switching_nodes.erase(node_A_index);

    assert(switching_nodes.size() == 1);

    node_B_index = *(switching_nodes.begin());

    // Now identify elements 2 and 4
    unsigned node_A_local_index_in_a = p_element_a->GetNodeLocalIndex(node_A_index);
    unsigned node_B_local_index_in_a = p_element_a->GetNodeLocalIndex(node_B_index);

    if ((node_B_local_index_in_a+1)%p_element_a->GetNumNodes() == node_A_local_index_in_a)
    {
        assert((p_element_b->GetNodeLocalIndex(node_A_index)+1)%p_element_b->GetNumNodes()
               == p_element_b->GetNodeLocalIndex(node_B_index));

        // Element 2 is element a, element 4 is element b
        element_2_index = element_a_index;
        element_4_index = element_b_index;
    }
    else
    {
        assert((p_element_b->GetNodeLocalIndex(node_B_index)+1)%p_element_b->GetNumNodes()
               == p_element_b->GetNodeLocalIndex(node_A_index));

        // Element 2 is element b, element 4 is element a
        element_2_index = element_b_index;
        element_4_index = element_a_index;
    }

    unsigned intersected_edge = this->GetLocalIndexForElementEdgeClosestToPoint(pNode->rGetLocation(), elementIndex);

    unsigned node_A_local_index_in_1 = this->GetElement(element_1_index)->GetNodeLocalIndex(node_A_index);

    unsigned node_A_local_index_in_2 = this->GetElement(element_2_index)->GetNodeLocalIndex(node_A_index);
    unsigned node_B_local_index_in_2 = this->GetElement(element_2_index)->GetNodeLocalIndex(node_B_index);

    unsigned node_B_local_index_in_3 = this->GetElement(elementIndex)->GetNodeLocalIndex(node_B_index);

    unsigned node_A_local_index_in_4 = this->GetElement(element_4_index)->GetNodeLocalIndex(node_A_index);
    unsigned node_B_local_index_in_4 = this->GetElement(element_4_index)->GetNodeLocalIndex(node_B_index);

    if (intersected_edge==node_B_local_index_in_3)
    {
        /*
         * Add node B to element 1 after node A
         * Add node A to element 3 after node B
         *
         * Remove node B from element 2
         * Remove node A from element 4
         */
        this->mElements[element_1_index]->AddNode(this->mNodes[node_B_index], node_A_local_index_in_1);
        this->mElements[element_3_index]->AddNode(this->mNodes[node_A_index], node_B_local_index_in_3);

        this->mElements[element_2_index]->DeleteNode(node_B_local_index_in_2);
        this->mElements[element_4_index]->DeleteNode(node_A_local_index_in_4);
    }
    else
    {
        assert((intersected_edge+1)%num_nodes==node_B_local_index_in_3);

        /*
         * Add node B to element 1 before node A
         * Add node A to element 3 before node B
         *
         * Remove node A from element 2
         * Remove node B from element 4
         */
        unsigned node_before_A_in_1 = (node_A_local_index_in_1 - 1)%this->GetElement(element_1_index)->GetNumNodes();
        unsigned node_before_B_in_3 = (node_B_local_index_in_3 - 1)%this->GetElement(element_3_index)->GetNumNodes();

        this->mElements[element_1_index]->AddNode(this->mNodes[node_B_index], node_before_A_in_1);
        this->mElements[element_3_index]->AddNode(this->mNodes[node_A_index], node_before_B_in_3);

        this->mElements[element_2_index]->DeleteNode(node_A_local_index_in_2);
        this->mElements[element_4_index]->DeleteNode(node_B_local_index_in_4);
    }
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::PerformT2Swap(VertexElement<ELEMENT_DIM,SPACE_DIM>& rElement)
{
    // Make sure that we are in the correct dimension - this code will be eliminated at compile time
    assert(SPACE_DIM == 2); // only works in 2D at present
    assert(ELEMENT_DIM == SPACE_DIM);

    /*
     * For removing a small triangle
     *
     *  \
     *   \
     *   |\
     *   | \
     *   |  \_ _ _ _ _
     *   |  /
     *   | /
     *   |/
     *   /
     *  /
     *
     * To
     *
     *  \
     *   \
     *    \
     *     \
     *      \_ _ _ _ _
     *      /
     *     /
     *    /
     *   /
     *  /
     *
     */

    // Assert that the triangle element has only three nodes (!)
    assert(rElement.GetNumNodes() == 3u);

    bool is_node_on_boundary = false;
    for (unsigned i=0; i<3; i++)
    {
        if (rElement.GetNode(i)->IsBoundaryNode())
        {
            is_node_on_boundary= true;
        }
    }

    // Create new node at centroid of element which will be a boundary node if any of the existing nodes was on the boundary.
    c_vector<double, SPACE_DIM> new_node_location = this->GetCentroidOfElement(rElement.GetIndex());
    unsigned new_node_global_index = this->AddNode(new Node<SPACE_DIM>(GetNumNodes(), new_node_location, is_node_on_boundary));
    Node<SPACE_DIM>* p_new_node = this->GetNode(new_node_global_index);

    std::set<unsigned> neighbouring_elements;

    for (unsigned i=0; i<3; i++)
    {
        Node<SPACE_DIM>* p_node_a = rElement.GetNode((i+1)%3);
        Node<SPACE_DIM>* p_node_b = rElement.GetNode((i+2)%3);

        std::set<unsigned> elements_of_node_a = p_node_a->rGetContainingElementIndices();
        std::set<unsigned> elements_of_node_b = p_node_b->rGetContainingElementIndices();

        std::set<unsigned> common_elements;

        std::set_intersection( elements_of_node_a.begin(), elements_of_node_a.end(),
                               elements_of_node_b.begin(), elements_of_node_b.end(),
                               std::inserter(common_elements, common_elements.begin()));

        assert(common_elements.size() <= 2u);
        common_elements.erase(rElement.GetIndex());

        if (common_elements.size() == 1u) // there is a neighbouring element
        {
            VertexElement<ELEMENT_DIM,SPACE_DIM>* p_neighbouring_element = this->GetElement(*(common_elements.begin()));

            if (p_neighbouring_element->GetNumNodes() < 4u)
            {
                EXCEPTION("One of the neighbours of a small triangular element is also a triangle - dealing with this has not been implemented yet");
            }

            p_neighbouring_element-> ReplaceNode(p_node_a, p_new_node);
            p_neighbouring_element->DeleteNode(p_neighbouring_element->GetNodeLocalIndex(p_node_b->GetIndex()));
        }
        else
        {
            assert( (p_node_a->IsBoundaryNode()) && (p_node_b->IsBoundaryNode()) );
        }
    }

    // Also have to mark pElement, pElement->GetNode(0), pElement->GetNode(1), and pElement->GetNode(2) as deleted.
    mDeletedNodeIndices.push_back(rElement.GetNodeGlobalIndex(0));
    mDeletedNodeIndices.push_back(rElement.GetNodeGlobalIndex(1));
    mDeletedNodeIndices.push_back(rElement.GetNodeGlobalIndex(2));
    rElement.GetNode(0)->MarkAsDeleted();
    rElement.GetNode(1)->MarkAsDeleted();
    rElement.GetNode(2)->MarkAsDeleted();

    mDeletedElementIndices.push_back(rElement.GetIndex());
    rElement.MarkAsDeleted();
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::PerformT3Swap(Node<SPACE_DIM>* pNode, unsigned elementIndex)
{
    // Make sure that we are in the correct dimension - this code will be eliminated at compile time
    assert(SPACE_DIM == 2); // only works in 2D at present
    assert(ELEMENT_DIM == SPACE_DIM);

    // Check pNode is a boundary node
    assert(pNode->IsBoundaryNode());

    // Store the index of the elements containing the intersecting node
    std::set<unsigned> elements_containing_intersecting_node = pNode->rGetContainingElementIndices();

    // Get the local index of the node in the intersected element after which the new node is to be added
    unsigned node_A_local_index = this->GetLocalIndexForElementEdgeClosestToPoint(pNode->rGetLocation(), elementIndex);

    // Get current node location
    c_vector<double, SPACE_DIM> node_location = pNode->rGetModifiableLocation();

    // Get element
    VertexElement<ELEMENT_DIM, SPACE_DIM>* p_element = this->GetElement(elementIndex);
    unsigned num_nodes = p_element->GetNumNodes();

//   TRACE("intersecting node");
//    PRINT_2_VARIABLES(pNode->GetIndex(),node_location);
//
//
//    for (std::set<unsigned>::const_iterator elem_iter = elements_containing_intersecting_node.begin();
//         elem_iter != elements_containing_intersecting_node.end();
//         elem_iter++)
//
//    {
//        TRACE("intersecting element");
//
//        VertexElement<ELEMENT_DIM, SPACE_DIM>* p_element_intersection = this->GetElement(*elem_iter);
//        PRINT_VARIABLE(p_element_intersection->GetIndex());
//
//        for (unsigned node_index = 0;
//             node_index < p_element_intersection->GetNumNodes();
//             node_index++)
//        {
//            PRINT_3_VARIABLES(p_element_intersection->GetNodeGlobalIndex(node_index),
//                              p_element_intersection->GetNode(node_index)->IsBoundaryNode(),
//                              p_element_intersection->GetNodeLocation(node_index));
//        }
//    }
//    TRACE("intersected element");
//    PRINT_VARIABLE(p_element->GetIndex());
//    for (unsigned node_index = 0;
//         node_index < p_element->GetNumNodes();
//         node_index++)
//    {
//        PRINT_3_VARIABLES(p_element->GetNodeGlobalIndex(node_index),
//                          p_element->GetNode(node_index)->IsBoundaryNode(),
//                          p_element->GetNodeLocation(node_index));
//    }

    // Get the nodes at either end of the edge to be divided
    unsigned vertexA_index = p_element->GetNodeGlobalIndex(node_A_local_index);
    unsigned vertexB_index = p_element->GetNodeGlobalIndex((node_A_local_index+1)%num_nodes);

    // Check these nodes are also boundary nodes if this fails then the elements have become concave and you need a smaller timestep
    if (!this->mNodes[vertexA_index]->IsBoundaryNode() || !this->mNodes[vertexB_index]->IsBoundaryNode())
    {
        EXCEPTION("A boundary node has intersected a non boundary edge, this is because the boundary element has become concave you need to rerun the simulation with a smaller time step to prevent this.");
    }

    // Get the nodes at either end of the edge to be divided and calculate intersection
    c_vector<double, SPACE_DIM> vertexA = p_element->GetNodeLocation(node_A_local_index);
    c_vector<double, SPACE_DIM> vertexB = p_element->GetNodeLocation((node_A_local_index+1)%num_nodes);
    c_vector<double, SPACE_DIM> vector_a_to_point = this->GetVectorFromAtoB(vertexA, node_location);

    c_vector<double, SPACE_DIM> vector_a_to_b = this->GetVectorFromAtoB(vertexA, vertexB);

    c_vector<double, SPACE_DIM> edge_ab_unit_vector = vector_a_to_b/norm_2(vector_a_to_b);
    c_vector<double, SPACE_DIM> intersection = vertexA + edge_ab_unit_vector*inner_prod(vector_a_to_point, edge_ab_unit_vector);

    // Store the location of the T3Swap, the location of the intersection with the edge.
    mLocationsOfT3Swaps.push_back(intersection);

    /*
     * If the edge is shorter than 4.0*mCellRearrangementRatio*mCellRearrangementThreshold move vertexA and vertexB
     * 4.0*mCellRearrangementRatio*mCellRearrangementThreshold apart. \todo investigate if moving A and B causes other issues with nearby nodes.
     *
     * Note: this distance so that there is always enough room for new nodes (if necessary)
     * \todo currently this assumes a worst case scenario of 3 nodes between A and B could be less movement for other cases. #1399
     */
    if (norm_2(vector_a_to_b) < 4.0*mCellRearrangementRatio*mCellRearrangementThreshold)
    {
        WARNING("Trying to merge a node onto an edge which is too small.");

        c_vector<double, SPACE_DIM> centre_a_and_b = vertexA + 0.5*vector_a_to_b;

        vertexA = centre_a_and_b  - 2.0*mCellRearrangementRatio*mCellRearrangementThreshold*vector_a_to_b/norm_2(vector_a_to_b);
        ChastePoint<SPACE_DIM> vertex_A_point(vertexA);
        SetNode(p_element->GetNodeGlobalIndex(node_A_local_index), vertex_A_point);

        vertexB = centre_a_and_b  + 2.0*mCellRearrangementRatio*mCellRearrangementThreshold*vector_a_to_b/norm_2(vector_a_to_b);
        ChastePoint<SPACE_DIM> vertex_B_point(vertexB);
        SetNode(p_element->GetNodeGlobalIndex((node_A_local_index+1)%num_nodes), vertex_B_point);

        // Reset distances
        vector_a_to_b = this->GetVectorFromAtoB(vertexA, vertexB);
        edge_ab_unit_vector = vector_a_to_b/norm_2(vector_a_to_b);

        // Reset the intersection to the middle to allow enough room for new nodes
        intersection = centre_a_and_b;
    }

    /*
     * If the intersection is within mCellRearrangementRatio^2*mCellRearrangementThreshold of vertexA or vertexB move it
     * mCellRearrangementRatio^2*mCellRearrangementThreshold away.
     *
     * Note: this distance so that there is always enough room for new nodes (if necessary).
     * \todo currently this assumes a worst case scenario of 3 nodes between A and B could be less movement for other cases.
     */
    if (norm_2(intersection - vertexA) < 2.0*mCellRearrangementRatio*mCellRearrangementThreshold)
    {
        intersection = vertexA + 2.0*mCellRearrangementRatio*mCellRearrangementThreshold*edge_ab_unit_vector;
    }
    if (norm_2(intersection - vertexB) < 2.0*mCellRearrangementRatio*mCellRearrangementThreshold)
    {
        intersection = vertexB - 2.0*mCellRearrangementRatio*mCellRearrangementThreshold*edge_ab_unit_vector;
    }

    if (pNode->GetNumContainingElements() == 1)
    {
        // Get the index of the element containing the intersecting node
        unsigned intersecting_element_index = *elements_containing_intersecting_node.begin();

        // Get element
        VertexElement<ELEMENT_DIM, SPACE_DIM>* p_intersecting_element = this->GetElement(intersecting_element_index);

        unsigned local_index = p_intersecting_element->GetNodeLocalIndex(pNode->GetIndex());
        unsigned next_node = p_intersecting_element->GetNodeGlobalIndex((local_index + 1)%(p_intersecting_element->GetNumNodes()));
        unsigned previous_node = p_intersecting_element->GetNodeGlobalIndex((local_index + p_intersecting_element->GetNumNodes() - 1)%(p_intersecting_element->GetNumNodes()));

        // Check to see if the nodes adjacent to the intersecting node are contained in the intersected element VertexA and VertexB
        if (next_node == vertexA_index || previous_node == vertexA_index || next_node == vertexB_index || previous_node == vertexB_index)
        {
            unsigned common_vertex_index;

            if (next_node == vertexA_index || previous_node == vertexA_index)
            {
                common_vertex_index = vertexA_index;
            }
            else
            {
                common_vertex_index = vertexB_index;
            }

            assert(this->mNodes[common_vertex_index]->GetNumContainingElements()>1);

            std::set<unsigned> elements_containing_common_vertex = this->mNodes[common_vertex_index]->rGetContainingElementIndices();
            std::set<unsigned>::const_iterator it = elements_containing_common_vertex.begin();
            VertexElement<ELEMENT_DIM, SPACE_DIM>* p_element_common_1 = this->GetElement(*it);
            it++;
            VertexElement<ELEMENT_DIM, SPACE_DIM>* p_element_common_2 = this->GetElement(*it);

            // Calculate the number of common vertices between element_1 and element_2
            unsigned num_common_vertices = 0;
            for (unsigned i=0; i<p_element_common_1->GetNumNodes(); i++)
            {
                for (unsigned j=0; j<p_element_common_2->GetNumNodes(); j++)
                {
                    if (p_element_common_1->GetNodeGlobalIndex(i)==p_element_common_2->GetNodeGlobalIndex(j))
                    {
                        num_common_vertices++;
                    }
                }
            }

            if (num_common_vertices == 1 || this->mNodes[common_vertex_index]->GetNumContainingElements() > 2)
            {
                /*
                 * This is the situation here.
                 *
                 *  From          To
                 *   _             _
                 *    |  <---       |
                 *    |  /\         |\
                 *    | /  \        | \
                 *   _|/____\      _|__\
                 *
                 * The edge goes from vertexA--vertexB to vertexA--pNode--vertexB
                 */

                // Move original node
                pNode->rGetModifiableLocation() = intersection;

                // Add the moved nodes to the element (this also updates the node)
                this->GetElement(elementIndex)->AddNode(pNode, node_A_local_index);

                // Check the nodes are updated correctly
                assert(pNode->GetNumContainingElements() == 2);
            }
            else if (num_common_vertices == 2)
            {
                /*
                 * This is the situation here.
                 *
                 * C is common_vertex D is the other one.
                 *
                 *  From          To
                 *   _ D          _
                 *    | <---       |
                 *    | /\         |\
                 *   C|/  \        | \
                 *   _|____\      _|__\
                 *
                 *  The edge goes from vertexC--vertexB to vertexC--pNode--vertexD
                 *  then VertexB is removed as it is no longer needed.
                 */

                // Move original node
                pNode->rGetModifiableLocation() = intersection;

                // Replace common_vertex with the the moved node (this also updates the nodes)
                this->GetElement(elementIndex)->ReplaceNode(this->mNodes[common_vertex_index], pNode);

                // Remove common_vertex
                unsigned common_vertex_local_index = this->GetElement(intersecting_element_index)->GetNodeLocalIndex(common_vertex_index);
                this->GetElement(intersecting_element_index)->DeleteNode(common_vertex_local_index);
                assert(this->mNodes[common_vertex_index]->GetNumContainingElements() == 0);

                this->mNodes[common_vertex_index]->MarkAsDeleted();
                mDeletedNodeIndices.push_back(common_vertex_index);

                // Check the nodes are updated correctly
                assert(pNode->GetNumContainingElements() == 2);
            }
            else
            {
                // This can't happen as nodes can't be on the internal edge of 2 elements.
                NEVER_REACHED;
            }
        }
        else
        {
            /*
             *  From          To
             *   ____        _______
             *                 / \
             *    /\   ^      /   \
             *   /  \  |
             *
             *  The edge goes from vertexA--vertexB to vertexA--new_node--pNode--vertexB
             */

            // Move original node
            pNode->rGetModifiableLocation() = intersection + 0.5*mCellRearrangementRatio*mCellRearrangementThreshold*edge_ab_unit_vector;

            c_vector<double, SPACE_DIM> new_node_location = intersection - 0.5*mCellRearrangementRatio*mCellRearrangementThreshold*edge_ab_unit_vector;

            // Add new node which will always be a boundary node
            unsigned new_node_global_index = this->AddNode(new Node<SPACE_DIM>(0, true, new_node_location[0], new_node_location[1]));

            // Add the moved and new nodes to the element (this also updates the node)
            this->GetElement(elementIndex)->AddNode(pNode, node_A_local_index);
            this->GetElement(elementIndex)->AddNode(this->mNodes[new_node_global_index], node_A_local_index);

            // Add the new node to the original element containing pNode (this also updates the node)
            this->GetElement(intersecting_element_index)->AddNode(this->mNodes[new_node_global_index], this->GetElement(intersecting_element_index)->GetNodeLocalIndex(pNode->GetIndex()));

            // Check the nodes are updated correctly
            assert(pNode->GetNumContainingElements() == 2);
            assert(this->mNodes[new_node_global_index]->GetNumContainingElements() == 2);
        }
    }
    else if (pNode->GetNumContainingElements() == 2)
    {
        // Find the nodes contained in elements containing the intersecting node
        std::set<unsigned>::const_iterator it = elements_containing_intersecting_node.begin();
        VertexElement<ELEMENT_DIM, SPACE_DIM>* p_element_intersection_1 = this->GetElement(*it);
        it++;
        VertexElement<ELEMENT_DIM, SPACE_DIM>* p_element_intersection_2 = this->GetElement(*it);

        unsigned local_index_1 = p_element_intersection_1->GetNodeLocalIndex(pNode->GetIndex());
        unsigned next_node_1 = p_element_intersection_1->GetNodeGlobalIndex((local_index_1 + 1)%(p_element_intersection_1->GetNumNodes()));
        unsigned previous_node_1 = p_element_intersection_1->GetNodeGlobalIndex((local_index_1 + p_element_intersection_1->GetNumNodes() - 1)%(p_element_intersection_1->GetNumNodes()));
        unsigned local_index_2 = p_element_intersection_2->GetNodeLocalIndex(pNode->GetIndex());
        unsigned next_node_2 = p_element_intersection_2->GetNodeGlobalIndex((local_index_2 + 1)%(p_element_intersection_2->GetNumNodes()));
        unsigned previous_node_2 = p_element_intersection_2->GetNodeGlobalIndex((local_index_2 + p_element_intersection_2->GetNumNodes() - 1)%(p_element_intersection_2->GetNumNodes()));

        // Check to see if the nodes adjacent to the intersecting node are contained in the intersected element VertexA and VertexB
        if ((next_node_1 == vertexA_index || previous_node_1 == vertexA_index || next_node_2 == vertexA_index || previous_node_2 == vertexA_index) &&
               (next_node_1 == vertexB_index || previous_node_1 == vertexB_index || next_node_2 == vertexB_index || previous_node_2 == vertexB_index))
        {
            /*
             * Here we have
             *        __
             *      /|             /
             *  __ / |     --> ___/
             *     \ |            \
             *      \|__           \
             *
             * Where the node on the left has overlapped the edge A B
             *
             * Move p_node to the intersection on A B and merge AB and p_node
             */

            //Check they are all boundary nodes
            assert(pNode->IsBoundaryNode());
            assert(this->mNodes[vertexA_index]->IsBoundaryNode());
            assert(this->mNodes[vertexB_index]->IsBoundaryNode());

            // Move p_node to the intersection with the edge A B
            pNode->rGetModifiableLocation() = intersection;
            pNode->SetAsBoundaryNode(false);

            // Add pNode to the intersected element
            this->GetElement(elementIndex)->AddNode(pNode, node_A_local_index);

            // Remove VertexA from elements
            std::set<unsigned> elements_containing_vertex_A = this->mNodes[vertexA_index]->rGetContainingElementIndices();
            for (std::set<unsigned>::const_iterator iter = elements_containing_vertex_A.begin();
                 iter != elements_containing_vertex_A.end();
                 iter++)
            {
                this->GetElement(*iter)->DeleteNode(this->GetElement(*iter)->GetNodeLocalIndex(vertexA_index));
            }

            // Remove VertexA from the mesh
            assert(this->mNodes[vertexA_index]->GetNumContainingElements()==0);

            this->mNodes[vertexA_index]->MarkAsDeleted();
            mDeletedNodeIndices.push_back(vertexA_index);

            // Remove VertexB from elements
            std::set<unsigned> elements_containing_vertex_B = this->mNodes[vertexB_index]->rGetContainingElementIndices();
            for (std::set<unsigned>::const_iterator iter = elements_containing_vertex_B.begin();
                 iter != elements_containing_vertex_B.end();
                 iter++)
            {
                this->GetElement(*iter)->DeleteNode(this->GetElement(*iter)->GetNodeLocalIndex(vertexB_index));
            }

            // Remove VertexB from the mesh
            assert(this->mNodes[vertexB_index]->GetNumContainingElements()==0);

            this->mNodes[vertexB_index]->MarkAsDeleted();
            mDeletedNodeIndices.push_back(vertexB_index);
        }
        else
        {
            if (next_node_1 == vertexA_index || previous_node_1 == vertexA_index || next_node_2 == vertexA_index || previous_node_2 == vertexA_index)
            {
                // Get elements containing vertexA_index (the Common vertex)

                assert(this->mNodes[vertexA_index]->GetNumContainingElements()>1);

                std::set<unsigned> elements_containing_vertex_A = this->mNodes[vertexA_index]->rGetContainingElementIndices();
                std::set<unsigned>::const_iterator iter = elements_containing_vertex_A.begin();
                VertexElement<ELEMENT_DIM, SPACE_DIM>* p_element_common_1 = this->GetElement(*iter);
                iter++;
                VertexElement<ELEMENT_DIM, SPACE_DIM>* p_element_common_2 = this->GetElement(*iter);

                // Calculate the number of common vertices between element_1 and element_2
                unsigned num_common_vertices = 0;
                for (unsigned i=0; i<p_element_common_1->GetNumNodes(); i++)
                {
                    for (unsigned j=0; j<p_element_common_2->GetNumNodes(); j++)
                    {
                        if (p_element_common_1->GetNodeGlobalIndex(i) == p_element_common_2->GetNodeGlobalIndex(j))
                        {
                            num_common_vertices++;
                        }
                    }
                }

                if (num_common_vertices == 1 || this->mNodes[vertexA_index]->GetNumContainingElements() > 2)
                {
                    /*
                     *  From          To
                     *   _ B              _ B
                     *    |  <---          |
                     *    |   /|\          |\
                     *    |  / | \         | \
                     *    | /  |  \        |\ \
                     *   _|/___|___\      _|_\_\
                     *     A                A
                     *
                     * The edge goes from vertexA--vertexB to vertexA--pNode--new_node--vertexB
                     */

                    // Move original node and change to non-boundary node
                    pNode->rGetModifiableLocation() = intersection - 0.5*mCellRearrangementRatio*mCellRearrangementThreshold*edge_ab_unit_vector;
                    pNode->SetAsBoundaryNode(false);

                    c_vector<double, SPACE_DIM> new_node_location = intersection + 0.5*mCellRearrangementRatio*mCellRearrangementThreshold*edge_ab_unit_vector;

                    // Add new node which will always be a boundary node
                    unsigned new_node_global_index = this->AddNode(new Node<SPACE_DIM>(0, true, new_node_location[0], new_node_location[1]));

                    // Add the moved nodes to the element (this also updates the node)
                    this->GetElement(elementIndex)->AddNode(this->mNodes[new_node_global_index], node_A_local_index);
                    this->GetElement(elementIndex)->AddNode(pNode, node_A_local_index);

                    // Add the new nodes to the original elements containing pNode (this also updates the node)
                    if (next_node_1 == previous_node_2)
                    {
                        p_element_intersection_1->AddNode(this->mNodes[new_node_global_index], (local_index_1 + p_element_intersection_1->GetNumNodes() - 1)%(p_element_intersection_1->GetNumNodes()));
                    }
                    else
                    {
                        assert(next_node_2 == previous_node_1);

                        p_element_intersection_2->AddNode(this->mNodes[new_node_global_index], (local_index_2 + p_element_intersection_2->GetNumNodes() - 1)%(p_element_intersection_2->GetNumNodes()));
                    }

                    // Check the nodes are updated correctly
                    assert(pNode->GetNumContainingElements() == 3);
                    assert(this->mNodes[new_node_global_index]->GetNumContainingElements() == 2);
                }
                else if (num_common_vertices == 2)
                {
                    /*
                     *  From          To
                     *   _ B              _ B
                     *    |<---          |
                     *    | /|\          |\
                     *    |/ | \         | \
                     *    |  |  \        |\ \
                     *   _|__|___\      _|_\_\
                     *     A              A
                     *
                     * The edge goes from vertexA--vertexB to vertexA--pNode--new_node--vertexB
                     * then vertexA is removed
                     */

                    // Move original node and change to non-boundary node
                    pNode->rGetModifiableLocation() = intersection - 0.5*mCellRearrangementRatio*mCellRearrangementThreshold*edge_ab_unit_vector;
                    pNode->SetAsBoundaryNode(false);

                     c_vector<double, SPACE_DIM> new_node_location = intersection + 0.5*mCellRearrangementRatio*mCellRearrangementThreshold*edge_ab_unit_vector;

                    // Add new node which will always be a boundary node
                    unsigned new_node_global_index = this->AddNode(new Node<SPACE_DIM>(0, true, new_node_location[0], new_node_location[1]));

                    // Add the moved nodes to the element (this also updates the node)
                    this->GetElement(elementIndex)->AddNode(this->mNodes[new_node_global_index], node_A_local_index);
                    this->GetElement(elementIndex)->AddNode(pNode, node_A_local_index);

                    // Add the new nodes to the original elements containing pNode (this also updates the node)
                    if (next_node_1 == previous_node_2)
                    {
                        p_element_intersection_1->AddNode(this->mNodes[new_node_global_index], (local_index_1 + p_element_intersection_1->GetNumNodes() - 1)%(p_element_intersection_1->GetNumNodes()));
                    }
                    else
                    {
                        assert(next_node_2 == previous_node_1);

                        p_element_intersection_2->AddNode(this->mNodes[new_node_global_index], (local_index_2 + p_element_intersection_2->GetNumNodes() - 1)%(p_element_intersection_2->GetNumNodes()));
                    }

                    // Remove VertexA from the mesh
                    p_element_common_1->DeleteNode(p_element_common_1->GetNodeLocalIndex(vertexA_index));
                    p_element_common_2->DeleteNode(p_element_common_2->GetNodeLocalIndex(vertexA_index));

                    assert(this->mNodes[vertexA_index]->GetNumContainingElements()==0);

                    this->mNodes[vertexA_index]->MarkAsDeleted();
                    mDeletedNodeIndices.push_back(vertexA_index);

                    // Check the nodes are updated correctly
                    assert(pNode->GetNumContainingElements() == 3);
                    assert(this->mNodes[new_node_global_index]->GetNumContainingElements() == 2);
                }
                else
                {
                    // This can't happen as nodes can't be on the internal edge of 2 elements.
                    NEVER_REACHED;
                }

            }
            else if (next_node_1 == vertexB_index || previous_node_1 == vertexB_index || next_node_2 == vertexB_index || previous_node_2 == vertexB_index)
            {
                // Get elements containing vertexB_index (the Common vertex)

                assert(this->mNodes[vertexB_index]->GetNumContainingElements()>1);

                std::set<unsigned> elements_containing_vertex_B = this->mNodes[vertexB_index]->rGetContainingElementIndices();
                std::set<unsigned>::const_iterator iter = elements_containing_vertex_B.begin();
                VertexElement<ELEMENT_DIM, SPACE_DIM>* p_element_common_1 = this->GetElement(*iter);
                iter++;
                VertexElement<ELEMENT_DIM, SPACE_DIM>* p_element_common_2 = this->GetElement(*iter);

                // Calculate the number of common vertices between element_1 and element_2
                unsigned num_common_vertices = 0;
                for (unsigned i=0; i<p_element_common_1->GetNumNodes(); i++)
                {
                    for (unsigned j=0; j<p_element_common_2->GetNumNodes(); j++)
                    {
                        if (p_element_common_1->GetNodeGlobalIndex(i)==p_element_common_2->GetNodeGlobalIndex(j))
                        {
                            num_common_vertices++;
                        }
                    }
                }

                if (num_common_vertices == 1 || this->mNodes[vertexB_index]->GetNumContainingElements() > 2)
                {
                    /*
                     *  From          To
                     *   _B_________      _B____
                     *    |\   |   /       | / /
                     *    | \  |  /        |/ /
                     *    |  \ | /         | /
                     *    |   \|/          |/
                     *   _|   <---        _|
                     *    A
                     *
                     * The edge goes from vertexA--vertexB to vertexA--new_node--pNode--vertexB
                     */

                    // Move original node and change to non-boundary node
                    pNode->rGetModifiableLocation() = intersection + 0.5*mCellRearrangementRatio*mCellRearrangementThreshold*edge_ab_unit_vector;
                    pNode->SetAsBoundaryNode(false);

                    c_vector<double, SPACE_DIM> new_node_location = intersection - 0.5*mCellRearrangementRatio*mCellRearrangementThreshold*edge_ab_unit_vector;

                    // Add new node which will always be a boundary node
                    unsigned new_node_global_index = this->AddNode(new Node<SPACE_DIM>(0, true, new_node_location[0], new_node_location[1]));

                    // Add the moved nodes to the element (this also updates the node)
                    this->GetElement(elementIndex)->AddNode(pNode, node_A_local_index);
                    this->GetElement(elementIndex)->AddNode(this->mNodes[new_node_global_index], node_A_local_index);

                    // Add the new nodes to the original elements containing pNode (this also updates the node)
                    if (next_node_1 == previous_node_2)
                    {
                        p_element_intersection_2->AddNode(this->mNodes[new_node_global_index], local_index_2);
                    }
                    else
                    {
                        assert(next_node_2 == previous_node_1);

                        p_element_intersection_1->AddNode(this->mNodes[new_node_global_index], local_index_1);
                    }

                    // Check the nodes are updated correctly
                    assert(pNode->GetNumContainingElements() == 3);
                    assert(this->mNodes[new_node_global_index]->GetNumContainingElements() == 2);
                }
                else if (num_common_vertices == 2)
                {
                    /*
                     *  From          To
                     *   _B_______      _B____
                     *    |  |   /       | / /
                     *    |  |  /        |/ /
                     *    |\ | /         | /
                     *    | \|/          |/
                     *   _| <---        _|
                     *    A
                     *
                     * The edge goes from vertexA--vertexB to vertexA--new_node--pNode--vertexB
                     * then vertexB is removed
                     */

                    // Move original node and change to non-boundary node
                    pNode->rGetModifiableLocation() = intersection + 0.5*mCellRearrangementRatio*mCellRearrangementThreshold*edge_ab_unit_vector;
                    pNode->SetAsBoundaryNode(false);

                    c_vector<double, SPACE_DIM> new_node_location = intersection - 0.5*mCellRearrangementRatio*mCellRearrangementThreshold*edge_ab_unit_vector;

                    // Add new node which will always be a boundary node
                    unsigned new_node_global_index = this->AddNode(new Node<SPACE_DIM>(0, true, new_node_location[0], new_node_location[1]));

                    // Add the moved nodes to the element (this also updates the node)
                    this->GetElement(elementIndex)->AddNode(pNode, node_A_local_index);
                    this->GetElement(elementIndex)->AddNode(this->mNodes[new_node_global_index], node_A_local_index);

                    // Add the new nodes to the original elements containing pNode (this also updates the node)
                    if (next_node_1 == previous_node_2)
                    {
                        p_element_intersection_2->AddNode(this->mNodes[new_node_global_index], local_index_2);
                    }
                    else
                    {
                        assert(next_node_2 == previous_node_1);

                        p_element_intersection_1->AddNode(this->mNodes[new_node_global_index], local_index_1);
                    }

                    // Remove VertexB from the mesh
                    p_element_common_1->DeleteNode(p_element_common_1->GetNodeLocalIndex(vertexB_index));
                    p_element_common_2->DeleteNode(p_element_common_2->GetNodeLocalIndex(vertexB_index));

                    assert(this->mNodes[vertexB_index]->GetNumContainingElements()==0);

                    this->mNodes[vertexB_index]->MarkAsDeleted();
                    mDeletedNodeIndices.push_back(vertexB_index);

                    // Check the nodes are updated correctly
                    assert(pNode->GetNumContainingElements() == 3);
                    assert(this->mNodes[new_node_global_index]->GetNumContainingElements() == 2);
                }
                else
                {
                    // This can't happen as nodes can't be on the internal edge of 2 elements.
                    NEVER_REACHED;
                }
            }
            else
            {
                /*
                 *  From          To
                 *   _____         _______
                 *                  / | \
                 *    /|\   ^      /  |  \
                 *   / | \  |
                 *
                 * The edge goes from vertexA--vertexB to vertexA--new_node_1--pNode--new_node_2--vertexB
                 */

                // Move original node and change to non-boundary node
                pNode->rGetModifiableLocation() = intersection;
                pNode->SetAsBoundaryNode(false);

                c_vector<double, SPACE_DIM> new_node_1_location = intersection - mCellRearrangementRatio*mCellRearrangementThreshold*edge_ab_unit_vector;
                c_vector<double, SPACE_DIM> new_node_2_location = intersection + mCellRearrangementRatio*mCellRearrangementThreshold*edge_ab_unit_vector;

                // Add new nodes which will always be boundary nodes
                unsigned new_node_1_global_index = this->AddNode(new Node<SPACE_DIM>(0, true, new_node_1_location[0], new_node_1_location[1]));
                unsigned new_node_2_global_index = this->AddNode(new Node<SPACE_DIM>(0, true, new_node_2_location[0], new_node_2_location[1]));

                // Add the moved and new nodes to the element (this also updates the node)
                this->GetElement(elementIndex)->AddNode(this->mNodes[new_node_2_global_index], node_A_local_index);
                this->GetElement(elementIndex)->AddNode(pNode, node_A_local_index);
                this->GetElement(elementIndex)->AddNode(this->mNodes[new_node_1_global_index], node_A_local_index);

                // Add the new nodes to the original elements containing pNode (this also updates the node)
                if (next_node_1 == previous_node_2)
                {
                    p_element_intersection_1->AddNode(this->mNodes[new_node_2_global_index], (local_index_1 + p_element_intersection_1->GetNumNodes() - 1)%(p_element_intersection_1->GetNumNodes()));
                    p_element_intersection_2->AddNode(this->mNodes[new_node_1_global_index], local_index_2);
                }
                else
                {
                    assert(next_node_2 == previous_node_1);

                    p_element_intersection_1->AddNode(this->mNodes[new_node_1_global_index], local_index_1);
                    p_element_intersection_2->AddNode(this->mNodes[new_node_2_global_index], (local_index_2 + p_element_intersection_2->GetNumNodes() - 1)%(p_element_intersection_2->GetNumNodes()));
                }

                // Check the nodes are updated correctly
                assert(pNode->GetNumContainingElements() == 3);
                assert(this->mNodes[new_node_1_global_index]->GetNumContainingElements() == 2);
                assert(this->mNodes[new_node_2_global_index]->GetNumContainingElements() == 2);
            }
        }
    }
    else
    {
        EXCEPTION("Trying to merge a node, contained in more than 2 elements, into another element, this is not possible with the vertex mesh.");
    }
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void MutableVertexMesh<ELEMENT_DIM, SPACE_DIM>::PerformVoidRemoval(Node<SPACE_DIM>* pNodeA, Node<SPACE_DIM>* pNodeB, Node<SPACE_DIM>* pNodeC)
{
    unsigned nodeA_index = pNodeA->GetIndex();
    unsigned nodeB_index = pNodeB->GetIndex();
    unsigned nodeC_index = pNodeC->GetIndex();

    c_vector<double, SPACE_DIM> nodes_midpoint = pNodeA->rGetLocation() + this->GetVectorFromAtoB(pNodeA->rGetLocation(), pNodeB->rGetLocation())/3.0
                                                                        + this->GetVectorFromAtoB(pNodeA->rGetLocation(), pNodeC->rGetLocation())/3.0;

    Node<SPACE_DIM>* p_low_node_A_B = (nodeA_index < nodeB_index) ? pNodeA : pNodeB; // Node with the lowest index out of A and B
    Node<SPACE_DIM>* p_low_node = (p_low_node_A_B->GetIndex() < nodeC_index) ? p_low_node_A_B : pNodeC; // Node with the lowest index out of A, B and C.
    PerformNodeMerge(pNodeA,pNodeB);
    PerformNodeMerge(p_low_node_A_B,pNodeC);

    c_vector<double, SPACE_DIM>& r_low_node_location = p_low_node->rGetModifiableLocation();

    r_low_node_location = nodes_midpoint;

    // Sort out boundary nodes
    p_low_node->SetAsBoundaryNode(false);

    // Remove the deleted nodes and re-index
    RemoveDeletedNodes();
}

// Explicit instantiation

template class MutableVertexMesh<1,1>;
template class MutableVertexMesh<1,2>;
template class MutableVertexMesh<1,3>;
template class MutableVertexMesh<2,2>;
template class MutableVertexMesh<2,3>;
template class MutableVertexMesh<3,3>;

// Serialization for Boost >= 1.36
#include "SerializationExportWrapperForCpp.hpp"
EXPORT_TEMPLATE_CLASS_ALL_DIMS(MutableVertexMesh);