/*=========================================================================

  Program:   wxMaracas
  Module:    $RCSfile: wxMaracasEmptyPanel_3.cxx,v $
  Language:  C++
  Date:      $Date: 2008/10/31 16:32:10 $
  Version:   $Revision: 1.1 $

  Copyright: (c) 2002, 2003
  License:

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/



#include "wxMaracasEmptyPanel_3.h"

#include <gdcm.h> 
#include <gdcmFile.h> 
#include "vtkGdcmReader.h"
#include <vtkTransform.h>
#include "marGdcmDicom.h"
#include <vtkMatrix4x4.h>

#define PI 3.14159265358979323846

wxMaracasEmptyPanel_3::wxMaracasEmptyPanel_3( wxWindow* parent  )
: wxPanel( parent, -1, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL)
{
  // Create the main frame window
  _emptyPanel_3_Widget = new wxEmptyPanel_3_Widget( this);
  wxBoxSizer* szTop = new wxBoxSizer( wxVERTICAL );
  szTop->Add( _emptyPanel_3_Widget, 1  , wxEXPAND |  wxALL , 0 );
  szTop->Fit( this );
  szTop->SetSizeHints( this );
  this->SetAutoLayout( true );
  this->SetSizer( szTop );
  this->Layout();
}
//----------------------------------------------------------------------------
wxMaracasEmptyPanel_3::~wxMaracasEmptyPanel_3( )
{
  // Nothing
}
//----------------------------------------------------------------------------
void wxMaracasEmptyPanel_3::ConfigureVTK( marImageData *marimagedata )
{
  _emptyPanel_3_Widget->ConfigureVTK(marimagedata);
}





//----------------------------------------------------------------------------
//----------------------------------------------------------------------------
//----------------------------------------------------------------------------





void GetAng(double &angX,double &angY,double &angZ,double positionVec[])
{
//	double PI=3.14159265358979323846;
	double x,y,z;
	x = positionVec[0];
	y = positionVec[1];
	z = positionVec[2];

	angZ = -atan2( y , x )*180/PI;
	angY = -atan2( z , sqrt( x*x + y*y ) )*180/PI;


	vtkTransform *transform = vtkTransform::New();
	transform->Identity();
	transform->RotateY( angY );
	transform->RotateZ( angZ );

	double in[4],out[4];
	in[0] = positionVec[3];
	in[1] = positionVec[4];
	in[2] = positionVec[5];
	in[3] = 1;

	transform->MultiplyPoint(in,out);

	transform->Delete();

	x = out[0] ;
	y = out[1] ;
	z = out[2] ;
	
	angX =  -atan2(z,y)*180/PI;

//--
FILE *ff=fopen("c:/Temp/PIXEL_EED.txt","a+");

	vtkTransform *transform2 = vtkTransform::New();
	transform2->Identity();
	transform2->RotateX( angX );
	transform2->RotateY( -angY );
	transform2->RotateZ( angZ );
	in[0] = (double)positionVec[0];
	in[1] = (double)positionVec[1];
	in[2] = (double)positionVec[2];;
	in[3] = 1;
	transform2->MultiplyPoint(in,out);
fprintf(ff,">>>> vx=( %f , %f , %f )  ",out[0],out[1],out[2]);

	in[0] = (double)positionVec[3];
	in[1] = (double)positionVec[4];
	in[2] = (double)positionVec[5];
	in[3] = 1;
	transform2->MultiplyPoint(in,out);
fprintf(ff,"vy=( %f , %f , %f )\n",out[0],out[1],out[2]);
	transform2->Delete();
fclose(ff);

}




//----------------------------------------------------------------------------
//----------------------------------------------------------------------------
//----------------------------------------------------------------------------


BEGIN_EVENT_TABLE( wxMaracasEmptyPanel_4, wxPanel )
	EVT_MENU( 12121, wxMaracasEmptyPanel_4::OnRefreshView )
	EVT_MENU( 12122, wxMaracasEmptyPanel_4::OnDClickLeft  )
END_EVENT_TABLE( );


wxMaracasEmptyPanel_4::wxMaracasEmptyPanel_4( wxWindow* parent )
: wxPanel( parent, -1, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL)
{

	_vtkbasedata1						= NULL;
	_vtkbasedata2						= NULL;
	_wxvtk2Dbaseview1					= NULL;
	_wxvtk2Dbaseview2					= NULL;

	wxSplitterWindow * splitPanel = new wxSplitterWindow(this,-1);
	wxPanel *imagePanel		= CreateImagePanel(splitPanel);
	wxPanel *panelControl	= CreateControlPanel_Algo(splitPanel);

	splitPanel->SplitVertically( imagePanel ,  panelControl , 2 );
//	splitPanel->FitInside();


	  // Create the main frame window
	wxBoxSizer* szTop = new wxBoxSizer( wxHORIZONTAL );
	szTop->Add( splitPanel  , 1  , wxEXPAND |  wxALL , 0 );
	szTop->Fit( this );
	szTop->SetSizeHints( this );
	this->SetAutoLayout( true );
	this->SetSizer( szTop );
	this->Layout();
}

//----------------------------------------------------------------------------

wxMaracasEmptyPanel_4::~wxMaracasEmptyPanel_4()
{
	delete	_wxvtk2Dbaseview1;	 
	delete	_wxvtk2Dbaseview2;	 
//	delete	_vtkbasedata1;		 
//	delete	_vtkbasedata2;	
}



//----------------------------------------------------------------------------

wxPanel* wxMaracasEmptyPanel_4::CreateImagePanel(wxWindow *parent)
{
	wxPanel *panel = new wxPanel(parent,-1);

	_panelImage1						= new wxPanel(panel,-1);
	_panelImage2						= new wxPanel(panel,-1);

	wxBoxSizer			*topsizer1		= new wxBoxSizer(wxVERTICAL  );
	_panelImage1->SetSizer(topsizer1);

	wxBoxSizer			*topsizer2		= new wxBoxSizer(wxVERTICAL  );
	_panelImage2->SetSizer(topsizer2);

	  // Create the main frame window
	wxBoxSizer* szTop = new wxBoxSizer( wxHORIZONTAL );
	szTop->Add( _panelImage1  , 1  , wxEXPAND |  wxALL , 0 );
	szTop->Add( _panelImage2  , 1  , wxEXPAND |  wxALL , 0 );
	szTop->Fit( panel );
	szTop->SetSizeHints( panel );
	panel->SetAutoLayout( true );
	panel->SetSizer( szTop );
	panel->Layout();
	return panel;

}


//----------------------------------------------------------------------------

wxPanel* wxMaracasEmptyPanel_4::CreateControlPanel_Algo(wxWindow *parent)
{
	wxPanel *panel = new wxPanel(parent,-1);

	wxBoxSizer			*szTop		= new wxBoxSizer(wxVERTICAL  );

	_rb1 = new wxRadioButton(panel,-1,_T("Algo. 1"));
	_rb2 = new wxRadioButton(panel,-1,_T("Algo. 2"));
	_rb3 = new wxRadioButton(panel,-1,_T("Algo. 3"));

	_rb1->SetValue(true);

	szTop->Add( _rb1  , 1  , wxEXPAND |  wxALL , 0 );
	szTop->Add( _rb2  , 1  , wxEXPAND |  wxALL , 0 );
	szTop->Add( _rb3  , 1  , wxEXPAND |  wxALL , 0 );
	szTop->Fit( parent );
	szTop->SetSizeHints( parent );
	panel->SetAutoLayout( true );
	panel->SetSizer( szTop );
	panel->Layout();

	return panel;
}

//----------------------------------------------------------------------------

void wxMaracasEmptyPanel_4::OnRefreshView(wxCommandEvent & event) // virtual
{

	int pIn[4],pOut[4]; 
	pIn[0] = (int)(_vtkbasedata1->GetX());
	pIn[1] = (int)(_vtkbasedata1->GetY());
	pIn[2] = (int)(_vtkbasedata1->GetZ());
	pIn[2] = 0;
	pIn[3] = 1;

	if (_rb1->GetValue()==true)
	{
		TranformPixel1(pIn,pOut, &_infoImg1,&_infoImg2);
	}
	if (_rb2->GetValue()==true)
	{
		TranformPixel2(pIn,pOut, &_infoImg1,&_infoImg2);
	}
	if (_rb3->GetValue()==true)
	{
		TranformPixel3(pIn,pOut, &_infoImg1,&_infoImg2);
	}


FILE *ff=fopen("c:/Temp/PIXEL_EED.txt","a+");
fprintf(ff,"-) %d %d %d \n"	,pIn[0]	,pIn[1]	,pIn[2]		);
fprintf(ff,"1) %d %d %d \n"	,pOut[0],pOut[1],pOut[2]	);
//TranformPixel2(pIn,pOut, &_infoImg1,&_infoImg2);
//fprintf(ff,"2) %d %d %d \n\n"	,pOut[0],pOut[1],pOut[2]	);
fclose(ff);

	_vtkbasedata2->SetX( pOut[0] );
	_vtkbasedata2->SetY( pOut[1] );
	_vtkbasedata2->SetZ( 0 );



	if (_wxvtk2Dbaseview1!=NULL) 
	{
		_wxvtk2Dbaseview1->RefreshView();
	}
	if (_wxvtk2Dbaseview2!=NULL) 
	{
		_wxvtk2Dbaseview2->Refresh();
		_wxvtk2Dbaseview2->RefreshView();
	}
}
//----------------------------------------------------------------------------
void wxMaracasEmptyPanel_4::OnDClickLeft(wxCommandEvent & event) // virtual
{
}
//----------------------------------------------------------------------------
void wxMaracasEmptyPanel_4::ConfigureVTK( )
{
	char filename1[255];
	char filename2[255];
	FILE *ff=fopen("c:/temp/filename.txt","r");
	fscanf(ff, "%s ",filename1);
	fscanf(ff, "%s ",filename2);
	fclose(ff);

	ConfigureImage(filename1,_panelImage1,1);
	ConfigureImage(filename2,_panelImage2,2);
}
//----------------------------------------------------------------------------
void wxMaracasEmptyPanel_4::ConfigureImage( char *fileName,wxPanel *panelImage, int ventana)
{
	vtkMPRBaseData *vtkbasedata;
	wxVtkMPR2DView *wxvtk2Dbaseview;

	struct InfoImag * infoImage;

	if (ventana==1)
	{
		vtkbasedata		= _vtkbasedata1;
		wxvtk2Dbaseview	= _wxvtk2Dbaseview1;
		infoImage=&_infoImg1;
	} else {
		vtkbasedata		= _vtkbasedata2;
		wxvtk2Dbaseview	= _wxvtk2Dbaseview2;
		infoImage=&_infoImg2;
	}
 

	// Read header file
	GDCM_NAME_SPACE::File *gdcmFile = GDCM_NAME_SPACE::File::New();
	gdcmFile->SetLoadMode( GDCM_NAME_SPACE::LD_NOSHADOW );
	gdcmFile->SetFileName( fileName );
	bool ok = gdcmFile->Load();
	long int offset = gdcmFile->GetPixelOffset(); 
	int dim[3];
	dim[0]=gdcmFile->GetXSize();
	dim[1]=gdcmFile->GetYSize();
	dim[2]=1; 

	infoImage->Spacing[0]=gdcmFile->GetXSpacing();
	infoImage->Spacing[1]=gdcmFile->GetYSpacing();
	infoImage->ImPoPa[0]=gdcmFile->GetXOrigin();
	infoImage->ImPoPa[1]=gdcmFile->GetYOrigin();
	infoImage->ImPoPa[2]=gdcmFile->GetZOrigin();

	float iop[6];
	gdcmFile->GetImageOrientationPatient(iop);
	infoImage->ImOrPa[0]=iop[0];
	infoImage->ImOrPa[1]=iop[1];
	infoImage->ImOrPa[2]=iop[2];
	infoImage->ImOrPa[3]=iop[3];
	infoImage->ImOrPa[4]=iop[4];
	infoImage->ImOrPa[5]=iop[5];
	

	gdcmFile->Delete();

	// Memory allocation
	vtkImageData *imageData	=	vtkImageData::New();
	imageData->SetDimensions ( dim );
	imageData->SetScalarTypeToUnsignedShort();
	imageData->AllocateScalars();    
	imageData->Update();  
	marImageData *marimagedata = new marImageData(imageData);

	// Read Image
	marGdcmDicom margdcmdicom;
	void *p_vol = imageData->GetScalarPointer(0,0,0);
	margdcmdicom.loadFileDicom( p_vol , dim , fileName );
	
	if (vtkbasedata)		{ delete vtkbasedata;     } 
	if (wxvtk2Dbaseview)	{ delete wxvtk2Dbaseview; } 

	vtkbasedata		= new vtkMPRBaseData(); 

wxWidgetMesure2D *widgetMesure	= new wxWidgetMesure2D(panelImage);
wxvtk2Dbaseview	= new wxVtkMPR2DView( widgetMesure,2 );

//	wxvtk2Dbaseview	= new wxVtkMPR2DView( panelImage,2 );


	vtkbasedata->SetMarImageData( marimagedata );
	wxvtk2Dbaseview->SetVtkBaseData(vtkbasedata);
	wxvtk2Dbaseview->Configure();

	widgetMesure					->	ConfigureA(wxvtk2Dbaseview);
	widgetMesure					->	SetMesureScale( infoImage->Spacing[0] );


	wxVTKRenderWindowInteractor *imagePanel = wxvtk2Dbaseview->GetWxVTKRenderWindowInteractor();       

	vtkbasedata->SetX( dim[0]/2 );
	vtkbasedata->SetY( dim[1]/2 );
	vtkbasedata->SetZ( 0 );

	wxSizer *sizer=panelImage->GetSizer();
//	sizer-> Add( imagePanel ,1,wxGROW  ,0);
	sizer-> Add( widgetMesure ,1,wxGROW  ,0);

	panelImage->SetAutoLayout(true);
	panelImage->Layout();  

	wxvtk2Dbaseview->Refresh();

	if (ventana==1)
	{
		_vtkbasedata1		= vtkbasedata;
		_wxvtk2Dbaseview1	= wxvtk2Dbaseview;
		_widgetMesure1		= widgetMesure;
	} else {
		_vtkbasedata2		= vtkbasedata;
		_wxvtk2Dbaseview2	= wxvtk2Dbaseview;
		_widgetMesure2		= widgetMesure;
	}
}



//-----------------------------------------------------------------------------


void wxMaracasEmptyPanel_4::TranformPixel1(int pIn[], int pOut[], struct InfoImag *infoImg1, struct InfoImag *infoImg2)
{
	vtkTransform *transform = vtkTransform::New();
	transform->Identity();

	double ang1X,ang1Y,ang1Z;
	double ang2X,ang2Y,ang2Z;
	double in[4],out[4];
	
	GetAng(ang1X,ang1Y,ang1Z,infoImg1->ImOrPa);
	GetAng(ang2X,ang2Y,ang2Z,infoImg2->ImOrPa);

	// Transformation
	transform->Scale( 1/infoImg2->Spacing[0] , 1/infoImg2->Spacing[1] , 1 );
	transform->RotateX( ang2X );
	transform->RotateY( -ang2Y );
	transform->RotateZ( ang2Z );
	transform->Translate( -infoImg2->ImPoPa[0] ,  -infoImg2->ImPoPa[1] ,  -infoImg2->ImPoPa[2] );
	transform->Translate( infoImg1->ImPoPa[0] ,  infoImg1->ImPoPa[1] ,  infoImg1->ImPoPa[2] );
	transform->RotateZ( -ang1Z );
	transform->RotateY( ang1Y );
	transform->RotateX( -ang1X );
	transform->Scale(	infoImg1->Spacing[0]	,	infoImg1->Spacing[1]	,	1	);

	in[0] = (double)pIn[0];
	in[1] = (double)pIn[1];
	in[2] = 0;
	in[3] = 1;

	transform->MultiplyPoint(in,out);

	transform->Delete();

	pOut[0] = (int)(out[0] + 0.5);
	pOut[1] = (int)(out[1] + 0.5);
	pOut[2] = (int)(out[2] + 0.5);
}



//-----------------------------------------------------------------------------

void wxMaracasEmptyPanel_4::TranformPixel2(int pIn[], int pOut[], struct InfoImag *infoImg1, struct InfoImag *infoImg2)
{
	double in[4],out[4];

	//Scale 1
	double src_pos_x = (double)pIn[0] * infoImg1->Spacing[0];
	double src_pos_y = (double)pIn[1] * infoImg1->Spacing[1];
	double src_pos_z = (double)pIn[2] * 1;



//Rotation 1
double dst_nrm_dircos_x;
double dst_nrm_dircos_y;
double dst_nrm_dircos_z;

double dst_pos_x;
double dst_pos_y;
double dst_pos_z;

double dst_row_dircos_x = infoImg1->ImOrPa[0];
double dst_row_dircos_y = infoImg1->ImOrPa[1];
double dst_row_dircos_z = infoImg1->ImOrPa[2];

double dst_col_dircos_x = infoImg1->ImOrPa[3];
double dst_col_dircos_y = infoImg1->ImOrPa[4];
double dst_col_dircos_z = infoImg1->ImOrPa[5];

dst_nrm_dircos_x = dst_row_dircos_y * dst_col_dircos_z - dst_row_dircos_z * dst_col_dircos_y; 
dst_nrm_dircos_y = dst_row_dircos_z * dst_col_dircos_x - dst_row_dircos_x * dst_col_dircos_z; 
dst_nrm_dircos_z = dst_row_dircos_x * dst_col_dircos_y - dst_row_dircos_y * dst_col_dircos_x; 


dst_pos_x = dst_row_dircos_x * src_pos_x
          + dst_row_dircos_y * src_pos_y
          + dst_row_dircos_z * src_pos_z;

dst_pos_y = dst_col_dircos_x * src_pos_x
          + dst_col_dircos_y * src_pos_y
          + dst_col_dircos_z * src_pos_z;

dst_pos_z = dst_nrm_dircos_x * src_pos_x
          + dst_nrm_dircos_y * src_pos_y
          + dst_nrm_dircos_z * src_pos_z;

//Translation 1
dst_pos_x = dst_pos_x + infoImg1->ImPoPa[0];
dst_pos_y = dst_pos_y + infoImg1->ImPoPa[1];
dst_pos_z = dst_pos_z + infoImg1->ImPoPa[2];

//Tranalacion  - 2
dst_pos_x = dst_pos_x - infoImg2->ImPoPa[0];
dst_pos_y = dst_pos_y - infoImg2->ImPoPa[1];
dst_pos_z = dst_pos_z - infoImg2->ImPoPa[2];


//Rotation - 2
dst_row_dircos_x = infoImg2->ImOrPa[0];
dst_row_dircos_y = infoImg2->ImOrPa[1];
dst_row_dircos_z = infoImg2->ImOrPa[2];

dst_col_dircos_x = infoImg2->ImOrPa[3];
dst_col_dircos_y = infoImg2->ImOrPa[4];
dst_col_dircos_z = infoImg2->ImOrPa[5];

dst_nrm_dircos_x = dst_row_dircos_y * dst_col_dircos_z - dst_row_dircos_z * dst_col_dircos_y; 
dst_nrm_dircos_y = dst_row_dircos_z * dst_col_dircos_x - dst_row_dircos_x * dst_col_dircos_z; 
dst_nrm_dircos_z = dst_row_dircos_x * dst_col_dircos_y - dst_row_dircos_y * dst_col_dircos_x; 


in[0] = dst_pos_x;
in[1] = dst_pos_y;
in[2] = dst_pos_z;
in[3] = 1;
vtkMatrix4x4 *mat=vtkMatrix4x4::New();
mat->Identity ();

/*
mat->SetElement(0,0,dst_row_dircos_x);
mat->SetElement(0,1,dst_row_dircos_y);
mat->SetElement(0,2,dst_row_dircos_z);
mat->SetElement(0,3,0);

mat->SetElement(1,0,dst_col_dircos_x);
mat->SetElement(1,1,dst_col_dircos_y);
mat->SetElement(1,2,dst_col_dircos_z);
mat->SetElement(1,3,0);

mat->SetElement(2,0,dst_nrm_dircos_x);
mat->SetElement(2,1,dst_nrm_dircos_y);
mat->SetElement(2,2,dst_nrm_dircos_z);
mat->SetElement(2,3,0);

mat->SetElement(3,0,0);
mat->SetElement(3,1,0);
mat->SetElement(3,2,0);
mat->SetElement(3,3,1);

*/

