C:/cmcintos/defOrgs/source/physical/Phys_Euler.cxx

#ifndef _PHYS_EULER_CXX
#define _PHYS_EULER_CXX

#include "Phys_Euler.h"
#include "itkImageRegionIterator.h"
namespace mial
{

        template<class DataType, class TGradientImage, int nDims,class MType, class VType>
        Phys_Euler<DataType, TGradientImage, nDims,MType,VType>
                ::Phys_Euler(int numNodes ,int numSprings,int numPossibleDeformations,int defK)
                :Physics<DataType,nDims,MType,VType>()
        {
                nodes.set_size(numNodes,nDims);

                nodesV.set_size(numNodes,nDims);

                nodesF.set_size(numNodes,nDims);

                nodesA.set_size(numNodes,nDims);

                nodesM.set_size(numNodes);

                springsRest.set_size(numSprings);

                springsDamp.set_size(numSprings);

                springsNodes.set_size(numSprings,2);

                springLengths.set_size(numSprings);

                springsK.set_size(numSprings);

                timeStep = 0.005;

                defaultK = defK;
                defaultDamp = 1;
                defaultMass = 1;

                defaultDrag = 1;

                gradientPointer = GradientImageType::New();

                imageForces = true;
        }


        template<class DataType, class TGradientImage, int nDims,class MType, class VType>
        void Phys_Euler<DataType, TGradientImage, nDims,MType,VType>::setRestLengths(int* a, DataType* values, int n)
        {
                if(this->geom->didTopologyChange())
                        updateSpringsFromGeometric();
                for(int i=0; i<n; i++)
                {
                        springsRest(a[i]) = values[i];
                }
        }



        template<class DataType, class TGradientImage, int nDims,class MType, class VType>
        void Phys_Euler<DataType, TGradientImage, nDims,MType,VType>::setRestLengths(VectorType a, VectorType values)
        {
                if(this->geom->didTopologyChange())
                        updateSpringsFromGeometric();
                for(int i=0; i<a.size(); i++)
                {
                        springsRest(a(i)) = values(i);
                }
        }


        template<class DataType, class TGradientImage, int nDims,class MType, class VType>
        void Phys_Euler<DataType, TGradientImage, nDims,MType,VType>::setSpringLengths(int* a, DataType* values, int n)
        {// array a is an array of index values, and array values are the values to set them too.

                if(this->geom->didTopologyChange())
                        updateSpringsFromGeometric();
                for(int i=0; i<n; i++)
                {
                        springLengths(a[i]) = values[i];
                }
        }


        template<class DataType, class TGradientImage, int nDims,class MType, class VType>
        void Phys_Euler<DataType, TGradientImage, nDims,MType,VType>::setSpringLengths(VectorType a, VectorType values)
        {
                if(this->geom->didTopologyChange())
                        updateSpringsFromGeometric();
                for(int i=0; i<a.size(); i++)
                {
                        springLengths(a(i)) = values(i);
                }
        }


        template<class DataType, class TGradientImage, int nDims,class MType, class VType>
        void Phys_Euler<DataType, TGradientImage, nDims,MType,VType>::setSpringsK(int* a, DataType* values, int n)
        {// array a is an array of index values, and array values are the values to set them too.

                if(this->geom->didTopologyChange())
                        updateSpringsFromGeometric();
                for(int i=0; i<n; i++)
                {
                        springsK(a[i]) = values[i];
                }
        }


        template<class DataType, class TGradientImage, int nDims,class MType, class VType>
        void Phys_Euler<DataType, TGradientImage, nDims,MType,VType>::setSpringsK(VectorType a, VectorType values)
        {
                if(this->geom->didTopologyChange())
                        updateSpringsFromGeometric();
                for(int i=0; i<a.size(); i++)
                {
                        springsK(a(i)) = values(i);
                }
        }

