PackRecalage/src/bbPackRecalageTransform3D3PointsBox.cxx

   1 #include "bbPackRecalageTransform3D3PointsBox.h"
   2 #include "bbPackRecalagePackage.h"
   3 namespace bbPackRecalage
   4 {
   5
   6 BBTK_ADD_BLACK_BOX_TO_PACKAGE(PackRecalage,Transform3D3PointsBox)
   7 BBTK_BLACK_BOX_IMPLEMENTATION(Transform3D3PointsBox,bbtk::AtomicBlackBox);
   8 void Transform3D3PointsBox::Process()
   9 {
  10         if(!bbGetInputInX1().empty() && bbGetInputInX1().size() >= 3 && !bbGetInputInX2().empty() && bbGetInputInX2().size() >= 3)
  11         {
  12                 double A1[3];
  13                 double B1[3];
  14                 double O1[3];
  15
  16                 double A2[3];
  17                 double B2[3];
  18                 double O2[3];
  19
  20                 //In case there is an order in the points
  21                 if(!bbGetInputLabels1().empty() && !bbGetInputLabels2().empty() && bbGetInputLabels1()[0].compare("1") == 0 && bbGetInputLabels1()[1].compare("2") == 0 && bbGetInputLabels1()[2].compare("3") == 0)
  22                 {
  23                         int i;
  24                         for(i = 0; i < 3; i++)
  25                         {
  26                                 if(bbGetInputLabels1()[i].compare("1") == 0)
  27                                 {
  28                                         O1[0] = bbGetInputInX1()[i];
  29                                         O1[1] = bbGetInputInY1()[i];
  30                                         O1[2] = bbGetInputInZ1()[i];
  31                                 }
  32                                 else if(bbGetInputLabels1()[i].compare("2") == 0)
  33                                 {
  34                                         A1[0] = bbGetInputInX1()[i];
  35                                         A1[1] = bbGetInputInY1()[i];
  36                                         A1[2] = bbGetInputInZ1()[i];
  37                                 }
  38                                 else if(bbGetInputLabels1()[i].compare("3") == 0)
  39                                 {
  40                                         B1[0] = bbGetInputInX1()[i];
  41                                         B1[1] = bbGetInputInY1()[i];
  42                                         B1[2] = bbGetInputInZ1()[i];
  43                                 }
  44                         }
  45                         for(i = 0; i < 3; i++)
  46                         {
  47                                 if(bbGetInputLabels2()[i].compare("1") == 0)
  48                                 {
  49                                         O2[0] = bbGetInputInX2()[i];
  50                                         O2[1] = bbGetInputInY2()[i];
  51                                         O2[2] = bbGetInputInZ2()[i];
  52                                 }
  53                                 else if(bbGetInputLabels2()[i].compare("2") == 0)
  54                                 {
  55                                         A2[0] = bbGetInputInX2()[i];
  56                                         A2[1] = bbGetInputInY2()[i];
  57                                         A2[2] = bbGetInputInZ2()[i];
  58                                 }
  59                                 else if(bbGetInputLabels2()[i].compare("3") == 0)
  60                                 {
  61                                         B2[0] = bbGetInputInX2()[i];
  62                                         B2[1] = bbGetInputInY2()[i];
  63                                         B2[2] = bbGetInputInZ2()[i];
  64                                 }
  65                         }
  66                 }
  67                 else
  68                 {
  69                         O1[0] = bbGetInputInX1()[0];
  70                         O1[1] = bbGetInputInY1()[0];
  71                         O1[2] = bbGetInputInZ1()[0];
  72
  73                         A1[0] = bbGetInputInX1()[1];
  74                         A1[1] = bbGetInputInY1()[1];
  75                         A1[2] = bbGetInputInZ1()[1];
  76
  77                         B1[0] = bbGetInputInX1()[2];
  78                         B1[1] = bbGetInputInY1()[2];
  79                         B1[2] = bbGetInputInZ1()[2];
  80
  81                         O2[0] = bbGetInputInX2()[0];
  82                         O2[1] = bbGetInputInY2()[0];
  83                         O2[2] = bbGetInputInZ2()[0];
  84
  85                         A2[0] = bbGetInputInX2()[1];
  86                         A2[1] = bbGetInputInY2()[1];
  87                         A2[2] = bbGetInputInZ2()[1];
  88
  89                         B2[0] = bbGetInputInX2()[2];
  90                         B2[1] = bbGetInputInY2()[2];
  91                         B2[2] = bbGetInputInZ2()[2];
  92                 }
  93
  94                 //Create the new center point
  95                 double * centerPoint;
  96                 centerPoint[0] = O2[0];
  97                 centerPoint[1] = O2[1];
  98                 centerPoint[2] = O2[2];
  99
 100                 transformer->SetSecondTranslation(centerPoint);
 101
 102                 centerPoint[0] = O1[0];
 103                 centerPoint[1] = O1[1];
 104                 centerPoint[2] = O1[2];
 105
 106                 transformer->SetFirstTranslation(centerPoint);
 107
 108                 //Create the first vector
 109                 double* vector1;
 110                 vector1 = planes->makeVector(O1, A1);
 111
 112                 //create the second vector
 113                 double* vector2;
 114                 vector2 = planes->makeVector(O2, A2);
 115
 116                 //The rotation axis
 117                 double* axis = planes->getCrossProduct(vector2, vector1);
 118
 119                 //Normalize the axis
 120                 axis = planes->getNormal(axis);
 121
 122                 //Normalize the 2 vectors for calculating the dot product
 123                 double* vector1N = planes->getNormal(vector1);
 124                 double* vector2N = planes->getNormal(vector2);
 125
 126                 //Dot product
 127                 double angle = planes->getDotProduct(vector2N, vector1N);
 128
 129                 //Convert the dot product to radians
 130                 angle = acos(angle);
 131
 132                 //Convert from Radians to Degrees (necesary for the transformation)
 133                 angle = vtkMath::DegreesFromRadians(angle);
 134
 135                 //Set the cross product and dot product of the first rotation
 136                 transformer->SetRotationAxis(axis);
 137                 transformer->SetAngle(angle);
 138
 139                 //Sets the second rotation axis (defined by the first vector) and the second angle as input
 140                 transformer->SetSecondRotationAxis(vector1);
 141
 142                 ///////////////////////////////
 143                 //Second Rotation calculations start now (adjust the angle between the planes)
 144                 /////////////////////////////////
 145
 146                 //Obtain the resultant transformation without moving the image first
 147                 vtkMatrix4x4* matrix = transformer->GetFirstResult()->GetMatrix();
 148
 149                 double newMatrix[3][3];
 150
 151                 int i,j;
 152                 for(i = 0; i < 3; i ++)
 153                 {
 154                         for(j = 0; j < 3; j ++)
 155                         {
 156                                 newMatrix[i][j] = matrix->GetElement(i,j);
 157                         }
 158                 }
 159
 160                 //Calculates the new position of the points after the first transformation
 161                 double A2N[3];
 162                 A2N[0] = A2[0]*newMatrix[0][0] + A2[0]*newMatrix[0][1] + A2[0]*newMatrix[0][2];
 163                 A2N[1] = A2[1]*newMatrix[1][0] + A2[0]*newMatrix[1][1] + A2[0]*newMatrix[1][2];
 164                 A2N[2] = A2[2]*newMatrix[2][0] + A2[0]*newMatrix[2][1] + A2[0]*newMatrix[2][2];
 165
 166                 double B2N[3];
 167                 B2N[0] = B2[0]*newMatrix[0][0] + B2[0]*newMatrix[0][1] + B2[0]*newMatrix[0][2];
 168                 B2N[1] = B2[1]*newMatrix[1][0] + B2[0]*newMatrix[1][1] + B2[0]*newMatrix[1][2];
 169                 B2N[2] = B2[2]*newMatrix[2][0] + B2[0]*newMatrix[2][1] + B2[0]*newMatrix[2][2];
 170
 171                 double O2N[3];
 172                 O2N[0] = O2[0]*newMatrix[0][0] + O2[0]*newMatrix[0][1] + O2[0]*newMatrix[0][2];
 173                 O2N[1] = O2[1]*newMatrix[1][0] + O2[0]*newMatrix[1][1] + O2[0]*newMatrix[1][2];
 174                 O2N[2] = O2[2]*newMatrix[2][0] + O2[0]*newMatrix[2][1] + O2[0]*newMatrix[2][2];
 175
 176                 ///////////////////////////////////////////////////////////////////////////////
 177                 //Creation of the two planes for the second transformation
 178                 ///////////////////////////////////////////////////////////////////////////////
 179
 180                 double * vector1A;
 181                 vector1A = planes->makeVector(O1, A1);
 182
 183                 double * vector1B;
 184                 vector1B = planes->makeVector(O1, B1);
 185
 186                 double * vector2AN;
 187                 vector2AN = planes->makeVector(O2N, A2N);
 188
 189                 double * vector2BN;
 190                 vector2BN = planes->makeVector(O2N, B2N);
 191
 192                 //Normal Plane 1
 193                 double* crossPlaneA = planes->getCrossProduct(vector1A, vector1B);
 194
 195                 //Normal Plane 2
 196                 double* crossPlaneB = planes->getCrossProduct(vector2AN, vector2BN);
 197
 198                 //Normalize the vectors
 199                 crossPlaneA = planes->getNormal(crossPlaneA);
 200                 crossPlaneB = planes->getNormal(crossPlaneB);
 201
 202                 //Angle between the planes
 203                 double anglePlanes = planes->getDotProduct(crossPlaneB, crossPlaneA);
 204
 205                 //Make necessary changes to the angle
 206                 anglePlanes = acos(anglePlanes);
 207
 208                 anglePlanes = vtkMath::DegreesFromRadians(anglePlanes);
 209
 210                 transformer->SetSecondAngle(-anglePlanes);
 211
 212                 // The calculation of the transformations are made
 213                 transformer->Run();
 214
 215                 // We get the results of transformer and set it as result of this box
 216                 bbSetOutputOut( transformer->GetResult() );
 217         }
 218         else
 219         {
 220                 bbSetOutputOut( NULL );
 221         }
 222 }
 223 void Transform3D3PointsBox::bbUserSetDefaultValues()
 224 {
 225         std::vector<int> empty;
 226         std::vector<std::string> emptyString;
 227         bbSetInputInX1(empty);
 228         bbSetInputInY1(empty);
 229         bbSetInputInZ1(empty);
 230         bbSetInputInX2(empty);
 231         bbSetInputInY2(empty);
 232         bbSetInputInZ2(empty);
 233         bbSetInputLabels1(emptyString);
 234         bbSetInputLabels2(emptyString);
 235         bbSetOutputOut(NULL);
 236 }
 237 void Transform3D3PointsBox::bbUserInitializeProcessing()
 238 {
 239         //We initialize the transformer
 240         transformer=new Transformer3D();
 241
 242         //We initialize the plane operator
 243         planes=new PlanesOperations();
 244 }
 245 void Transform3D3PointsBox::bbUserFinalizeProcessing()
 246 {
 247         //We delete the transformer
 248         delete transformer;
 249
 250         //We delete the plane operator
 251         delete planes;
 252 }
 253 }
 254 // EO namespace bbPackRecalage