mat->SetElement(0,0,dst_row_dircos_x);
mat->SetElement(1,0,dst_row_dircos_y);
mat->SetElement(2,0,dst_row_dircos_z);
mat->SetElement(3,0,0);

mat->SetElement(0,1,dst_col_dircos_x);
mat->SetElement(1,1,dst_col_dircos_y);
mat->SetElement(2,1,dst_col_dircos_z);
mat->SetElement(3,1,0);

mat->SetElement(0,2,dst_nrm_dircos_x);
mat->SetElement(1,2,dst_nrm_dircos_y);
mat->SetElement(2,2,dst_nrm_dircos_z);
mat->SetElement(3,2,0);

mat->SetElement(0,3,0);
mat->SetElement(1,3,0);
mat->SetElement(2,3,0);
mat->SetElement(3,3,1);




/*
vtkTransform *transform2 = vtkTransform::New();
transform2->Identity();
transform2->SetMatrix(mat);
transform2->Update();
*/


vtkMatrix4x4 *matB			= vtkMatrix4x4::New();
vtkTransform *transform3	= vtkTransform::New();
matB->Identity ();
mat->Invert(mat,matB);
//transform2->GetInverse(matB);
transform3->SetMatrix(matB);
transform3->Update();

double res=0;
FILE *hh=fopen("c:/Temp/Matrix.txt","w");
for (int i=0; i<4; i++)
{
	for (int j=0; j<4; j++)
	{
		res=0;
		for (int ii=0;ii<4;ii++)
		{
			res = res + mat->GetElement(ii,j) * matB->GetElement(i,ii);
		}
	fprintf(hh,"%f ",res);
	}
}
fclose(hh);