        template<class DataType, class TGradientImage, int nDims,class MType, class VType>
        bool Phys_Euler<DataType, TGradientImage, nDims,MType,VType>::runDeformation(const std::string defName,typename DeformationType::deformationIn* const arg ,std::stringstream * const stream)
        {
                //TODO make this more efficient--only update if change, only update new springs etc
                nodes = this->geom->getMatrixNodePositions();
                if(this->geom->didTopologyChange())
                        updateSpringsFromGeometric();
                if(this->geom->didNodesChange())
                {
                        nodesF.set_size(this->geom->getNumNodes(),nDims);
                        nodesF.fill(0);
                        nodesA.set_size(this->geom->getNumNodes(),nDims);
                        nodesA.fill(0);
                        nodesV.set_size(this->geom->getNumNodes(),nDims);
                        nodesV.fill(0);
                        nodesFDef.set_size(this->geom->getNumNodes(),nDims);
                        nodesFDef.fill(0);
                        nodesM.set_size(this->geom->getNumNodes());
                        nodesM.fill(defaultMass);
                }

                //std::string defName;
                //args >> defName; //read the first string from the stream
                // TODO: replace O(n) search with lg(n) search on sorted list.
                for(int i=0; i< this->numDeformations; i++)
                {
                        if( defName.compare(this->deformationsList[i]->getName()) == 0 )
                        {
                                try
                                {
                                        typename DeformationType::DefArgSet org;
                                        org.nodes = &nodes;
                                        org.nodesV = &nodesV;
                                        org.nodesF = &nodesF;
                                        org.nodesFDef = &nodesFDef;
                                        org.springsRest = &springsRest;
                                        org.springsNodes = &springsNodes;
                                        org.springLengths = &springLengths;

                                        this->deformationsList[i]->run(arg,&org,stream);
                                        break;
                                }catch( typename DeformationType::Error * de)
                                {
                                        std::cerr << "Could not complete deformation" << std::endl;
                                        Error e;
                                        e.msg = "Could not complete deformation";
                                        e.deformationNumber = i;
                                        e.deformationError = de;
                                        throw & e;
                                }
                        }
                }
                return false;

        }
        template<class DataType, class TGradientImage, int nDims,class MType, class VType>
        void Phys_Euler<DataType, TGradientImage, nDims,MType,VType>::updateSpringsFromGeometric()
        {
                unsigned int length = springsNodes.rows();
                if(this->geom->getNumConnections() != length) //if there are new springs
                {
                        unsigned int newLength = this->geom->getNumConnections();

                        springsRest.set_size(newLength);
                        springsDamp.set_size(newLength);
                        springLengths.set_size(newLength);
                        springsK.set_size(newLength);
                        springsNodes.set_size(newLength,2);

                        this->springsNodes = this->geom->getMatrixConnections();
                        int from;
                        int to;
                        for(int i=0; i<newLength; i++)
                        {
                                from = springsNodes(i,0);
                                to = springsNodes(i,1);
                                if(DEBUG)
                                        std::cout << "from: " << from << " to: " << to << " i " << i << std::endl;
                                springsRest(i) = (nodes.get_row(from)-nodes.get_row(to)).two_norm();
                                springsDamp(i) = defaultDamp;
                                springLengths(i) = (nodes.get_row(from)-nodes.get_row(to)).two_norm();
                                springsK(i) =defaultK;
                        }

                        // TODO write method for dynamically changing topology that preserves current settings
                        // This will need to search the set of current connections for existing springs and find those
                        // still in use
                        /*
                        //Store current values in a temp variable

                        vnl_Vector tmp(length*4);

                        tmp.update(springsRest,0);
                        tmp.update(springsDamp, length);
                        tmp.update(springLengths, length*2);
                        tmp.update(springsK, length*3);

                        vnl_matrix<int> tmp2(length,2);
                        tmp2.update(springsNodes,0,0);

                        //Increase the size
                        springsRest.set_size(newLength);
                        springsDamp.set_size(newLength);
                        springLengths.set_size(newLength);
                        springsK.set_size(newLength);
                        springsNodes.set_size(newLength,2);

                        //Copy back the old values
                        for(int i =0; i< length; i++)
                        {
                        springsRest(i) = tmp(i);

                        //springsDamp.update(tmp.get_n_rows(nodes.rows(),nodes.rows()) ,0);
                        springsDamp(i) = tmp(i+length);

                        //springLengths.update(tmp.get_n_rows(nodes.rows()*2,nodes.rows()) ,0);
                        springLengths(i) = tmp(i+2*length);

                        //springsK.update(tmp.get_n_rows(nodes.rows()*3,nodes.rows()) ,0);
                        springsK(i) = tmp(i+3*length);

                        }
                        springsNodes.update(tmp2,0);

                        //Add the new springs
                        int from =0;
                        int to =0;
                        for(int i=length; i<newLength; i++)
                        {
                        from = 0;
                        springsRest(i) = (nodes.get_row(from)-nodes.get_row(to)).two_norm();
                        springsDamp(i) = defaultDamp;

                        springLengths(length) = (nodes.get_row(from)-nodes.get_row(to)).two_norm();

                        springsK(length) =defaultK;

                        springsNodes(length,0) = from;
                        springsNodes(length,1) = to;*/
                }
        }

