C:/cmcintos/defOrgs/examples/vesselCrawler/source/Beh_Growing.cxx

#ifndef _Beh_Growing_txx
#define _Beh_Growing_txx

#include "Beh_Growing.h"
#include "vnl_cross.h"
#include "vnl_math.h"
#include "algo\vnl_matrix_inverse.h"
#define PI 3.14159265

namespace mial {

        template<class Type,class CrawlerPhysType>
        Beh_Growing<Type,CrawlerPhysType>::Beh_Growing()
                :Behavior<Type,3>("Beh_Growing"), nDims(3)
        {
                //Default constructor
        }
        template<class Type,class CrawlerPhysType>
        bool Beh_Growing<Type,CrawlerPhysType>::run(typename Behavior<Type,3>::behaviorIn * i, std::stringstream *s)
        {
                //function [pointsXYZ,newAxis] = growTo(nodesXYZ,newAxis,pointXYZ,nodeNum,bifFound,radius,numNodesPerLayer,growDistance);
                behaviorIn * in;
                if(i != NULL)
                        in = (reinterpret_cast<behaviorIn *>(i)); //Typecast the input to its desired form
                else
                {
                        Error e;
                        e.msg = "Stream call not supported";
                        e.name = "Beh_Growing";
                        throw e;
                }
                //Get the new set of nodes
                MatrixType newNodes = growTo(in);

                VectorType classes(in->vesselGeom->getNumNodesPerLayer());
                classes.fill(1);//Boundary nodes
                classes(0) = 0;//Medial nodes
                in->vesselGeom->addLayer(newNodes,classes);
                
                //Set springs rest.
                //TODO get params from gui
                int springsKDef = 3;
                VectorType acSprings = in->vesselGeom->getActiveSprings(-1);
                VectorType vals = acSprings;
                vals.fill(springsKDef);
                (reinterpret_cast<CrawlerPhysType *>(physLayer))->setSpringsK(acSprings,vals);

                acSprings = in->vesselGeom->getActiveSprings(1);
                vals = acSprings;
                vals.fill(0.5*springsKDef);
                (reinterpret_cast<CrawlerPhysType *>(physLayer))->setSpringsK(acSprings,vals);
        }
        template<class Type,class CrawlerPhysType>
        typename Beh_Growing<Type,CrawlerPhysType>::MatrixType Beh_Growing<Type,CrawlerPhysType>::growTo(behaviorIn* in)
        {
                //TODO test/clean

                int numNodesPerLayer = in->vesselGeom->getNumNodesPerLayer();
                int nodeNum = in->vesselGeom->getNumNodes() - numNodesPerLayer;//0-based indexing so no +1;

                MatrixType points(numNodesPerLayer,nDims);
                points.fill(0);
                Type theta                      = 0;
                for(int i=0; i<numNodesPerLayer;i++)
                {
                        theta= i*2*PI/(numNodesPerLayer-1);
                        points(i,1) = sin(theta)*in->radius;
                        points(i,2) = cos(theta)*in->radius;
                        //points(i,3) = 0;//matrix is filled with zeros
                }    
                //Rotate the new axis to the Zaxis, and 0 degree vector to the y axis
                VectorType finalAxis = (in->newAxis).normalize();
                MatrixType nodesXYZ = in->vesselGeom->getMatrixNodePositions();
                VectorType beta(nDims);
                if(nodeNum<=0)//There are no layers yet
                {
                        beta(0) =0;
                        beta(0) =1;
                        beta(0) =0;
                }
                else
                {
                        //Prevent mesh twisting
                        beta = nodesXYZ.get_row(nodeNum+1) - nodesXYZ.get_row(nodeNum);//align with y axis
                }

                //beta = beta/beta.norm();
                //TODO make sure works
                beta.normalize();
                VectorType alpha = -vnl_cross_3d(beta,finalAxis);// cross(beta,Tvecu);
                alpha.normalize();
                MatrixType Rtz(nDims,nDims);// = [ alpha 0 ; beta 0;Tvecu 0; 0 0 0 1];
                Rtz.fill(0);
                Rtz.set_row(0,alpha);
                Rtz.set_row(1,beta);
                Rtz.set_row(2,finalAxis);
                //Now if Rnz takes Tvecu to the z axis, then Rtz^-1 takes the Z-axis to
                //Tvecu

                //Perform the transformation (rotation + translation)
                MatrixType Rtzi = vnl_matrix_inverse<Type>(Rtz) ;
                if(nodeNum<0)
                {
                        for(int i=0; i<numNodesPerLayer;i++)
                                points.set_row(i,Rtzi*points.get_row(i) + in->locXYZ);
                }
                else
                {
                        for(int i=0; i<numNodesPerLayer;i++)
                                points.set_row(i,Rtzi*points.get_row(i));
                        //If two layers are nearby and turn sharply they can cross one another
                        //This would produce uwanted folds in the mesh.
                        //Find the set of points on the wrong side of the plane defined by the
                        //previous layer
                        VectorType p_norm = nodesXYZ.get_row(nodeNum) - nodesXYZ.get_row(nodeNum-numNodesPerLayer);
                        VectorType growDirection = in->locXYZ - nodesXYZ.get_row(nodeNum);      
                        growDirection.normalize();
                        p_norm.normalize();
                        VectorType signVec(nDims);
                        Type dp =0;
                        Type maxDist =in->growDistance;//At least this distance must be used
                        Type dist =0;
                        for(int a=0; a<numNodesPerLayer;a++)// a = 1:length(pointsXYZ(:,1))
                        {
                                signVec = points.get_row(a) - nodesXYZ.get_row(nodeNum); //This is a point on our plane
                                dp = dot_product(signVec,p_norm);
                                if( dp <0)
                                {
                                        dist = abs(dp/dot_product(p_norm,finalAxis));
                                        if(dist > maxDist)
                                                maxDist = dist;
                                }
                        }
                        if(maxDist >2)
                                maxDist = 2;
                        //[Yv,Indm] = max(w_dist);
                        //Yv = max(min(Yv,2),0);

                        //now find a vector that places this point on the correct side of the
                        //plane. This vector is the norm * the w_dist;
                        for(int i=0; i<numNodesPerLayer;i++)
                        {
                                points.set_row(i,points.get_row(i) + maxDist*growDirection);
                        }
                }

                return points;
        }

}//end namespace mial

#endif

---