transform3->MultiplyPoint(in,out);
transform3->Delete();
mat->Delete();
matB->Delete();

	//Scale 1/2
out[0] = out[0] / infoImg2->Spacing[0];
out[1] = out[1] / infoImg2->Spacing[1];
out[2] = out[2] / 1.0;

//--

pOut[0] = (int)(out[0] + 0.5);
pOut[1] = (int)(out[1] + 0.5);
pOut[2] = (int)(out[2] + 0.5);
}



//-----------------------------------------------------------------------------

void wxMaracasEmptyPanel_4::TranformPixel2a(int pIn[], int pOut[], struct InfoImag *infoImg1, struct InfoImag *infoImg2)
{
	double in[4],out[4];

	//Scale 1
	double src_pos_x = (double)pIn[0] * infoImg1->Spacing[0];
	double src_pos_y = (double)pIn[1] * infoImg1->Spacing[1];
	double src_pos_z = (double)pIn[2] * 1;



//Rotation 1
double dst_nrm_dircos_x;
double dst_nrm_dircos_y;
double dst_nrm_dircos_z;

double dst_pos_x;
double dst_pos_y;
double dst_pos_z;

double dst_row_dircos_x = infoImg1->ImOrPa[0];
double dst_row_dircos_y = infoImg1->ImOrPa[1];
double dst_row_dircos_z = infoImg1->ImOrPa[2];

double dst_col_dircos_x = infoImg1->ImOrPa[3];
double dst_col_dircos_y = infoImg1->ImOrPa[4];
double dst_col_dircos_z = infoImg1->ImOrPa[5];

dst_nrm_dircos_x = dst_row_dircos_y * dst_col_dircos_z - dst_row_dircos_z * dst_col_dircos_y; 
dst_nrm_dircos_y = dst_row_dircos_z * dst_col_dircos_x - dst_row_dircos_x * dst_col_dircos_z; 
dst_nrm_dircos_z = dst_row_dircos_x * dst_col_dircos_y - dst_row_dircos_y * dst_col_dircos_x; 

in[0] = src_pos_x;
in[1] = src_pos_y;
in[2] = src_pos_z;
in[3] = 0;
vtkMatrix4x4 *mat=vtkMatrix4x4::New();

mat->SetElement(0,0,dst_row_dircos_x);
mat->SetElement(0,1,dst_row_dircos_y);
mat->SetElement(0,2,dst_row_dircos_z);
mat->SetElement(0,3,0);

mat->SetElement(1,0,dst_col_dircos_x);
mat->SetElement(1,1,dst_col_dircos_y);
mat->SetElement(1,2,dst_col_dircos_z);
mat->SetElement(1,3,0);

mat->SetElement(2,0,dst_nrm_dircos_x);
mat->SetElement(2,1,dst_nrm_dircos_y);
mat->SetElement(2,2,dst_nrm_dircos_z);
mat->SetElement(2,3,0);

mat->SetElement(3,0,0);
mat->SetElement(3,1,0);
mat->SetElement(3,2,0);
mat->SetElement(3,3,1);

/*

mat->SetElement(0,0,dst_row_dircos_x);
mat->SetElement(1,0,dst_row_dircos_y);
mat->SetElement(2,0,dst_row_dircos_z);
mat->SetElement(3,0,0);

mat->SetElement(0,1,dst_col_dircos_x);
mat->SetElement(1,1,dst_col_dircos_y);
mat->SetElement(2,1,dst_col_dircos_z);
mat->SetElement(3,1,0);

mat->SetElement(0,2,dst_nrm_dircos_x);
mat->SetElement(1,2,dst_nrm_dircos_y);
mat->SetElement(2,2,dst_nrm_dircos_z);
mat->SetElement(3,2,0);

mat->SetElement(0,3,0);
mat->SetElement(1,3,0);
mat->SetElement(2,3,0);
mat->SetElement(3,3,1);
*/

vtkMatrix4x4 *matB			= vtkMatrix4x4::New();
vtkTransform *transform3	= vtkTransform::New();
mat->Invert(mat,matB);
transform3->SetMatrix(matB);
transform3->Update();

double res=0;
FILE *hh=fopen("c:/Temp/Matrix.txt","w");
for (int i=0; i<4; i++)
{
	for (int j=0; j<4; j++)
	{
		res=0;
		for (int ii=0;ii<4;ii++)
		{
			res = res + mat->GetElement(ii,j) * matB->GetElement(i,ii);
		}
	fprintf(hh,"%f ",res);
	}
}
fclose(hh);

transform3->MultiplyPoint(in,out);
transform3->Delete();
mat->Delete();
matB->Delete();


//Translation 1
dst_pos_x = out[0] + infoImg1->ImPoPa[0];
dst_pos_y = out[0] + infoImg1->ImPoPa[1];
dst_pos_z = out[0] + infoImg1->ImPoPa[2];

//Tranalacion  - 2
src_pos_x = dst_pos_x - infoImg2->ImPoPa[0];
src_pos_y = dst_pos_y - infoImg2->ImPoPa[1];
src_pos_z = dst_pos_z - infoImg2->ImPoPa[2];




//Rotation - 2
dst_row_dircos_x = infoImg2->ImOrPa[0];
dst_row_dircos_y = infoImg2->ImOrPa[1];
dst_row_dircos_z = infoImg2->ImOrPa[2];

dst_col_dircos_x = infoImg2->ImOrPa[3];
dst_col_dircos_y = infoImg2->ImOrPa[4];
dst_col_dircos_z = infoImg2->ImOrPa[5];

dst_nrm_dircos_x = dst_row_dircos_y * dst_col_dircos_z - dst_row_dircos_z * dst_col_dircos_y; 
dst_nrm_dircos_y = dst_row_dircos_z * dst_col_dircos_x - dst_row_dircos_x * dst_col_dircos_z; 
dst_nrm_dircos_z = dst_row_dircos_x * dst_col_dircos_y - dst_row_dircos_y * dst_col_dircos_x; 


dst_pos_x = dst_row_dircos_x * src_pos_x
          + dst_row_dircos_y * src_pos_y
          + dst_row_dircos_z * src_pos_z;

dst_pos_y = dst_col_dircos_x * src_pos_x
          + dst_col_dircos_y * src_pos_y
          + dst_col_dircos_z * src_pos_z;

dst_pos_z = dst_nrm_dircos_x * src_pos_x
          + dst_nrm_dircos_y * src_pos_y
          + dst_nrm_dircos_z * src_pos_z;



	//Scale 1/2
out[0]=dst_pos_x / infoImg2->Spacing[0];
out[1]=dst_pos_y / infoImg2->Spacing[1];
out[2]=dst_pos_z / 1.0;

//--

pOut[0] = (int)(out[0] + 0.5);
pOut[1] = (int)(out[1] + 0.5);
pOut[2] = (int)(out[2] + 0.5);
}


