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

00001 #ifndef _Phys_VesselCrawlerEuler_CXX
00002 #define _Phys_VesselCrawlerEuler_CXX
00003 
00004 #include "Phys_VesselCrawlerEuler.h"
00005 
00006 namespace mial
00007 {
00008 
00009         template<class DataType, class TGradientImage, int nDims,class MType, class VType>
00010                 void Phys_VesselCrawlerEuler<DataType, TGradientImage, nDims,MType,VType>::updateSpringsFromGeometric()
00011         {
00012                 unsigned int length = springsNodes.rows();
00013                 if(geom->getNumConnections() != length) //if there are new springs
00014                 {
00015                         unsigned int newLength = geom->getNumConnections();
00016 
00017                         springsRest.set_size(newLength);
00018                         springsDamp.set_size(newLength);
00019                         springLengths.set_size(newLength);
00020                         springsK.set_size(newLength);
00021                         springsNodes.set_size(newLength,2);
00022 
00023                         springsNodes = geom->getMatrixConnections();
00024                         int from;
00025                         int to;
00026                         for(int i=0; i<newLength; i++)
00027                         {
00028                                 from = springsNodes(i,0);
00029                                 to = springsNodes(i,1);
00030                                 std::cout << "from: " << from << " to: " << to << " i " << i << std::endl;
00031                                 springsRest(i) = (nodes.get_row(from)-nodes.get_row(to)).two_norm();
00032                                 springsDamp(i) = defaultDamp;
00033                                 springLengths(i) = (nodes.get_row(from)-nodes.get_row(to)).two_norm();
00034                                 springsK(i) =defaultK;
00035                         }
00036 
00037                         // TODO write method for dynamically changing topology that preserves current settings
00038                         // This will need to search the set of current connections for existing springs and find those
00039                         // still in use
00040                         /*
00041                         //Store current values in a temp variable
00042 
00043                         vnl_Vector tmp(length*4);
00044 
00045                         tmp.update(springsRest,0);
00046                         tmp.update(springsDamp, length);
00047                         tmp.update(springLengths, length*2);
00048                         tmp.update(springsK, length*3);
00049 
00050                         vnl_matrix<int> tmp2(length,2);
00051                         tmp2.update(springsNodes,0,0);
00052 
00053                         //Increase the size
00054                         springsRest.set_size(newLength);
00055                         springsDamp.set_size(newLength);
00056                         springLengths.set_size(newLength);
00057                         springsK.set_size(newLength);
00058                         springsNodes.set_size(newLength,2);
00059 
00060                         //Copy back the old values
00061                         for(int i =0; i< length; i++)
00062                         {
00063                         springsRest(i) = tmp(i);
00064 
00065                         //springsDamp.update(tmp.get_n_rows(nodes.rows(),nodes.rows()) ,0);
00066                         springsDamp(i) = tmp(i+length);
00067 
00068                         //springLengths.update(tmp.get_n_rows(nodes.rows()*2,nodes.rows()) ,0);
00069                         springLengths(i) = tmp(i+2*length);
00070 
00071                         //springsK.update(tmp.get_n_rows(nodes.rows()*3,nodes.rows()) ,0);
00072                         springsK(i) = tmp(i+3*length);
00073 
00074                         }
00075                         springsNodes.update(tmp2,0);
00076 
00077                         //Add the new springs
00078                         int from =0;
00079                         int to =0;
00080                         for(int i=length; i<newLength; i++)
00081                         {
00082                         from = 0;
00083                         springsRest(i) = (nodes.get_row(from)-nodes.get_row(to)).two_norm();
00084                         springsDamp(i) = defaultDamp;
00085 
00086                         springLengths(length) = (nodes.get_row(from)-nodes.get_row(to)).two_norm();
00087 
00088                         springsK(length) =defaultK;
00089 
00090                         springsNodes(length,0) = from;
00091                         springsNodes(length,1) = to;*/
00092                 }
00093         }
00094         
00095         template<class DataType, class TGradientImage, int nDims,class MType, class VType>
00096                 bool Phys_VesselCrawlerEuler<DataType, TGradientImage, nDims,MType,VType>::simulate()
00097         {
00098                 //**************************//
00099                 //Simulate the spring forces
00100                 //**************************//
00101 
00102                 //TODO make this more efficient--only update if change, only update new springs etc
00103                 nodes = geom->getMatrixNodePositions();
00104                 if(geom->didTopologyChange())
00105                         updateSpringsFromGeometric();
00106                 if(geom->didNodesChange())
00107                 {
00108                         nodesF.set_size(geom->getNumNodes(),nDims);
00109                         nodesF.fill(0);
00110                         nodesA.set_size(geom->getNumNodes(),nDims);
00111                         nodesA.fill(0);
00112                         nodesV.set_size(geom->getNumNodes(),nDims);
00113                         nodesV.fill(0);
00114                         nodesFDef.set_size(geom->getNumNodes(),nDims);
00115                         nodesFDef.fill(0);
00116                         nodesM.set_size(geom->getNumNodes());
00117                         nodesM.fill(defaultMass);
00118                 }
00119 
00120                 VectorType springsActive = geom->getActiveSprings(-1);
00121                 const unsigned int length = springsActive.size();
00122                 VectorType nodesActive = geom->getActiveNodes(-1);
00123 
00124                 MatrixType nodeDist(length,nDims);
00125                 VectorType distNorm(length);
00126                 MatrixType velDiff(length,nDims);
00127                 VectorType D(length);
00128                 MatrixType F(length,nDims);
00129                 VectorType pointForce(nDims);
00130                 VectorType tmpVel(nDims);
00131                 VectorType tmpDist(nDims);
00132                 //Can't seem to get working with length
00133                 /*      vnl_matrix_fixed<DataType, 4 ,nDims> nodeDist;
00134                 vnl_vector_fixed<DataType,length> distNorm;
00135                 vnl_matrix_fixed<DataType, 4,nDims> velDiff;
00136                 vnl_vector_fixed<DataType, 4> D;
00137                 vnl_matrix_fixed<DataType, 4, nDims> F;*/
00138                 int a,b,ind;
00139                 DataType rowSum = 0;
00140                 int count = 0;
00141                 int runTime = 75000;
00142                 int numNodes = nodesF.rows();
00143 
00144                 itk::ImageRegionIterator<GradientImageType> gradIT(gradientPointer,gradientPointer->GetLargestPossibleRegion());
00145                 GradientImageType::IndexType gradIndex;
00146 
00147                 typedef itk::VectorLinearInterpolateImageFunction< GradientImageType, DataType > InterpolatorType;
00148                 InterpolatorType::Pointer interp = InterpolatorType::New();
00149                 InterpolatorType::OutputType imgForce;
00150                 interp->SetInputImage(gradientPointer);
00151 
00152                 //      springsRest(0) = 15;
00153                 bool incTime = true;
00154                 DataType dispLength =0;
00155                 DataType localTimeStep = timeStep;
00156 
00157 
00158                 //TODO Get from geometry
00159                 VectorType layerNorm(3);
00160                 layerNorm.normalize();
00161                 MatrixType projMatrix(3,3);
00162                 
00163                 //TODO verify row column Only update if change in layerNorm
00164                 projMatrix(0,0) = layerNorm(1)*layerNorm(1) + layerNorm(2)*layerNorm(2);
00165                 projMatrix(0,1) = -layerNorm(0)*layerNorm(1);
00166                 projMatrix(0,2) = -layerNorm(0)*layerNorm(2);
00167 
00168                 projMatrix(1,0) = -layerNorm(1)*layerNorm(0);
00169                 projMatrix(1,1) = layerNorm(0)*layerNorm(0) + layerNorm(2)*layerNorm(2);
00170                 projMatrix(1,2) = -layerNorm(1)*layerNorm(2);
00171 
00172                 projMatrix(2,0) = -layerNorm(2)*layerNorm(0);
00173                 projMatrix(2,1) = -layerNorm(2)*layerNorm(1);
00174                 projMatrix(2,2) = layerNorm(0)*layerNorm(0) + layerNorm(1)*layerNorm(1);
00175 
00176                 while(count<runTime)
00177                 {
00178                         nodesF.fill(0);
00179                         if(count==0)
00180                         {
00181                                 nodesF = nodesF + nodesFDef; //Add on any deformation forces, then zero them
00182                         }
00183                         for(int i=0; i<length ;i++)
00184                         {
00185                                 a = springsNodes(springsActive(i),0);
00186                                 b = springsNodes(springsActive(i),1);
00187                                 //A = nodesXYZ(springsNodes(:,2),:)  -  nodesXYZ(springsNodes(:,1),:);
00188                                 //B = sum(nodeDist.^2,2).^0.5; //%norm(Xa-Xb) for all springs (numSprings*1)
00189                                 //C = nodesV(springsNodes(:,2),:)  -  nodesV(springsNodes(:,1),:);
00190                                 nodeDist.set_row(i,nodes.get_row(b) - nodes.get_row(a) );
00191                                 distNorm(i)= (nodeDist.get_row(i)).two_norm();
00192                                 velDiff.set_row(i,nodesV.get_row(b) - nodesV.get_row(a));
00193 
00194                                 //D = sum(C.*nodeDist,2) ./ B ;// %[(Va-Vb)'(Xa-Xb)]/norm(Xa-Xb)  for all springs (numSprings*1) 
00195                                 for(int j=0; j<nDims;j++)
00196                                 {
00197                                         rowSum+= velDiff(i,j)*nodeDist(i,j);
00198                                 }
00199                                 D(i) = rowSum/distNorm(i);
00200                                 rowSum =0;
00201 
00202                                 //F = repmat((springsK.*(B-springsRest)+springsDamp.*D)./B , [1 3]).*nodeDist; //%spring forces for all springs (numSprings*2)
00203                                 F.set_row(i,  (( (springsK(springsActive(i))* (distNorm(i)-springsRest(springsActive(i))) + springsDamp(springsActive(i))*D(i))/ distNorm(i) ) )* nodeDist.get_row(i)  );
00204 
00205                                 nodesF.set_row(a,nodesF.get_row(a)+ F.get_row(i));
00206                                 nodesF.set_row(b,nodesF.get_row(b)- F.get_row(i));
00207                         }
00208 
00209                         for(int i=0; i<nodesActive.size(); i++)
00210                         {
00211                                 ind = nodesActive(i);
00212                                 //**************************//
00213                                 //Calculate Image forces
00214                                 //**************************//
00215                                 //nodesF=nodesF+ repmat(nodesExt.* nodesBnd , [1,2]) .* [...
00216                                 //     diag(Gx(round(nodesXY(:,2)),round(nodesXY(:,1)))) , ...
00217                                 //   diag(Gy(round(nodesXY(:,2)),round(nodesXY(:,1)))) ];
00218 
00219                                 if(DEBUG)
00220                                 {       std::cout << "node: " << i << " x: " << nodes(i,0) << " y: " << nodes(i,1) << " z: " << nodes(i,2) << std::endl;
00221                                 std::cout << "Force: " << i << " x: " << nodesF(i,0) << " y: " << nodesF(i,1) << " z: " << nodesF(i,2) << std::endl;
00222                                 }
00223 
00224                                 for(int a =0; a<nDims; a++)
00225                                         gradIndex[a] = nodes(ind,a);
00226                                 
00227 
00228                                 //pointForce(0) = interp->EvaluateAtIndex(gradIndex);
00229                                 imgForce = interp->EvaluateAtIndex(gradIndex);
00230 
00231                                 for(int a =0; a<nDims; a++)
00232                                 {pointForce(a) = 5000 * imgForce[a];
00233                                 }
00234                                 //pointForce(1) = interp->EvaluateAtIndex(gradIndex);
00235 
00236                                 nodesF.set_row(ind, nodesF.get_row(ind) +  pointForce);
00237 
00238                                 if(DEBUG)
00239                                         std::cout << "Force: " << i << " x: " << nodesF(i,0) << " y: " << nodesF(i,1) << " z: " << nodesF(i,2) << std::endl;
00240 
00241                                 //std::cout << "Force: " << i << " x: " << 100*pointForce(0) << " y: " << 100*pointForce(1) << " z: " << std::endl;
00242 
00243                                 //**************************//
00244                                 //Drag Force
00245                                 //**************************//
00246                                 nodesF.set_row(ind, nodesF.get_row(ind)-nodesV.get_row(ind)*defaultDrag); 
00247 
00248                                 //**************************//
00249                                 //Apply forces to model
00250                                 //**************************//
00251                                 //nodesA=nodesF ./ (repmat(nodesM,[1,3]));%accelaration
00252                                 //nodesV=DT * nodesA + nodesV;%new velocity 
00253                                 //nodesXYZ=(DT * nodesV).*repmat(1-nodesFxd,[1,3]) + nodesXYZ;%new position 
00254 
00255                                 
00256                                 //TODO Project the force
00257                                 nodesF.set_row(ind,projMatrix*nodesF.get_row(ind));
00258 
00259                                 nodesA.set_row(ind, nodesF.get_row(ind)/nodesM(ind) );
00260                                 //std::cout << "Accel: " << i << " x: " << nodesA(i,0) << " y: " << nodesA(i,1) << " z: " << std::endl;
00261                                 while(incTime)
00262                                 {
00263                                         tmpVel = (localTimeStep*nodesA.get_row(ind) + nodesV.get_row(ind));
00264                                         tmpDist = (localTimeStep * tmpVel );
00265 
00266                                         if(DEBUG)
00267                                         {       std::cout << "Dist: " << i << " x: " << tmpDist(0) << " y: " << tmpDist(1) << " z: " << tmpDist(2) << std::endl;
00268                                         std::cout << "Vel: " << i << " x: " << nodesV(i,0) << " y: " << nodesV(i,1) << " z: " << nodesV(i,2) << std::endl;
00269                                         }
00270 
00271                                         //Check if velocity passes thresh
00272                                         dispLength = abs(tmpDist.two_norm());
00273                                         if(  dispLength< 1 )
00274                                         {
00275                                                 incTime = false;
00276                                                 nodesV.set_row(ind, tmpVel );                   
00277                                                 nodes.set_row(ind,  tmpDist + nodes.get_row(ind) );
00278                                         }
00279                                         /*      else if( vLength < 0.5)
00280                                         {
00281                                         localTimeStep = localTimeStep*2;
00282                                         }*/
00283                                         else //shrink the time step and recalc.
00284                                         {
00285                                                 std::cout << "DT : " << localTimeStep << std::endl;
00286                                                 localTimeStep = localTimeStep/2;
00287                                         }
00288                                 }
00289                                 incTime = true;
00290                         }
00291                         count++;
00292                 }
00293 
00294                 geom->setMatrixNodePositions(nodes);
00295                 return 1;
00296         }
00297 
00298 } // end namespace mial
00299 #endif
Generated on Wed Jul 19 13:05:16 2006 for IDO by  doxygen 1.4.7