--- /dev/null
+#include "bbPackRecalageTransform3D3PointsBox.h"
+#include "bbPackRecalagePackage.h"
+namespace bbPackRecalage
+{
+
+BBTK_ADD_BLACK_BOX_TO_PACKAGE(PackRecalage,Transform3D3PointsBox)
+BBTK_BLACK_BOX_IMPLEMENTATION(Transform3D3PointsBox,bbtk::AtomicBlackBox);
+void Transform3D3PointsBox::Process()
+{
+ if(!bbGetInputInX1().empty() && bbGetInputInX1().size() >= 3 && !bbGetInputInX2().empty() && bbGetInputInX2().size() >= 3)
+ {
+ double A1[3];
+ double B1[3];
+ double O1[3];
+
+ double A2[3];
+ double B2[3];
+ double O2[3];
+
+ //In case there is an order in the points
+ if(!bbGetInputLabels1().empty() && !bbGetInputLabels2().empty() && bbGetInputLabels1()[0].compare("1") == 0 && bbGetInputLabels1()[1].compare("2") == 0 && bbGetInputLabels1()[2].compare("3") == 0)
+ {
+ int i;
+ for(i = 0; i < 3; i++)
+ {
+ if(bbGetInputLabels1()[i].compare("1") == 0)
+ {
+ O1[0] = bbGetInputInX1()[i];
+ O1[1] = bbGetInputInY1()[i];
+ O1[2] = bbGetInputInZ1()[i];
+ }
+ else if(bbGetInputLabels1()[i].compare("2") == 0)
+ {
+ A1[0] = bbGetInputInX1()[i];
+ A1[1] = bbGetInputInY1()[i];
+ A1[2] = bbGetInputInZ1()[i];
+ }
+ else if(bbGetInputLabels1()[i].compare("3") == 0)
+ {
+ B1[0] = bbGetInputInX1()[i];
+ B1[1] = bbGetInputInY1()[i];
+ B1[2] = bbGetInputInZ1()[i];
+ }
+ }
+ for(i = 0; i < 3; i++)
+ {
+ if(bbGetInputLabels2()[i].compare("1") == 0)
+ {
+ O2[0] = bbGetInputInX2()[i];
+ O2[1] = bbGetInputInY2()[i];
+ O2[2] = bbGetInputInZ2()[i];
+ }
+ else if(bbGetInputLabels2()[i].compare("2") == 0)
+ {
+ A2[0] = bbGetInputInX2()[i];
+ A2[1] = bbGetInputInY2()[i];
+ A2[2] = bbGetInputInZ2()[i];
+ }
+ else if(bbGetInputLabels2()[i].compare("3") == 0)
+ {
+ B2[0] = bbGetInputInX2()[i];
+ B2[1] = bbGetInputInY2()[i];
+ B2[2] = bbGetInputInZ2()[i];
+ }
+ }
+ }
+ else
+ {
+ O1[0] = bbGetInputInX1()[0];
+ O1[1] = bbGetInputInY1()[0];
+ O1[2] = bbGetInputInZ1()[0];
+
+ A1[0] = bbGetInputInX1()[1];
+ A1[1] = bbGetInputInY1()[1];
+ A1[2] = bbGetInputInZ1()[1];
+
+ B1[0] = bbGetInputInX1()[2];
+ B1[1] = bbGetInputInY1()[2];
+ B1[2] = bbGetInputInZ1()[2];
+
+ O2[0] = bbGetInputInX2()[0];
+ O2[1] = bbGetInputInY2()[0];
+ O2[2] = bbGetInputInZ2()[0];
+
+ A2[0] = bbGetInputInX2()[1];
+ A2[1] = bbGetInputInY2()[1];
+ A2[2] = bbGetInputInZ2()[1];
+
+ B2[0] = bbGetInputInX2()[2];
+ B2[1] = bbGetInputInY2()[2];
+ B2[2] = bbGetInputInZ2()[2];
+ }
+
+ //Create the new center point
+ double * centerPoint;
+ centerPoint[0] = O2[0];
+ centerPoint[1] = O2[1];
+ centerPoint[2] = O2[2];
+
+ transformer->SetSecondTranslation(centerPoint);
+
+ centerPoint[0] = O1[0];
+ centerPoint[1] = O1[1];
+ centerPoint[2] = O1[2];
+
+ transformer->SetFirstTranslation(centerPoint);
+
+ //Create the first vector
+ double* vector1;
+ vector1 = planes->makeVector(O1, A1);
+
+ //create the second vector
+ double* vector2;
+ vector2 = planes->makeVector(O2, A2);
+
+ //The rotation axis
+ double* axis = planes->getCrossProduct(vector2, vector1);
+
+ //Normalize the axis
+ axis = planes->getNormal(axis);
+
+ //Normalize the 2 vectors for calculating the dot product
+ double* vector1N = planes->getNormal(vector1);
+ double* vector2N = planes->getNormal(vector2);
+
+ //Dot product
+ double angle = planes->getDotProduct(vector2N, vector1N);
+
+ //Convert the dot product to radians
+ angle = acos(angle);
+
+ //Convert from Radians to Degrees (necesary for the transformation)
+ angle = vtkMath::DegreesFromRadians(angle);
+
+ //Set the cross product and dot product of the first rotation
+ transformer->SetRotationAxis(axis);
+ transformer->SetAngle(angle);
+
+ //Sets the second rotation axis (defined by the first vector) and the second angle as input
+ transformer->SetSecondRotationAxis(vector1);
+
+ ///////////////////////////////
+ //Second Rotation calculations start now (adjust the angle between the planes)
+ /////////////////////////////////
+
+ //Obtain the resultant transformation without moving the image first
+ vtkMatrix4x4* matrix = transformer->GetFirstResult()->GetMatrix();
+
+ double newMatrix[3][3];
+
+ int i,j;
+ for(i = 0; i < 3; i ++)
+ {
+ for(j = 0; j < 3; j ++)
+ {
+ newMatrix[i][j] = matrix->GetElement(i,j);
+ }
+ }
+
+ //Calculates the new position of the points after the first transformation
+ double A2N[3];
+ A2N[0] = A2[0]*newMatrix[0][0] + A2[0]*newMatrix[0][1] + A2[0]*newMatrix[0][2];
+ A2N[1] = A2[1]*newMatrix[1][0] + A2[0]*newMatrix[1][1] + A2[0]*newMatrix[1][2];
+ A2N[2] = A2[2]*newMatrix[2][0] + A2[0]*newMatrix[2][1] + A2[0]*newMatrix[2][2];
+
+ double B2N[3];
+ B2N[0] = B2[0]*newMatrix[0][0] + B2[0]*newMatrix[0][1] + B2[0]*newMatrix[0][2];
+ B2N[1] = B2[1]*newMatrix[1][0] + B2[0]*newMatrix[1][1] + B2[0]*newMatrix[1][2];
+ B2N[2] = B2[2]*newMatrix[2][0] + B2[0]*newMatrix[2][1] + B2[0]*newMatrix[2][2];
+
+ double O2N[3];
+ O2N[0] = O2[0]*newMatrix[0][0] + O2[0]*newMatrix[0][1] + O2[0]*newMatrix[0][2];
+ O2N[1] = O2[1]*newMatrix[1][0] + O2[0]*newMatrix[1][1] + O2[0]*newMatrix[1][2];
+ O2N[2] = O2[2]*newMatrix[2][0] + O2[0]*newMatrix[2][1] + O2[0]*newMatrix[2][2];
+
+ ///////////////////////////////////////////////////////////////////////////////
+ //Creation of the two planes for the second transformation
+ ///////////////////////////////////////////////////////////////////////////////
+
+ double * vector1A;
+ vector1A = planes->makeVector(O1, A1);
+
+ double * vector1B;
+ vector1B = planes->makeVector(O1, B1);
+
+ double * vector2AN;
+ vector2AN = planes->makeVector(O2N, A2N);
+
+ double * vector2BN;
+ vector2BN = planes->makeVector(O2N, B2N);
+
+ //Normal Plane 1
+ double* crossPlaneA = planes->getCrossProduct(vector1A, vector1B);
+
+ //Normal Plane 2
+ double* crossPlaneB = planes->getCrossProduct(vector2AN, vector2BN);
+
+ //Normalize the vectors
+ crossPlaneA = planes->getNormal(crossPlaneA);
+ crossPlaneB = planes->getNormal(crossPlaneB);
+
+ //Angle between the planes
+ double anglePlanes = planes->getDotProduct(crossPlaneB, crossPlaneA);
+
+ //Make necessary changes to the angle
+ anglePlanes = acos(anglePlanes);
+
+ anglePlanes = vtkMath::DegreesFromRadians(anglePlanes);
+
+ transformer->SetSecondAngle(-anglePlanes);
+
+ // The calculation of the transformations are made
+ transformer->Run();
+
+ // We get the results of transformer and set it as result of this box
+ bbSetOutputOut( transformer->GetResult() );
+ }
+ else
+ {
+ bbSetOutputOut( NULL );
+ }
+}
+void Transform3D3PointsBox::bbUserSetDefaultValues()
+{
+ std::vector<int> empty;
+ std::vector<std::string> emptyString;
+ bbSetInputInX1(empty);
+ bbSetInputInY1(empty);
+ bbSetInputInZ1(empty);
+ bbSetInputInX2(empty);
+ bbSetInputInY2(empty);
+ bbSetInputInZ2(empty);
+ bbSetInputLabels1(emptyString);
+ bbSetInputLabels2(emptyString);
+ bbSetOutputOut(NULL);
+}
+void Transform3D3PointsBox::bbUserInitializeProcessing()
+{
+ //We initialize the transformer
+ transformer=new Transformer3D();
+
+ //We initialize the plane operator
+ planes=new PlanesOperations();
+}
+void Transform3D3PointsBox::bbUserFinalizeProcessing()
+{
+ //We delete the transformer
+ delete transformer;
+
+ //We delete the plane operator
+ delete planes;
+}
+}
+// EO namespace bbPackRecalage
\ No newline at end of file
git://git.creatis.insa-lyon.fr / creaRigidRegistration.git / blobdiff