//----------------------------------------------------------------------------


void wxMaracasEmptyPanel_4::TranformPixel3(int pIn[], int pOut[], struct InfoImag *infoImg1, struct InfoImag *infoImg2)
{

	manualPoint *p1a = _widgetMesure1->GetManualContourModel()->GetManualPoint(0);	
	manualPoint *p1b = _widgetMesure1->GetManualContourModel()->GetManualPoint(1);	

	manualPoint *p2a = _widgetMesure2->GetManualContourModel()->GetManualPoint(0);	
	manualPoint *p2b = _widgetMesure2->GetManualContourModel()->GetManualPoint(1);	

    double pp1aX = p1a->GetX() * infoImg1->Spacing[0];
    double pp1aY = p1a->GetY() * infoImg1->Spacing[1];
    double pp1bX = p1b->GetX() * infoImg1->Spacing[0];
    double pp1bY = p1b->GetY() * infoImg1->Spacing[1];

    double pp2aX = p2a->GetX() * infoImg2->Spacing[0];
    double pp2aY = p2a->GetY() * infoImg2->Spacing[1];
    double pp2bX = p2b->GetX() * infoImg2->Spacing[0];
    double pp2bY = p2b->GetY() * infoImg2->Spacing[1];

	double ang1= atan2( pp1bY-pp1aY , pp1bX-pp1aX )*180/PI;
	double ang2= atan2( pp2bY-pp2aY , pp2bX-pp2aX )*180/PI;

	double in[4],inB[4],out[4];
	in[0] = (double)pIn[0] * infoImg1->Spacing[0];
	in[1] = (double)pIn[1] * infoImg1->Spacing[1];
	in[2] = 0;
	in[3] = 1;

	in[0]=in[0]-pp1aX;
	in[1]=in[1]-pp1aY;

	vtkTransform *transform; 
	transform = vtkTransform::New();
	transform->Identity();
//	transform->RotateZ( ang2 );
	transform->RotateZ( -ang1 );
	transform->MultiplyPoint(in,out);
	transform->Delete();

	inB[0] = out[0];
	inB[1] = out[1];
	inB[2] = out[2];
	inB[3] = out[3];
	vtkTransform *transform2; 
	transform2 = vtkTransform::New();
	transform2->Identity();
	transform2->RotateZ( ang2 );
//	transform2->RotateZ( ang1 );
	transform2->MultiplyPoint(inB,out);
	transform2->Delete();

	out[0] = out[0] + pp2aX;
	out[1] = out[1] + pp2aY;

	out[0] = out[0] / infoImg2->Spacing[0];
	out[1] = out[1] / infoImg2->Spacing[1];

	pOut[0]= (int)(out[0] + 0.5);	
	pOut[1]= (int)(out[1] + 0.5);	
	pOut[2]= 0;	



}


// EOF - wxMaracasEmptyPanel_3.cxx