        template<class DataType, class TGradientImage, int nDims,class MType, class VType>
        bool Phys_Euler<DataType, TGradientImage, nDims,MType,VType>::simulate()
        {
                //**************************//
                //Simulate the spring forces
                //**************************//

                //std::cout << "Begin deform" << std::endl;
                //TODO make this more efficient--only update if change, only update new springs etc
                nodes = this->geom->getMatrixNodePositions();
                if(this->geom->didTopologyChange())
                        updateSpringsFromGeometric();
                if(this->geom->didNodesChange())
                {
                        nodesF.set_size(this->geom->getNumNodes(),nDims);
                        nodesF.fill(0);
                        nodesA.set_size(this->geom->getNumNodes(),nDims);
                        nodesA.fill(0);
                        nodesV.set_size(this->geom->getNumNodes(),nDims);
                        nodesV.fill(0);
                        nodesFDef.set_size(this->geom->getNumNodes(),nDims);
                        nodesFDef.fill(0);
                        nodesM.set_size(this->geom->getNumNodes());
                        nodesM.fill(defaultMass);
                }

                const unsigned int length = springsNodes.rows();

                VectorType nodeDist(nDims);
                DataType distNorm;
                VectorType velDiff(nDims);
                DataType D(length);
                VectorType F(nDims);
                VectorType pointForce(nDims);
                VectorType tmpVel(nDims);
                VectorType tmpDist(nDims);
                VectorType tmpPos(nDims);
                //Can't seem to get working with length
                /*      vnl_matrix_fixed<DataType, 4 ,nDims> nodeDist;
                vnl_vector_fixed<DataType,length> distNorm;
                vnl_matrix_fixed<DataType, 4,nDims> velDiff;
                vnl_vector_fixed<DataType, 4> D;
                vnl_matrix_fixed<DataType, 4, nDims> F;*/
                int a,b;
                DataType rowSum = 0;
                int count = 0;
                int runTime = 25;
                int numNodes = nodesF.rows();
                typename GradientImageType::RegionType gradientImageRegion = gradientPointer->GetLargestPossibleRegion();
                itk::ImageRegionIterator<GradientImageType> gradIT(gradientPointer,gradientImageRegion);
                typename GradientImageType::IndexType gradIndex;

                typedef itk::VectorLinearInterpolateImageFunction< GradientImageType, DataType > InterpolatorType;
                typename InterpolatorType::Pointer interp = InterpolatorType::New();
                typename InterpolatorType::OutputType imgForce;
                interp->SetInputImage(gradientPointer);

                //      springsRest(0) = 15;
                bool incTime = true;
                DataType dispLength =0;
                DataType localTimeStep = timeStep;

                DataType endTime = this->time+0.5;
                //while(count<runTime)
                while(this->time< endTime)
                {
                        nodesF.fill(0);
                        if(count==0)
                        {
                                nodesF = nodesF + nodesFDef; //Add on any deformation forces, then zero them
                                nodesFDef.fill(0);
                        }
                        //Loop over all springs
                        for(int i=0; i<length ;i++)
                        {

                                //TODO: Finalize code, vectorize or loop

                                //A = nodesXYZ(springsNodes(:,2),:)  -  nodesXYZ(springsNodes(:,1),:);
                                //B = sum(nodeDist.^2,2).^0.5; //%norm(Xa-Xb) for all springs (numSprings*1)
                                //C = nodesV(springsNodes(:,2),:)  -  nodesV(springsNodes(:,1),:);
                                nodeDist = nodes.get_row(springsNodes(i,1)) - nodes.get_row(springsNodes(i,0));
                                distNorm = nodeDist.two_norm();
                                velDiff = nodesV.get_row(springsNodes(i,1)) - nodesV.get_row(springsNodes(i,0));

                                //D = sum(C.*nodeDist,2) ./ B ;// %[(Va-Vb)'(Xa-Xb)]/norm(Xa-Xb)  for all springs (numSprings*1) 
                                for(int j=0; j<nDims;j++)
                                {
                                        rowSum+= velDiff(j)*nodeDist(j);
                                }
                                D = rowSum/distNorm;
                                rowSum =0;

                                //F = repmat((springsK.*(B-springsRest)+springsDamp.*D)./B , [1 3]).*nodeDist; //%spring forces for all springs (numSprings*2)
                                F = (( (springsK(i)* (distNorm-springsRest(i)) + springsDamp(i)*D)/ distNorm ) )* nodeDist;

                                /*      for k=1:numSprings,  // %assign spring forces to nodes
                                a=springsNodes(k,1);
                                b=springsNodes(k,2);
                                nodesF(a,:)=nodesF(a,:) + F(k,:);
                                nodesF(b,:)=nodesF(b,:) - F(k,:);
                                end*/

                                a = springsNodes(i,0);
                                b = springsNodes(i,1);
                                nodesF.set_row(a,nodesF.get_row(a)+ F);
                                nodesF.set_row(b,nodesF.get_row(b)- F);
                        }
                        //Loop over all nodes
                        for(int i=0; i<numNodes; i++)
                        {
                                //**************************//
                                //Calculate Image forces
                                //**************************//
                                //nodesF=nodesF+ repmat(nodesExt.* nodesBnd , [1,2]) .* [...
                                //     diag(Gx(round(nodesXY(:,2)),round(nodesXY(:,1)))) , ...
                                //   diag(Gy(round(nodesXY(:,2)),round(nodesXY(:,1)))) ];

                                if(DEBUG)
                                {       std::cout << "node: " << i << " x: " << nodes(i,0) << " y: " << nodes(i,1) << " z: " << nodes(i,2) << std::endl;
                                std::cout << "Force: " << i << " x: " << nodesF(i,0) << " y: " << nodesF(i,1) << " z: " << nodesF(i,2) << std::endl;
                                }

                                if(imageForces)
                                {
                                        for(int a =0; a<nDims; a++)
                                        {gradIndex[a] = nodes(i,a);
                                        }

                                        //pointForce(0) = interp->EvaluateAtIndex(gradIndex);
                                        imgForce = interp->EvaluateAtIndex(gradIndex);

                                        for(int a =0; a<nDims; a++)
                                        {pointForce(a) = imgForce[a];
                                        }
                                        //pointForce(1) = interp->EvaluateAtIndex(gradIndex);

                                        nodesF.set_row(i, nodesF.get_row(i) +  pointForce);
                                }

                                if(DEBUG)
                                        std::cout << "Force: " << i << " x: " << nodesF(i,0) << " y: " << nodesF(i,1) << " z: " << nodesF(i,2) << std::endl;

                                //std::cout << "Force: " << i << " x: " << 100*pointForce(0) << " y: " << 100*pointForce(1) << " z: " << std::endl;

                                //**************************//
                                //Drag Force
                                //**************************//
                                nodesF.set_row(i, nodesF.get_row(i)-nodesV.get_row(i)*defaultDrag); 

                                //**************************//
                                //Apply forces to model
                                //**************************//
                                //nodesA=nodesF ./ (repmat(nodesM,[1,3]));%accelaration
                                //nodesV=DT * nodesA + nodesV;%new velocity 
                                //nodesXYZ=(DT * nodesV).*repmat(1-nodesFxd,[1,3]) + nodesXYZ;%new position 

                                nodesA.set_row(i, nodesF.get_row(i)/nodesM(i) );
                                if(DEBUG)
                                        std::cout << "Accel: " << i << " x: " << nodesA(i,0) << " y: " << nodesA(i,1) << " z: " << std::endl;

                                //TODO: This method of time adjustment may actually slow down time for some nodes and not others. Fix that.
                                //TODO: Possible solution: Build a tmp matrix of new velocities, stop and update if time changes.
                                while(incTime)
                                {
                                        tmpVel = (localTimeStep*nodesA.get_row(i) + nodesV.get_row(i));
                                        tmpDist = (localTimeStep * tmpVel );

                                        if(DEBUG)
                                        {       std::cout << "Dist: " << i << " x: " << tmpDist(0) << " y: " << tmpDist(1) << " z: " << tmpDist(2) << std::endl;
                                        std::cout << "Vel: " << i << " x: " << nodesV(i,0) << " y: " << nodesV(i,1) << " z: " << nodesV(i,2) << std::endl;
                                        }

                                        //Check if velocity passes thresh
                                        dispLength = abs(tmpDist.two_norm());
                                        if(  dispLength< 1 )
                                        {
                                                incTime = false;
                                                //Make sure it doesn't push the node outside of the boundary
                                                if(BOUND_CHECKING)
                                                {
                                                        tmpPos = tmpDist+nodes.get_row(i);
                                                        //TODO: Is there a way without converting to an itk index?
                                                        for(int a =0; a<nDims; a++)
                                                                gradIndex[a] = tmpPos(a);
                                                        if(gradientImageRegion.IsInside(gradIndex))
                                                        {
                                                                nodesV.set_row(i, tmpVel );             
                                                                nodes.set_row(i,tmpPos);
                                                        }
                                                }
                                                else
                                                {
                                                        nodesV.set_row(i, tmpVel );                     
                                                        nodes.set_row(i,  tmpDist + nodes.get_row(i) );
                                                }
                                        }
                                        else //shrink the time step and recalc.
                                        {
                                                //std::cout << "DT : " << localTimeStep << std::endl;
                                                localTimeStep = localTimeStep/2;
                                        }
                                }
                                incTime = true;
                        }
                        count++;
                        //Update the time
                        this->time = this->time+localTimeStep;
                }
                this->geom->setMatrixNodePositions(nodes);
                //std::cout << "End deform" << std::endl;
                std::cout << "Clock: " << this->time << std::endl;
                return 1;
        }

} // end namespace mial

#endif

---