/*
--------------------
Program: ESQUI
Info and Bugs: {marf,rsantana}@ctm.ulpgc.es 
-------------------------------------------

Copyright (c) 2006, Center for Technology in Medicine, University of Las Palmas
de Gran Canaria (Canary Islands, Spain).
All rights reserved.

This program is free software; you can redistribute it and/or modify it under 
the terms of the Creative Commons' "by-attribution" license 
(http://creativecommons.org/licenses/by/2.5).

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

1) Redistributions of source code must retain the above copyright
   notice, this list of conditions and the following disclaimer.
2) Redistributions in binary form must reproduce the above copyright 
   notice, this list of conditions and the following disclaimer in the
   documentation and/or other materials provided with the distribution.


THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS''
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
=============================================================================*/


#include "vtkColDetect.h"//
#include "Inversa.h"//
#include "vtkModeloDeformable.h"
#include "vtkModeloHerramienta.h"
#include "vtkModeloEstatico.h"
#include "vtkInfoColision.h"
#include "vtkInfoColisionCollection.h"
#include "vtkObjectFactory.h"


#include "vtkSphereSource.h"
#include "vtkPolyDataMapper.h"
#include "vtkActor.h"
#include "vtkRenderer.h"


// Este valor de limite de cercania es el que activa el calculo
// real de las colisiones, cuando la distancia entre dos objetos
// es menor que este valor
static const double LIMITE_CERCANIA = 1;
static const float	UMBRAL_SEGURIDAD= 0.01f;


using namespace MG;


vtkCxxRevisionMacro(vtkColDetect, "$Revision: 0.1 $");
vtkStandardNewMacro(vtkColDetect);

vtkColDetect::vtkColDetect(){

}

vtkColDetect::~vtkColDetect(){
	IdDeformables.clear();
	IdHerramientas.clear();
	IdEstaticos.clear();
}

bool vtkColDetect::inicializar(){
	return true;
}

unsigned vtkColDetect::obtenerIdColisiones(unsigned idOrgano, unsigned idH, unsigned idCaja, unsigned tipoEnvolvente){
// Para convertir los identificadores del organo y la herramienta a lo identificadores usados por sundaraj

	unsigned id1 = idDeformable(idOrgano);
	unsigned id2 = idEnvolventeCajaHerramienta(idH, idCaja, tipoEnvolvente);


unsigned idColision = collideDeformable(id1, id2, true);

	return idColision;
}

void vtkColDetect::crearArbolesColisionOrgano(unsigned idOrgano)
{	
	
	
	unsigned id = idDeformable(idOrgano);
	createBoundingBoxTree(id);




}

void vtkColDetect::crearArbolesColisionHerramienta(unsigned idH, unsigned idCaja, unsigned tipoEnvolvente)
{
	unsigned id = idEnvolventeCajaHerramienta(idH, idCaja, tipoEnvolvente);
	createBoundingBoxTree(id);
}

unsigned vtkColDetect::posIdColisiones(unsigned idOrgano, unsigned idH, unsigned idCaja){
	unsigned idColision = 18*idOrgano+9*idH+3*idCaja+IdHerramientas[idH].tipoEnvolventeMovimiento[idCaja];

	return idColision;
}

int vtkColDetect::anadirDeformable(vtkModeloDeformable* pDeformable){
	// Creacion del objeto y de su identificador
	unsigned id = createObject();
	// Guardar los identificadores locales para facilitar el uso.
	IdDeformables.push_back(id);
	// Definicion del objeto
	
	beginObject(id, pDeformable->GetnumeroCaras(), pDeformable->GetnumeroVertices());			//Se crean los vectores para almacenar los puntos y las caras
		
	// Configuracion de los vertices del objeto
	double* vectorDeVertices = new double[3*(pDeformable->GetnumeroVertices())]; //creacion del vector
	convertirADouble(pDeformable->vertices, 3*(pDeformable->GetnumeroVertices()), vectorDeVertices);
	setVertices(id, vectorDeVertices);
	delete[] vectorDeVertices; //destruccion del vector
	// Configuracion de las caras del objeto
	for (unsigned i=0; i<pDeformable->GetnumeroCaras(); i++)
		addTriangle(id, i, 
				static_cast<unsigned> (pDeformable->caras[i*3]), 
				static_cast<unsigned> (pDeformable->caras[i*3 + 1]), 
				static_cast<unsigned> (pDeformable->caras[i*3 + 2]));
	// Final de la construccion del objeto
	endObject(id);
	
	pDeformable->verticesAnterior=new float[3*(pDeformable->GetnumeroVertices())];

	memcpy(pDeformable->verticesAnterior,pDeformable->vertices,3*pDeformable->GetnumeroVertices()*sizeof(float)); 
	
	//Configurar la matrix de transformacion del objeto
	double** matriz;
	matriz = new (double*);
	(*matriz) = new double[TAM_MATRIZ_TRANS];
	convertirMatrizTransformacion(pDeformable->Caja.matriz, TAM_MATRIZ_TRANS, matriz);
	setTransformationMatrix(id, matriz);
	VectorDeDeformables.push_back(pDeformable);
	return 0;
}

int vtkColDetect::anadirHerramienta(vtkModeloHerramienta* pHerramienta){

	// Para la libreria de deteccion no existe el objeto herramienta,
	//	ni sus cajas, solo las ENVOLVENTES DE MOVIMIENTO DE LAS CAJAS

	// Guardar los identificadores locales para facilitar el uso.
	tipoHerramienta datos;
	datos.numCajas = pHerramienta->VectorDeCajas.size();
		
	for(unsigned cajaActual=0; cajaActual<pHerramienta->VectorDeCajas.size(); cajaActual++)
	{
		//Configurar la matrix de transformacion del objeto. La de la caja sea cual sea el tipo de envolvente.
		double** matriz;        
		matriz = new (double*);
		(*matriz) = new double[TAM_MATRIZ_TRANS];	// No hace falta liberarla luego
		double** matrizActual;        
		matrizActual = new (double*);
		(*matrizActual) = new double[TAM_MATRIZ_TRANS]; // Esta si hay que liberarla
		convertirMatrizTransformacion(pHerramienta->VectorDeCajas[cajaActual].matriz, TAM_MATRIZ_TRANS, matrizActual);

		
		// PARA CADA CAJA TENGO QUE CREAR TRES OBJETOS-->LAS TRES POSIBLES TOPOLOGIAS 
		// DE LA ENVOLVENTE DE MOVIMIENTO:
		// Creacion del objeto y de su identificador
		
		// ENVOLVENTE DE 8 VERTICES (movimiento en un solo plano)
		unsigned id = createObject();
		datos.idCajas[cajaActual*3+0]=id;
		// Definicion del objeto
		// Hay que anadir un objeto a la libreria por cada topologia de cada caja de inclusion


		beginObject(id, /*caras*/6*2, /*vertices*/8);

        // Configuracion de los vertices del objeto IGUAL QUE UNA CAJA NORMAL
		double vectorDeVerticesCaja[24];
		crearVectorVertices(vectorDeVerticesCaja, pHerramienta, cajaActual);
		double vectorDeVertices[24];
		unsigned int i;
		for( i=0; i<8; i++){
			vectorDeVertices[3*i]=(*matrizActual)[0]*vectorDeVerticesCaja[3*i]+
									(*matrizActual)[4]*vectorDeVerticesCaja[3*i+1]+
									(*matrizActual)[8]*vectorDeVerticesCaja[3*i+2]+
									(*matrizActual)[12];
			vectorDeVertices[3*i+1]=(*matrizActual)[1]*vectorDeVerticesCaja[3*i]+
									(*matrizActual)[5]*vectorDeVerticesCaja[3*i+1]+
									(*matrizActual)[9]*vectorDeVerticesCaja[3*i+2]+
									(*matrizActual)[13];
			vectorDeVertices[3*i+2]=(*matrizActual)[2]*vectorDeVerticesCaja[3*i]+
									(*matrizActual)[6]*vectorDeVerticesCaja[3*i+1]+
									(*matrizActual)[10]*vectorDeVerticesCaja[3*i+2]+
									(*matrizActual)[14];
		}
		setVertices(id, vectorDeVertices);
		
		configurarCarasHerramienta(id);
		endObject(id);	
		
		(*matriz)[0]=1; (*matriz)[1]=0; (*matriz)[2]=0; (*matriz)[3]=0;
		(*matriz)[4]=0;(*matriz)[5]=1;(*matriz)[6]=0;(*matriz)[7]=0;
		(*matriz)[8]=0;(*matriz)[9]=0;(*matriz)[10]=1;(*matriz)[11]=0;
		(*matriz)[12]=0;(*matriz)[13]=0;(*matriz)[14]=0;(*matriz)[15]=0;
		setTransformationMatrix(id, matriz);
			
		//ENVOLVENTE DE 12 VERTICES (movimiento en dos ejes)
		unsigned id1 = createObject();
		datos.idCajas[cajaActual*3+1]=id1;
		beginObject(id1, /*caras*/20, /*vertices*/12);
		// Configuracion de los vertices del objeto 
		double vectorDeVertices12[36];
		crearVectorDoceVertices(vectorDeVertices12, pHerramienta, cajaActual);
		setVertices(id1, vectorDeVertices12);
		
		configurarCarasEnvolventeDoce(id1);
		endObject(id1);
			
		// Cuando se llama a este metodo ya se libera la memoria de la matriz antigua dentro
		// No hace falta liberarla luego
		setTransformationMatrix(id1, matriz);
		
		//ENVOLVENTE DE 14 VERTICES (movimiento en los tres ejes)
		unsigned id2 = createObject();
		datos.idCajas[cajaActual*3+2]=id2;
		beginObject(id2, /*caras*/24, /*vertices*/14);
		// Configuracion de los vertices del objeto 
		double vectorDeVertices14[42];
		crearVectorCatorceVertices(vectorDeVertices14, pHerramienta, cajaActual);
		setVertices(id2, vectorDeVertices14);
		
		configurarCarasEnvolventeCatorce(id2);
		endObject(id2);

		setTransformationMatrix(id2, matriz);
		
		// La MatrizActual hay que liberarla
		delete[] (*matrizActual);
		delete[] matrizActual;
		
		double posAnterior[3]={0,0,0};
		// ******Obtengo el centroide de la caja para indicar la posicion en el instante anterior;
		// AUNQUE SE USE LA ENVOLVENTE DE MOVIMIENTO SE CONSIDERA LA POSICION ANTERIOR DE LA CAJA, NO DE 
		// LA ENVOLVENTE
		for (i=0; i<8; i++){
			posAnterior[0]=posAnterior[0]+vectorDeVertices[i*3];
			posAnterior[1]=posAnterior[1]+vectorDeVertices[i*3+1];
			posAnterior[2]=posAnterior[2]+vectorDeVertices[i*3+2];
		}
			
		for (i=0; i<3; i++){
			posAnterior[i]=posAnterior[i]/8; // 8 -->numero de vertices:
		}
				
		//coordVertices(id+cajaActual,0,posAnterior);
		datos.posAnterior[cajaActual*3] = posAnterior[0];
		datos.posAnterior[cajaActual*3 + 1] = posAnterior[1];
		datos.posAnterior[cajaActual*3 + 2] = posAnterior[2];

		//*** EvitarNormalesOrgano
		for (i=0; i<9; i++){
			// Inicialmente no se ha deformado ninguna cara del órgano
			datos.normalCarasOrganoDeformadas[i] = 0;
		}
		//*** /EvitarNormalesOrgano
		datos.tipoEnvolventeMovimiento[cajaActual]=0;
	}
	// Guarda los valores necesarios para el tratamiento de las herramientas
	// Se hace aqui abajo, para poder introducir uno a uno los valores de 
	// posicion anterior de cada una de las cajas de inclusion.
	IdHerramientas.push_back(datos);
	return 0;
}// fin anadirHerramienta()

int vtkColDetect::anadirEstatico(vtkModeloEstatico* pEstatico){
	// Creacion del objeto y de su identificador
	unsigned id = createObject();
		// Guardar los identificadores locales para facilitar el uso.
	IdEstaticos.push_back(id);
	// Definicion del objeto
	beginObject(id, /*caras*/6*2, /*vertices*/8);
	// Configuracion de los vertices del objeto
	double vectorDeVertices[/*vertices x 3*/24]; //creacion del vector
	crearVectorVertices(vectorDeVertices, pEstatico);
	setVertices(id, vectorDeVertices);
	configurarCarasEstatico(id);

	// Final de la construccion del objeto
	endObject(id);

	//Configurar la matrix de transformacion del objeto
	double** matriz;
	matriz = new (double*);
	(*matriz) = new double[TAM_MATRIZ_TRANS];
	convertirMatrizTransformacion(pEstatico->Caja.matriz, TAM_MATRIZ_TRANS, matriz);
	setTransformationMatrix(id, matriz);

	return 0;
}

bool vtkColDetect::eliminarDeformable(int pos){
	return false;
}

bool vtkColDetect::eliminarHerramienta(int pos){
	return false;
}

bool vtkColDetect::eliminarEstatico(int pos){
	return false;
}

bool vtkColDetect::insertarDeformable(int pos,vtkModeloDeformable* pDeformable){

	// Obtener el identificador del objeto
	unsigned id = IdDeformables[pos];
	VectorDeDeformables[pos] = pDeformable;


	// Configuracion de los vertices del objeto
	double* vectorDeVertices = new double[3*(pDeformable->GetnumeroVertices())]; //creacion del vector
	convertirADouble(pDeformable->vertices, 3*(pDeformable->GetnumeroVertices()), vectorDeVertices);

	
	
	for(unsigned i=0; i<(pDeformable->GetnumVerticesMovidos()); i++){
		unsigned idVerticeMovido=static_cast<unsigned> (pDeformable->idVerticesMovidos[i]);
		updateVertex(id, idVerticeMovido, 
					 vectorDeVertices[3*idVerticeMovido], 
					 vectorDeVertices[3*idVerticeMovido + 1],
                     vectorDeVertices[3*idVerticeMovido + 2]);
	}
	

	delete[] vectorDeVertices; //destruccion del vector
 

	//Configurar la matrix de transformacion del objeto
	double** matriz;
	matriz = new (double*);
	(*matriz) = new double[TAM_MATRIZ_TRANS]; // No hay que liberarla, porque se libera en setTransformationMatrix
	convertirMatrizTransformacion(pDeformable->Caja.matriz, TAM_MATRIZ_TRANS, matriz);
	setTransformationMatrix(id, matriz);

	return true;
}//fin insertarDeformable()

bool vtkColDetect::insertarHerramienta(int pos,vtkModeloHerramienta* pHerramienta, bool bNueva){
	vtkSphereSource *Esfera;
	vtkPolyDataMapper *Mapper;
	vtkActor *Actor;
	if (bNueva){
		anadirHerramienta(pHerramienta);
		return true;
	}

	for(unsigned cajaActual=0; cajaActual<pHerramienta->VectorDeCajas.size(); cajaActual++)
	{
		double** matrizActual;
		matrizActual = new (double*);
		(*matrizActual) = new double[TAM_MATRIZ_TRANS]; // Esta matriz hay que liberarla al final
		convertirMatrizTransformacion(pHerramienta->VectorDeCajas[cajaActual].matriz, TAM_MATRIZ_TRANS, matrizActual);

		//En vectorDeVertices tenemos las coordenadas locales. (iniciales)
		double vectorDeVertices[24];
		crearVectorVertices(vectorDeVertices, pHerramienta, cajaActual);

		//Identificadores de los tres tipos de envolvente de cada caja.
		unsigned idActualEnv0 = IdHerramientas[pos].idCajas[cajaActual*3];		 
		unsigned idActualEnv1 = IdHerramientas[pos].idCajas[cajaActual*3+1];	 
		unsigned idActualEnv2 = IdHerramientas[pos].idCajas[cajaActual*3+2];	 
	
		double vVerticesActual[24];	// Coord de los vertices de la caja en t
		// Multiplicar las coordenadas locales por la matriz de transformacion 
		// para obtener los vertices actuales
		unsigned int i;
		for(i=0; i<8; i++){
			IdHerramientas[pos].vVerticesActual[cajaActual*24 + 3*i]= vVerticesActual[i*3] =	(*matrizActual)[0] * vectorDeVertices[i*3] + 
									(*matrizActual)[4] * vectorDeVertices[i*3+1] + 
									(*matrizActual)[8] * vectorDeVertices[i*3+2] + 
									(*matrizActual)[12];
			IdHerramientas[pos].vVerticesActual[cajaActual*24 + 3*i+1] = vVerticesActual[i*3+1] =(*matrizActual)[1] * vectorDeVertices[i*3] + 
									(*matrizActual)[5] * vectorDeVertices[i*3+1] + 
									(*matrizActual)[9] * vectorDeVertices[i*3+2] + 
									(*matrizActual)[13];
			IdHerramientas[pos].vVerticesActual[cajaActual*24 + 3*i+2] = vVerticesActual[i*3+2] =(*matrizActual)[2] * vectorDeVertices[i*3] + 
									(*matrizActual)[6] * vectorDeVertices[i*3+1] + 
									(*matrizActual)[10]* vectorDeVertices[i*3+2] + 
									(*matrizActual)[14];
		}
		

		//Calculo el centroActual para obtener el vector de movimiento:
		double centroActualLocal[3]={0,0,0};
		char j;
		for(i=0; i<8; i++){
			for(j=0; j<3; j++){
				centroActualLocal[j]=centroActualLocal[j]+vectorDeVertices[i*3+j];
			}
		}
		for(j=0; j<3; j++){
			centroActualLocal[j]=centroActualLocal[j]/8.0f;
		}


		double centroActual[3]={0,0,0};
		for(i=0; i<8; i++){
			for(j=0; j<3; j++){
				centroActual[j]=centroActual[j]+vVerticesActual[i*3+j];
			}
		}
		for(j=0; j<3; j++){
			centroActual[j]=centroActual[j]/8.0f;
		}


		double matrizInversa[16];

		double aux[3];
		

		aux[0] = IdHerramientas[pos].posAnterior[cajaActual*3];
		aux[1] = IdHerramientas[pos].posAnterior[cajaActual*3+1];
		aux[2] = IdHerramientas[pos].posAnterior[cajaActual*3+2];
		

		calculaMatrizInversa(matrizInversa, (*matrizActual));
		double centroAnteriorLocal[3];

		centroAnteriorLocal[0] =	matrizInversa[0] * aux[0] + 
									matrizInversa[4] * aux[1] + 
									matrizInversa[8] * aux[2] + 
									matrizInversa[12];
		centroAnteriorLocal[1] =	matrizInversa[1] * aux[0] + 
									matrizInversa[5] * aux[1] + 
									matrizInversa[9] * aux[2] + 
									matrizInversa[13];
		centroAnteriorLocal[2] =	matrizInversa[2] * aux[0] + 
									matrizInversa[6] * aux[1] + 
									matrizInversa[10]* aux[2] + 
									matrizInversa[14];
	
		double moduloVectorDeMovimiento=0;
		double vectorDeMovimientoCaja[3]={0,0,0};
		double vectorDeMovimientoGlobal[3]={0,0,0};
	
		for(i=0; i<3; i++){
			vectorDeMovimientoCaja[i]= centroActualLocal[i]-centroAnteriorLocal[i];
			if(fabs(vectorDeMovimientoCaja[i])<UMBRALMOVIMIENTO) vectorDeMovimientoCaja[i]=0.0f;
			IdHerramientas[pos].vectorDeMovimiento[cajaActual*3+i]= centroActual[i]-IdHerramientas[pos].posAnterior[cajaActual*3+i];
			if(fabs(IdHerramientas[pos].vectorDeMovimiento[cajaActual*3+i])<UMBRALMOVIMIENTO) IdHerramientas[pos].vectorDeMovimiento[cajaActual*3+i]=0.0f;
			moduloVectorDeMovimiento = IdHerramientas[pos].vectorDeMovimiento[cajaActual*3+i]*IdHerramientas[pos].vectorDeMovimiento[cajaActual*3+i]+moduloVectorDeMovimiento;
		}

		moduloVectorDeMovimiento=sqrt(moduloVectorDeMovimiento);


		if(true){
			char i;
			for(i=0; i<8; i++){
				updateVertex(idActualEnv0, i, 
							 vVerticesActual[3*i], 
							 vVerticesActual[3*i + 1],
							 vVerticesActual[3*i + 2]);
			
			}
		

			IdHerramientas[pos].tipoEnvolventeMovimiento[cajaActual] = 0;
		}
		else{
		// La caja sí se ha movido o la caja anterior no tenia 8 vertices:
		// Hay que actualizar todos los vertices
		
			//Obtengo la posicion anterio de los vertices a partir del vector de movimiento:
			double* vVerticesAnterior = new double[3*8];	// Coord de los vértices de la caja en t-1
			char i;
			for(i=0; i<8; i++){
				char j;
				for(j=0; j<3; j++){
					vVerticesAnterior[i*3+j]=vectorDeVertices[i*3+j]-vectorDeMovimientoCaja[j];
				}
			}
			
			//Crear el objeto EnvolventeTriangulada y construirlo
			vtkEnvolventeTriangulada *ET = new vtkEnvolventeTriangulada();
			double* vVerticesEnvolvente;
			if( fabs(vectorDeMovimientoCaja[0])>UMBRALMOVIMIENTO && 
				fabs(vectorDeMovimientoCaja[1])>UMBRALMOVIMIENTO && 
				fabs(vectorDeMovimientoCaja[2])>UMBRALMOVIMIENTO) {
					vVerticesEnvolvente=new double[42]; //14*3
					for(i=0;i<42;i++)
					{
						vVerticesEnvolvente[i]=0.0;
					}
					IdHerramientas[pos].tipoEnvolventeMovimiento[cajaActual] = 2;
					//setTransformationMatrix(idActualEnv2, matriz);
			}
			else if((fabs(vectorDeMovimientoCaja[0])>UMBRALMOVIMIENTO && 
					(fabs(vectorDeMovimientoCaja[1])>UMBRALMOVIMIENTO ||
					fabs(vectorDeMovimientoCaja[2])>UMBRALMOVIMIENTO))||
					(fabs(vectorDeMovimientoCaja[1])>UMBRALMOVIMIENTO &&
					fabs(vectorDeMovimientoCaja[2])>UMBRALMOVIMIENTO)){
						vVerticesEnvolvente=new double[36]; //12*3
						
						for(i=0;i<36;i++)
						{
							vVerticesEnvolvente[i]=0.0;
						}
						IdHerramientas[pos].tipoEnvolventeMovimiento[cajaActual] = 1;
						//setTransformationMatrix(idActualEnv1, matriz);
			}else{
				vVerticesEnvolvente=new double[24]; //8*3
				for(i=0;i<24;i++)
				{
					vVerticesEnvolvente[i]=0.0;
				}
				IdHerramientas[pos].tipoEnvolventeMovimiento[cajaActual] = 0;
			}
			
			ET->construyeEnvolvente(vVerticesAnterior, vectorDeVertices, vectorDeMovimientoCaja, (*matrizActual),vVerticesEnvolvente);
		
			if (IdHerramientas[pos].tipoEnvolventeMovimiento[cajaActual] == 0){
				char i;
				for(i=0; i<8; i++){
					updateVertex(idActualEnv0, i, 
								 vVerticesEnvolvente[3*i], 
								 vVerticesEnvolvente[3*i + 1],
								 vVerticesEnvolvente[3*i + 2]);
				
				}
			
			}
			else if(IdHerramientas[pos].tipoEnvolventeMovimiento[cajaActual] == 1)
			{
				char i;
				for(i=0; i<12; i++){
					updateVertex(idActualEnv1, i, 
								 vVerticesEnvolvente[3*i], 
								 vVerticesEnvolvente[3*i + 1],
								 vVerticesEnvolvente[3*i + 2]);
				
				}	
			}
			else if(IdHerramientas[pos].tipoEnvolventeMovimiento[cajaActual] == 2){
				char i;
				for(i=0; i<14; i++){
					updateVertex(idActualEnv2, i, 
								 vVerticesEnvolvente[3*i], 
								 vVerticesEnvolvente[3*i + 1],
								 vVerticesEnvolvente[3*i + 2]);
					
				}
			
			}
			delete[] vVerticesEnvolvente;
		}
			
				
		// Actualizar el valor de la posición anterior de la caja de inclusion
		IdHerramientas[pos].posAnterior[cajaActual*3] = centroActual[0];
		IdHerramientas[pos].posAnterior[cajaActual*3 + 1] = centroActual[1];
		IdHerramientas[pos].posAnterior[cajaActual*3 + 2] = centroActual[2];
		delete[] (*matrizActual);
		delete[] matrizActual;
	}	
	return true;
}//fin insertarHerramienta()

bool vtkColDetect::insertarEstatico(int pos,vtkModeloEstatico* pEstatico){

	// Obtener el identificador del objeto
	unsigned id = IdEstaticos[pos];
	
	// Configuracion de los vertices del objeto
	double vectorDeVertices[/*vertices x 3*/24]; //creacion del vector
	crearVectorVertices(vectorDeVertices, pEstatico);
	setVertices(id, vectorDeVertices);
	configurarCarasEstatico(id);
	
	//Configurar la matrix de transformacion del objeto
	double** matriz;
	matriz = new (double*);
	(*matriz) = new double[TAM_MATRIZ_TRANS];	// No hay que liberarla, porque se libera en setTransformationMatrix
	convertirMatrizTransformacion(pEstatico->Caja.matriz, TAM_MATRIZ_TRANS, matriz);
	setTransformationMatrix(id, matriz);

	return true;
}

bool vtkColDetect::detectarColisionOrganoHerramienta(unsigned idColision, unsigned idOrgano, unsigned idH, unsigned idCaja, vtkInfoColisionCollection* vColisiones, std::vector<unsigned> &listaVerticesOrgano, vtkOBB caja){

	unsigned id1 = idDeformable(idOrgano);
	unsigned id2 = idEnvolventeCajaHerramienta(idH, idCaja, IdHerramientas[idH].tipoEnvolventeMovimiento[idCaja]);
	
	double distancia=0;
	// Comprueba si los objetos estan peligrosamente cercanos
	if (distancia < LIMITE_CERCANIA) {



		if (verifyCollisionDeformable(idColision)){
		
			std::vector<unsigned> carasColisionOrgano;		// caras del deformable
			std::vector<unsigned> carasColisionHerramienta;	// caras de la envolvente de movimiento de la herramienta
			std::vector<unsigned> carasColisionHerramientaAux;	// caras de la herramienta
	
			// Obtiene de la libreria la informacion relativa a las colisiones producidas
			reportCollisionDeformable(idColision, carasColisionOrgano, carasColisionHerramienta);
			// Nos quedamos con las caras de la caja que no son restringidas, caras a las que se pueden
			// mover los vértices
			//Sin envolvente de movimiento
		
			carasColisionHerramienta.clear();
			unsigned carasHerramienta=0;
			for (carasHerramienta=0;carasHerramienta<6;carasHerramienta++){
				if (!caja.bCaraRestringida[carasHerramienta]){
				//Si no es una cara restringida
					carasColisionHerramienta.push_back(2*carasHerramienta);
					carasColisionHerramienta.push_back(2*carasHerramienta+1);
				}//fin if
			}//fin for
			//Sin envolvente de movimiento
								
			

			// El vector de movimiento no está normalizado!!!
			double moduloVectorMovimiento=sqrt((IdHerramientas[idH].vectorDeMovimiento[idCaja*3]*
												IdHerramientas[idH].vectorDeMovimiento[idCaja*3])+
												(IdHerramientas[idH].vectorDeMovimiento[idCaja*3+1]*
												IdHerramientas[idH].vectorDeMovimiento[idCaja*3+1])+
												(IdHerramientas[idH].vectorDeMovimiento[idCaja*3+2]*
												IdHerramientas[idH].vectorDeMovimiento[idCaja*3+2]));

			if ( (fabs(IdHerramientas[idH].vectorDeMovimiento[idCaja*3]))<UMBRALMOVIMIENTO && 
				 (fabs(IdHerramientas[idH].vectorDeMovimiento[idCaja*3+1]))<UMBRALMOVIMIENTO && 
				 (fabs(IdHerramientas[idH].vectorDeMovimiento[idCaja*3+2]))<UMBRALMOVIMIENTO){
		

				unsigned indiceVertice[3];
				float verticeOrgano[3];



				float media[3]={0, 0, 0};

/********** CALCULA EL VERTICE CENTRAL ED TODOS LOS QUE COLISIONAN Y DETERMINA SU VECTOR DE MOVIMIENTO *****/
				char i;
				for(i=0; i<carasColisionOrgano.size(); i++)
				{
					obtenerVertices(id1, carasColisionOrgano[i], indiceVertice);
				
					char j;
					for(j=0; j<3; j++)
					{
						coordenadasVertices(id1, indiceVertice[j], verticeOrgano); 
						// PUNTO MEDIO--> Coordenadas del vertice que estamos considerando + media acumulada
						media[0]=verticeOrgano[0]+media[0];				
						media[1]=verticeOrgano[1]+media[1];				
						media[2]=verticeOrgano[2]+media[2];				
					}
				}
				
				for(i=0; i<3; i++)
				{
					media[i]=media[i]/(carasColisionOrgano.size()*3);
				}

				// Buscar el vertice mas cercano al punto medio.
				float distanciaminima=1000;
				float distancia[3];
				float modulodistancia;
				unsigned indiceCaraCentro=0;
				unsigned indiceVerticeCentro=0;
				for(i=0; i<carasColisionOrgano.size(); i++)
				{
					obtenerVertices(id1, carasColisionOrgano[i], indiceVertice);
				
					char j;
					for(j=0; j<3; j++)
					{
						coordenadasVertices(id1, indiceVertice[j], verticeOrgano); 
						// DISTANCIA--> Distancia del vertice al punto medio
						distancia[0]=verticeOrgano[0]-media[0];				
						distancia[1]=verticeOrgano[1]-media[1];				
						distancia[2]=verticeOrgano[2]-media[2];	
						modulodistancia=sqrt(distancia[0]*distancia[0]+distancia[1]*distancia[1]+distancia[2]*distancia[2]);
						if(modulodistancia<=distanciaminima)
						{
							distanciaminima=modulodistancia;
							indiceCaraCentro=i;
							indiceVerticeCentro=j;
						}
					}
				}
				
				// Calculo el vector de movimoento del punto central
				obtenerVertices(id1, carasColisionOrgano[indiceCaraCentro], indiceVertice);
				coordenadasVertices(id1, indiceVertice[indiceVerticeCentro], verticeOrgano); 

				moduloVectorMovimiento=0;
				for(i=0; i<3; i++){
					IdHerramientas[idH].vectorDeMovimiento[idCaja*3+i]=-(verticeOrgano[i]-VectorDeDeformables[idOrgano]->verticesAnterior[indiceVertice[indiceVerticeCentro]*3+i]);//Esto considera el vector de movimiento de un vertice elegido al azar
				//	IdHerramientas[idH].vectorDeMovimiento[idCaja*3+i]=-media[i];
					moduloVectorMovimiento=moduloVectorMovimiento+IdHerramientas[idH].vectorDeMovimiento[idCaja*3+i]*IdHerramientas[idH].vectorDeMovimiento[idCaja*3+i];
				}
				moduloVectorMovimiento=sqrt(moduloVectorMovimiento);


		/******************************************************************************/


				if (fabs(IdHerramientas[idH].vectorDeMovimiento[idCaja*3])<UMBRALMOVIMIENTO &&
					fabs(IdHerramientas[idH].vectorDeMovimiento[idCaja*3+1])<UMBRALMOVIMIENTO
					&& fabs(IdHerramientas[idH].vectorDeMovimiento[idCaja*3+2])<UMBRALMOVIMIENTO){
				// Si la herramienta no se mueve mantener la posicion anterior de los vertices
				
				//Si no se ha movido el órgano ni la herramienta
					construirVectorColisionesPuntosFijos(carasColisionOrgano, idH, idCaja, idOrgano, vColisiones,
														 listaVerticesOrgano);
					return true;
				}

	
			}
			
			// Normalizamos el vector de movimiento (porque ya no necesitamos la informacion del modulo
			char i;
			for(i=0; i<3; i++){
				IdHerramientas[idH].vectorDeMovimiento[idCaja*3+i]=IdHerramientas[idH].vectorDeMovimiento[idCaja*3+i]/moduloVectorMovimiento;
			}
			// A partir de aquí tenemos el vector de movimiento NORMALIZADO

			// Calculamos las normales a las caras de la herramienta que colisionan
			//double planoDestinoVertices[carasColisionHerramienta.size()];
	
			double *normalCarasHerramienta;
			normalCarasHerramienta=new double[3*12];
			//*** EvitarNormalesOrgano
			//unsigned indicePlanoDestino=calcularPlanoDestino(idH, idCaja, carasColisionHerramienta,
			//												 normalCarasHerramienta, mediaNormalesOrgano);
			unsigned indicePlanoDestino=calcularPlanoDestino(idH, idCaja, carasColisionHerramienta,
															 normalCarasHerramienta, carasColisionOrgano, id1);

			
			float normalPlanoDestino[3];
			for(i=0; i<3; i++){
				normalPlanoDestino[i]=normalCarasHerramienta[carasColisionHerramienta[indicePlanoDestino]*3+i];
				//*** EvitarNormalesOrgano
				//*** Se le cambia el signo para no tener que hacer más modificaciones
				IdHerramientas[idH].normalCarasOrganoDeformadas[idCaja*3+i]=-normalPlanoDestino[i];
			}
			delete[] normalCarasHerramienta;

			float puntoPlanoDestino[3];
			unsigned verticesPlanoDestino[3];
		
			obtenerVerticePlano(verticesPlanoDestino, carasColisionHerramienta[indicePlanoDestino]);
			puntoPlanoDestino[0]=IdHerramientas[idH].vVerticesActual[idCaja*24+verticesPlanoDestino[0]*3+0];
			puntoPlanoDestino[1]=IdHerramientas[idH].vVerticesActual[idCaja*24+verticesPlanoDestino[0]*3+1];
			puntoPlanoDestino[2]=IdHerramientas[idH].vVerticesActual[idCaja*24+verticesPlanoDestino[0]*3+2];
			
			

			bool pinzamiento=false;
			if(idCaja==0 && 
				( (carasColisionHerramienta[indicePlanoDestino]==(CaraInferior*2)) 
				||(carasColisionHerramienta[indicePlanoDestino]==(CaraInferior*2+1)) ) )
			{
				pinzamiento=true;
			}
			if(idCaja==1 && 
				( (carasColisionHerramienta[indicePlanoDestino]==(CaraSuperior*2)) 
				||(carasColisionHerramienta[indicePlanoDestino]==(CaraSuperior*2+1)) ) )
			{
				pinzamiento=true;
			}
			construirVectorColisiones(normalPlanoDestino, puntoPlanoDestino, carasColisionOrgano, 
									  idH, idCaja, idOrgano, vColisiones, listaVerticesOrgano,
									  pinzamiento);
						
			// Liberar los vectores utilizados durante el calculo
			carasColisionOrgano.clear();
			carasColisionHerramienta.clear();

			return true;
		} // fin del if (verifyCollision(idColision))
		else{
			//*** EvitarNormalesOrgano
			for (int i=0; i<9; i++){
			IdHerramientas[idH].normalCarasOrganoDeformadas[idCaja*3+i]=0;
			}
			//*** /EvitarNormalesOrgano
		}
	}// fin del if (getDistanceConcave(idDistancia) 



	return false;
}//fin detectarColisionOrganoHerramienta()


bool vtkColDetect::detectaColisionBolaHerramienta(unsigned int herramActual, vtkEsfera *bola){

	float distancia;
//
	bool hayColision=false;
	char i;
	for (i=0; i<24; i++){
	
	  distancia=(IdHerramientas[herramActual].vVerticesActual[3*i]-(bola->centro[0]))*
			(IdHerramientas[herramActual].vVerticesActual[3*i]-(bola->centro[0]))+
			(IdHerramientas[herramActual].vVerticesActual[3*i+1]-(bola->centro[1]))*
			(IdHerramientas[herramActual].vVerticesActual[3*i+1]-(bola->centro[1]))+
			(IdHerramientas[herramActual].vVerticesActual[3*i+2]-(bola->centro[2]))*
			(IdHerramientas[herramActual].vVerticesActual[3*i+2]-(bola->centro[2]));

	if (distancia <= bola->radio)
		return true;
	}


return false;
}

void vtkColDetect::convertirADouble(float* vectorDeFloat, unsigned longitud, double* vectorDeDouble){
	unsigned i;
	for(i=0; i<longitud; i++)
		vectorDeDouble[i] = static_cast<double>(vectorDeFloat[i]);
}

void vtkColDetect::convertirMatrizTransformacion(float* matrizDeFloat, unsigned longitud, double** matrizDeDouble){

	for(unsigned i=0; i<longitud; i++)
		(* matrizDeDouble)[i] = static_cast<double>(matrizDeFloat[i]);	
}

//Convierte la matriz de transformacion double** a float*
void vtkColDetect::convertirMatrizTransformacionDoubleFloat(double* mDouble[TAM_MATRIZ_TRANS], 	  float* mFloat){
	unsigned i;
	for (i=0; i<TAM_MATRIZ_TRANS; i++){
		mFloat[i] = (*mDouble)[i];
	}//fin for

}//fin convertirMatrizTransformacionDoubleFloat

void vtkColDetect::crearVectorVertices(double vector[], vtkModeloHerramienta* pHerramienta, unsigned posicion) {
		vector[0] = pHerramienta->VectorDeCajas[posicion].abajoDetrasDerecha[0];
		vector[1] = pHerramienta->VectorDeCajas[posicion].abajoDetrasDerecha[1];
		vector[2] = pHerramienta->VectorDeCajas[posicion].abajoDetrasDerecha[2];
   
		vector[3] = pHerramienta->VectorDeCajas[posicion].abajoDetrasIzquierda[0];
		vector[4] = pHerramienta->VectorDeCajas[posicion].abajoDetrasIzquierda[1];
		vector[5] = pHerramienta->VectorDeCajas[posicion].abajoDetrasIzquierda[2];
		
		vector[6] = pHerramienta->VectorDeCajas[posicion].abajoFrenteDerecha[0];
		vector[7] = pHerramienta->VectorDeCajas[posicion].abajoFrenteDerecha[1];
		vector[8] = pHerramienta->VectorDeCajas[posicion].abajoFrenteDerecha[2];
	
		vector[9] = pHerramienta->VectorDeCajas[posicion].abajoFrenteIzquierda[0];
		vector[10] = pHerramienta->VectorDeCajas[posicion].abajoFrenteIzquierda[1];
		vector[11] = pHerramienta->VectorDeCajas[posicion].abajoFrenteIzquierda[2];
	
		vector[12] = pHerramienta->VectorDeCajas[posicion].arribaDetrasDerecha[0];
		vector[13] = pHerramienta->VectorDeCajas[posicion].arribaDetrasDerecha[1];
		vector[14] = pHerramienta->VectorDeCajas[posicion].arribaDetrasDerecha[2];
	
		vector[15] = pHerramienta->VectorDeCajas[posicion].arribaDetrasIzquierda[0];
		vector[16] = pHerramienta->VectorDeCajas[posicion].arribaDetrasIzquierda[1];
		vector[17] = pHerramienta->VectorDeCajas[posicion].arribaDetrasIzquierda[2];
	
		vector[18] = pHerramienta->VectorDeCajas[posicion].arribaFrenteDerecha[0];
		vector[19] = pHerramienta->VectorDeCajas[posicion].arribaFrenteDerecha[1];
		vector[20] = pHerramienta->VectorDeCajas[posicion].arribaFrenteDerecha[2];
	
		vector[21] = pHerramienta->VectorDeCajas[posicion].arribaFrenteIzquierda[0];
		vector[22] = pHerramienta->VectorDeCajas[posicion].arribaFrenteIzquierda[1];
		vector[23] = pHerramienta->VectorDeCajas[posicion].arribaFrenteIzquierda[2];
}



void vtkColDetect::crearVectorVertices(double vector[], vtkModeloEstatico* pEstatico) {
	vector[0] = pEstatico->Caja.centro[0] - pEstatico->Caja.extension[0];
	vector[1] = pEstatico->Caja.centro[1] - pEstatico->Caja.extension[1];
	vector[2] = pEstatico->Caja.centro[2] - pEstatico->Caja.extension[2];

	vector[3] = pEstatico->Caja.centro[0] - pEstatico->Caja.extension[0];
	vector[4] = pEstatico->Caja.centro[1] - pEstatico->Caja.extension[1];
	vector[5] = pEstatico->Caja.centro[2] + pEstatico->Caja.extension[2];
		
	vector[6] = pEstatico->Caja.centro[0] - pEstatico->Caja.extension[0];
	vector[7] = pEstatico->Caja.centro[1] + pEstatico->Caja.extension[1];
	vector[8] = pEstatico->Caja.centro[2] - pEstatico->Caja.extension[2];
	
	vector[9] = pEstatico->Caja.centro[0] + pEstatico->Caja.extension[0];
	vector[10] = pEstatico->Caja.centro[1] - pEstatico->Caja.extension[1];
	vector[11] = pEstatico->Caja.centro[2] - pEstatico->Caja.extension[2];
	
	vector[12] = pEstatico->Caja.centro[0] - pEstatico->Caja.extension[0];
	vector[13] = pEstatico->Caja.centro[1] + pEstatico->Caja.extension[1];
	vector[14] = pEstatico->Caja.centro[2] + pEstatico->Caja.extension[2];
	
	vector[15] = pEstatico->Caja.centro[0] + pEstatico->Caja.extension[0];
	vector[16] = pEstatico->Caja.centro[1] - pEstatico->Caja.extension[1];
	vector[17] = pEstatico->Caja.centro[2] + pEstatico->Caja.extension[2];
	
	vector[18] = pEstatico->Caja.centro[0] + pEstatico->Caja.extension[0];
	vector[19] = pEstatico->Caja.centro[1] + pEstatico->Caja.extension[1];
	vector[20] = pEstatico->Caja.centro[2] - pEstatico->Caja.extension[2];
	
	vector[21] = pEstatico->Caja.centro[0] + pEstatico->Caja.extension[0];
	vector[22] = pEstatico->Caja.centro[1] + pEstatico->Caja.extension[1];
	vector[23] = pEstatico->Caja.centro[2] + pEstatico->Caja.extension[2];

}

void vtkColDetect::crearVectorDoceVertices(double vector[], vtkModeloHerramienta* pHerramienta, unsigned posicion) {
		//0=vertice 0 desplazado
		vector[0] = pHerramienta->VectorDeCajas[posicion].abajoDetrasDerecha[0]+1.01f;
		vector[1] = pHerramienta->VectorDeCajas[posicion].abajoDetrasDerecha[1]+1.01f;
		vector[2] = pHerramienta->VectorDeCajas[posicion].abajoDetrasDerecha[2];
   
		//1=vertice 1 desplazado
		vector[3] = pHerramienta->VectorDeCajas[posicion].abajoDetrasIzquierda[0]+1.01f;
		vector[4] = pHerramienta->VectorDeCajas[posicion].abajoDetrasIzquierda[1]+1.01f;
		vector[5] = pHerramienta->VectorDeCajas[posicion].abajoDetrasIzquierda[2];
		
		//2=vertice 1
		vector[6] = pHerramienta->VectorDeCajas[posicion].abajoDetrasIzquierda[0];
		vector[7] = pHerramienta->VectorDeCajas[posicion].abajoDetrasIzquierda[1];
		vector[8] = pHerramienta->VectorDeCajas[posicion].abajoDetrasIzquierda[2];
		
		//3=vertice 2 desplazado
		vector[9] = pHerramienta->VectorDeCajas[posicion].abajoFrenteDerecha[0]+1.01f;
		vector[10] = pHerramienta->VectorDeCajas[posicion].abajoFrenteDerecha[1]+1.01f;
		vector[11] = pHerramienta->VectorDeCajas[posicion].abajoFrenteDerecha[2];
	
		//4=vertice 3 desplazado
		vector[12] = pHerramienta->VectorDeCajas[posicion].abajoFrenteIzquierda[0]+1.01f;
		vector[13] = pHerramienta->VectorDeCajas[posicion].abajoFrenteIzquierda[1]+1.01f;
		vector[14] = pHerramienta->VectorDeCajas[posicion].abajoFrenteIzquierda[2];
	
		//5=vertice 3
		vector[15] = pHerramienta->VectorDeCajas[posicion].abajoFrenteIzquierda[0];
		vector[16] = pHerramienta->VectorDeCajas[posicion].abajoFrenteIzquierda[1];
		vector[17] = pHerramienta->VectorDeCajas[posicion].abajoFrenteIzquierda[2];
	
		//6=vertice 4 desplazado
		vector[18] = pHerramienta->VectorDeCajas[posicion].arribaDetrasDerecha[0]+1.01f;
		vector[19] = pHerramienta->VectorDeCajas[posicion].arribaDetrasDerecha[1]+1.01f;
		vector[20] = pHerramienta->VectorDeCajas[posicion].arribaDetrasDerecha[2];
	
		//7=vertice 4
		vector[21] = pHerramienta->VectorDeCajas[posicion].arribaDetrasDerecha[0];
		vector[22] = pHerramienta->VectorDeCajas[posicion].arribaDetrasDerecha[1];
		vector[23] = pHerramienta->VectorDeCajas[posicion].arribaDetrasDerecha[2];
	
		//8=vertice 5
		vector[24] = pHerramienta->VectorDeCajas[posicion].arribaDetrasIzquierda[0];
		vector[25] = pHerramienta->VectorDeCajas[posicion].arribaDetrasIzquierda[1];
		vector[26] = pHerramienta->VectorDeCajas[posicion].arribaDetrasIzquierda[2];
	
		//9=vertice 6 desplazado
		vector[27] = pHerramienta->VectorDeCajas[posicion].arribaFrenteDerecha[0]+1.01f;
		vector[28] = pHerramienta->VectorDeCajas[posicion].arribaFrenteDerecha[1]+1.01f;
		vector[29] = pHerramienta->VectorDeCajas[posicion].arribaFrenteDerecha[2];
	
		//10=vertice 6
		vector[30] = pHerramienta->VectorDeCajas[posicion].arribaFrenteDerecha[0];
		vector[31] = pHerramienta->VectorDeCajas[posicion].arribaFrenteDerecha[1];
		vector[32] = pHerramienta->VectorDeCajas[posicion].arribaFrenteDerecha[2];
	
		//11=vertice 7
		vector[33] = pHerramienta->VectorDeCajas[posicion].arribaFrenteIzquierda[0];
		vector[34] = pHerramienta->VectorDeCajas[posicion].arribaFrenteIzquierda[1];
		vector[35] = pHerramienta->VectorDeCajas[posicion].arribaFrenteIzquierda[2];
}


void vtkColDetect::crearVectorCatorceVertices(double vector[], vtkModeloHerramienta* pHerramienta, unsigned posicion) {
		//0=vertice 0 desplazado
		vector[0]	= pHerramienta->VectorDeCajas[posicion].abajoDetrasDerecha[0]+1.01f;
		vector[1] = pHerramienta->VectorDeCajas[posicion].abajoDetrasDerecha[1]+1.01f;
		vector[2] = pHerramienta->VectorDeCajas[posicion].abajoDetrasDerecha[2]+1.01f;
   
		//1=vertice 1 desplazado
		vector[3] = pHerramienta->VectorDeCajas[posicion].abajoDetrasIzquierda[0]+1.01f;
		vector[4] = pHerramienta->VectorDeCajas[posicion].abajoDetrasIzquierda[1]+1.01f;
		vector[5] = pHerramienta->VectorDeCajas[posicion].abajoDetrasIzquierda[2]+1.01f;
		
		//2=vertice 2 desplazado
		vector[6] = pHerramienta->VectorDeCajas[posicion].abajoFrenteDerecha[0]+1.01f;
		vector[7] = pHerramienta->VectorDeCajas[posicion].abajoFrenteDerecha[1]+1.01f;
		vector[8] = pHerramienta->VectorDeCajas[posicion].abajoFrenteDerecha[2]+1.01f;
	
		//3=vertice 3 desplazado
		vector[9] = pHerramienta->VectorDeCajas[posicion].abajoFrenteIzquierda[0]+1.01f;
		vector[10] = pHerramienta->VectorDeCajas[posicion].abajoFrenteIzquierda[1]+1.01f;
		vector[11] = pHerramienta->VectorDeCajas[posicion].abajoFrenteIzquierda[2]+1.01f;

		//4=vertice 4 desplazado
		vector[12] = pHerramienta->VectorDeCajas[posicion].arribaDetrasDerecha[0]+1.01f;
		vector[13] = pHerramienta->VectorDeCajas[posicion].arribaDetrasDerecha[1]+1.01f;
		vector[14] = pHerramienta->VectorDeCajas[posicion].arribaDetrasDerecha[2]+1.01f;

		//5=vertice 5 desplazado
		vector[15] = pHerramienta->VectorDeCajas[posicion].arribaDetrasIzquierda[0]+1.01f;
		vector[16] = pHerramienta->VectorDeCajas[posicion].arribaDetrasIzquierda[1]+1.01f;
		vector[17] = pHerramienta->VectorDeCajas[posicion].arribaDetrasIzquierda[2]+1.01f;
	
		//6=vertice 6 desplazado
		vector[18] = pHerramienta->VectorDeCajas[posicion].arribaFrenteDerecha[0]+1.01f;
		vector[19] = pHerramienta->VectorDeCajas[posicion].arribaFrenteDerecha[1]+1.01f;
		vector[20] = pHerramienta->VectorDeCajas[posicion].arribaFrenteDerecha[2]+1.01f;
	
		//7=vertice 0
		vector[21] = pHerramienta->VectorDeCajas[posicion].abajoDetrasDerecha[0];
		vector[22] = pHerramienta->VectorDeCajas[posicion].abajoDetrasDerecha[1];
		vector[23] = pHerramienta->VectorDeCajas[posicion].abajoDetrasDerecha[2];
		
		//8=vertice 1
		vector[24] = pHerramienta->VectorDeCajas[posicion].abajoDetrasIzquierda[0];
		vector[25] = pHerramienta->VectorDeCajas[posicion].abajoDetrasIzquierda[1];
		vector[26] = pHerramienta->VectorDeCajas[posicion].abajoDetrasIzquierda[2];
	
		//9=vertice 3
		vector[27] = pHerramienta->VectorDeCajas[posicion].abajoFrenteIzquierda[0];
		vector[28] = pHerramienta->VectorDeCajas[posicion].abajoFrenteIzquierda[1];
		vector[29] = pHerramienta->VectorDeCajas[posicion].abajoFrenteIzquierda[2];
	
		//10=vertice 4
		vector[30] = pHerramienta->VectorDeCajas[posicion].arribaDetrasDerecha[0];
		vector[31] = pHerramienta->VectorDeCajas[posicion].arribaDetrasDerecha[1];
		vector[32] = pHerramienta->VectorDeCajas[posicion].arribaDetrasDerecha[2];

		//11=vertice 5
		vector[33] = pHerramienta->VectorDeCajas[posicion].arribaDetrasIzquierda[0];
		vector[34] = pHerramienta->VectorDeCajas[posicion].arribaDetrasIzquierda[1];
		vector[35] = pHerramienta->VectorDeCajas[posicion].arribaDetrasIzquierda[2];
	
		//12=vertice 6
		vector[36] = pHerramienta->VectorDeCajas[posicion].arribaFrenteDerecha[0];
		vector[37] = pHerramienta->VectorDeCajas[posicion].arribaFrenteDerecha[1];
		vector[38] = pHerramienta->VectorDeCajas[posicion].arribaFrenteDerecha[2];
	
		//13=vertice 7
		vector[39] = pHerramienta->VectorDeCajas[posicion].arribaFrenteIzquierda[0];
		vector[40] = pHerramienta->VectorDeCajas[posicion].arribaFrenteIzquierda[1];
		vector[41] = pHerramienta->VectorDeCajas[posicion].arribaFrenteIzquierda[2];
}

void vtkColDetect::configurarCarasHerramienta(unsigned id){
		addTriangle(id, 0, 0, 1, 2);
		addTriangle(id, 1, 2, 1, 3);

		addTriangle(id, 2, 1, 5, 3);
		addTriangle(id, 3, 3, 5, 7);

		addTriangle(id, 4, 0, 2, 4);
		addTriangle(id, 5, 2, 6, 4);

		addTriangle(id, 6, 0, 4, 1);
		addTriangle(id, 7, 1, 4, 5);

		addTriangle(id, 8, 2, 3, 6);
		addTriangle(id, 9, 6, 3, 7);

		addTriangle(id, 10, 5, 4, 6);
		addTriangle(id, 11, 5, 6, 7);

}

void vtkColDetect::configurarCarasEnvolventeDoce(unsigned id){
		addTriangle(id, 0, 0, 1, 3);
		addTriangle(id, 1, 1, 4, 3);

		addTriangle(id, 2, 1, 2, 4);
		addTriangle(id, 3, 2, 5, 4);

		addTriangle(id, 4, 0, 3, 9);
		addTriangle(id, 5, 0, 9, 6);

		addTriangle(id, 6, 0, 6, 1);
		addTriangle(id, 7, 1, 6, 7);

		addTriangle(id, 8, 1, 7, 2);
		addTriangle(id, 9, 2, 7, 8);

		addTriangle(id, 10, 2, 8, 11);
		addTriangle(id, 11, 2, 11, 5);

		addTriangle(id, 12, 5, 11, 10);
		addTriangle(id, 13, 5, 10, 4);

		addTriangle(id, 14, 10, 9, 4);
		addTriangle(id, 15, 9, 3, 4);

		addTriangle(id, 16, 6, 9, 7);
		addTriangle(id, 17, 7, 9, 10);

		addTriangle(id, 18, 7, 10, 8);
		addTriangle(id, 19, 8, 10, 11);
}

void vtkColDetect::configurarCarasEnvolventeCatorce(unsigned id){
		addTriangle(id, 0, 0, 1, 2);
		addTriangle(id, 1, 2, 1, 3);

		addTriangle(id, 2, 0, 2, 4);
		addTriangle(id, 3, 2, 5, 4);

		addTriangle(id, 4, 0, 4, 10);
		addTriangle(id, 5, 10, 7, 0);

		addTriangle(id, 6, 1, 7, 8);
		addTriangle(id, 7, 7, 1, 0);

		addTriangle(id, 8, 3, 9, 6);
		addTriangle(id, 9, 9, 13, 6);

		addTriangle(id, 10, 6, 13, 12);
		addTriangle(id, 11, 12, 5, 6);

		addTriangle(id, 12, 2, 6, 5);
		addTriangle(id, 13, 2, 3, 6);

		addTriangle(id, 14, 10, 4, 12);
		addTriangle(id, 15, 4, 5, 12);

		addTriangle(id, 16, 8, 7, 11);
		addTriangle(id, 17, 10, 11, 7);

		addTriangle(id, 18, 8, 11, 9);
		addTriangle(id, 19, 11, 13, 9);

		addTriangle(id, 20, 11, 10, 12);
		addTriangle(id, 21, 12, 13, 11);
		
		addTriangle(id, 22, 8, 9, 1);
		addTriangle(id, 23, 9, 3, 1);
}

void vtkColDetect::configurarCarasEstatico(unsigned id){
	addTriangle(id, 0, 2, 6, 7);
	addTriangle(id, 1, 2, 7, 4);

	addTriangle(id, 2, 0, 3, 5);
	addTriangle(id, 3, 0, 5, 1);
	
	addTriangle(id, 4, 4, 7, 1);
	addTriangle(id, 5, 7, 5, 1);

	addTriangle(id, 6, 7, 6, 3);
	addTriangle(id, 7, 7, 3, 5);

	addTriangle(id, 8, 2, 6, 0);
	addTriangle(id, 9, 6, 3, 0);

	addTriangle(id, 10, 4, 2, 0);
	addTriangle(id, 11, 4, 0, 1);
}

double* vtkColDetect::obtenerMatriz(unsigned id){
	return vtkColDetection::ObtenerMatriz(id);
}

unsigned vtkColDetect::idDeformable(unsigned id){
	return IdDeformables[id];
}
unsigned vtkColDetect::idHerramienta(unsigned id){
	return 0;
}

unsigned vtkColDetect::idCajaHerramienta(unsigned idHerr, unsigned idCaja){
	unsigned idLibreria = IdHerramientas[idHerr].idCajas[idCaja];
	return idLibreria;
}


unsigned vtkColDetect::idEnvolventeCajaHerramienta(unsigned idHerr, unsigned idCaja, unsigned idEnvolvente){
	unsigned id=idCaja*3+idEnvolvente;  //Existen tres tipo de envolventes por herramienta, por lo que se selecciona la caja correcta
	unsigned idLibreria = IdHerramientas[idHerr].idCajas[id];
	return idLibreria;
}

unsigned vtkColDetect::idEstatico(unsigned id){
	return IdEstaticos[id];
}

void vtkColDetect::obtenerPosAnteriorCaja(unsigned id, unsigned caja, float* posicion){
	posicion[0] = obtenerPosAnteriorCajaX(id, caja);
	posicion[1] = IdHerramientas[id].posAnterior[caja*3 + 1];
	posicion[2] = IdHerramientas[id].posAnterior[caja*3 + 2];
}

void vtkColDetect::obtenerPosCaja(unsigned id, float* posicion){
	coordVertices(id, 0, posicion);
}


void vtkColDetect::obtenerVertices(unsigned id, unsigned cara, unsigned* vertices){
	getVertices(id, cara, vertices);
}

void vtkColDetect::coordenadasVertices(unsigned id, unsigned vertice, float* coord){
	coordVertices(id, vertice, coord);
}

float vtkColDetect::obtenerPosAnteriorCajaX(unsigned id, unsigned caja) {
	return 0.0;
}


void vtkColDetect::calcularMediaNormales(unsigned id, std::vector<unsigned> caras, float* mediaNormales)
{
	mediaNormales[0]=0;
	mediaNormales[1]=0;
	mediaNormales[2]=0;
	
	unsigned vertices[3];
		
	float verticeA[3];
	float verticeB[3];
	float verticeC[3];
		
	float normalCara[3];
	
	unsigned i;
	for (i=0; i<caras.size(); i++)
	{
		obtenerVertices(id, caras[i], vertices);
			
		coordenadasVertices(id, vertices[0], verticeA);
		coordenadasVertices(id, vertices[1], verticeB);
		coordenadasVertices(id, vertices[2], verticeC);
		
		calcularNormal(verticeA, verticeB, verticeC, normalCara);
			
		mediaNormales[0]=mediaNormales[0]+normalCara[0];
		mediaNormales[1]=mediaNormales[1]+normalCara[1];
		mediaNormales[2]=mediaNormales[2]+normalCara[2];
	}
			
	float modulo=sqrt(mediaNormales[0]*mediaNormales[0]+mediaNormales[1]*mediaNormales[1]+mediaNormales[2]*mediaNormales[2]);
	
	for(i=0; i<3; i++)
	{
		mediaNormales[i]=mediaNormales[i]/modulo;
	}
}

void vtkColDetect::calcularNormal(float *verticeA,  float *verticeB, float *verticeC, float *normal)
{
	float vectorAB[3];
	float vectorBC[3];
	
	char i;
	for(i=0; i<3; i++){
		vectorAB[i]=verticeB[i]-verticeA[i];
		vectorBC[i]=verticeC[i]-verticeB[i];
	}
	
	// Producto vectorial.
	normal[0]=vectorAB[1]*vectorBC[2]-vectorAB[2]*vectorBC[1];
	normal[1]=vectorAB[2]*vectorBC[0]-vectorAB[0]*vectorBC[2];
	normal[2]=vectorAB[0]*vectorBC[1]-vectorAB[1]*vectorBC[0];

//*** Las potencias con **	
	float modulo=sqrt(normal[0]*normal[0]+normal[1]*normal[1]+normal[2]*normal[2]);
	for(i=0; i<3; i++){
		normal[i]=normal[i]/modulo;
	}
}

unsigned vtkColDetect::calcularPlanoDestino(unsigned idH, unsigned idCaja, std::vector<unsigned> carasColisionHerramienta, double *normalCarasHerramienta, std::vector<unsigned> carasColisionOrgano, unsigned id1) 
{
	double* planoDestinoVertices;
	planoDestinoVertices=new double[carasColisionHerramienta.size()];
	float producto[3];
	

	// Comprobamos si es PENETRACION o DESLIZAMIENTO
	double indicePenetracion;		// Si es menor que un umbral, se considera deslizamiento
	bool   primeraPenetracion = false;

	
	if (IdHerramientas[idH].normalCarasOrganoDeformadas[idCaja*3]==0   &&
		IdHerramientas[idH].normalCarasOrganoDeformadas[idCaja*3+1]==0 &&
		IdHerramientas[idH].normalCarasOrganoDeformadas[idCaja*3+2]==0){
			primeraPenetracion = true;
	}

	// Producto escalar de la normal a las caras del órgano deformadas por 
	// el vector de movimiento = nod · vm
	if(primeraPenetracion) // Si es primera penetracion hay que calcular el indice para comprobar si estamos sacando o metiendo
	{
		char i;
		for(i=0; i<3; i++){
			float mediaNormalesOrgano[3];
			calcularMediaNormales(id1, carasColisionOrgano, mediaNormalesOrgano);
			producto[i]=IdHerramientas[idH].vectorDeMovimiento[idCaja*3+i]*
					mediaNormalesOrgano[i];
		}
	}
	else
	{
		char i;
		for(i=0; i<3; i++){
			producto[i]=IdHerramientas[idH].vectorDeMovimiento[idCaja*3+i]*
					IdHerramientas[idH].normalCarasOrganoDeformadas[idCaja*3+i];
		}
	}

	indicePenetracion=producto[0]+producto[1]+producto[2];
	calcularNormalCarasHerramienta(idH, idCaja, normalCarasHerramienta);
	unsigned i;
	for (i=0; i<carasColisionHerramienta.size(); i++){
		if(fabs(indicePenetracion)<=UMBRALPENETRACION && !primeraPenetracion){
			producto[0] = normalCarasHerramienta[carasColisionHerramienta[i]*3]*
						  IdHerramientas[idH].normalCarasOrganoDeformadas[idCaja*3];
			producto[1] =  normalCarasHerramienta[carasColisionHerramienta[i]*3+1]*
						 IdHerramientas[idH].normalCarasOrganoDeformadas[idCaja*3+1];
			producto[2] = normalCarasHerramienta[carasColisionHerramienta[i]*3+2]*
						  IdHerramientas[idH].normalCarasOrganoDeformadas[idCaja*3+2];
		}
		else{
			producto[0] = normalCarasHerramienta[carasColisionHerramienta[i]*3]*
						  IdHerramientas[idH].vectorDeMovimiento[idCaja*3+0];
			producto[1] = normalCarasHerramienta[carasColisionHerramienta[i]*3+1]*
						  IdHerramientas[idH].vectorDeMovimiento[idCaja*3+1];
			producto[2] = normalCarasHerramienta[carasColisionHerramienta[i]*3+2]*
						  IdHerramientas[idH].vectorDeMovimiento[idCaja*3+2];
		}
		planoDestinoVertices[i]=producto[0]+producto[1]+producto[2];
	}
	
	unsigned indicePlanoDestino = identificarPenetracionDeslizamiento(indicePenetracion,
								  planoDestinoVertices, carasColisionHerramienta.size(), primeraPenetracion);
		
	delete[] planoDestinoVertices;
	return indicePlanoDestino;

}// fin calcularPlanoDestino()

void vtkColDetect::calcularNormalCarasHerramienta(unsigned idH, unsigned idCaja, double *normalCarasHerramienta){
	float normal[3];
	float vertice0[3];
	float vertice1[3];
	float vertice2[3];
	float vertice3[3];
	float vertice4[3];
	float vertice5[3];
	float vertice6[3];
	float vertice7[3];
	
	for (int i=0; i<3; i++){
		vertice0[i]=IdHerramientas[idH].vVerticesActual[idCaja*24+i];
		vertice1[i]=IdHerramientas[idH].vVerticesActual[idCaja*24+3+i];
		vertice2[i]=IdHerramientas[idH].vVerticesActual[idCaja*24+6+i];
		vertice3[i]=IdHerramientas[idH].vVerticesActual[idCaja*24+9+i];
		vertice4[i]=IdHerramientas[idH].vVerticesActual[idCaja*24+12+i];
		vertice5[i]=IdHerramientas[idH].vVerticesActual[idCaja*24+15+i];
		vertice6[i]=IdHerramientas[idH].vVerticesActual[idCaja*24+18+i];
		vertice7[i]=IdHerramientas[idH].vVerticesActual[idCaja*24+21+i];
	}
	
	    calcularNormal(vertice0, vertice1, vertice2, normal);
		normalCarasHerramienta[0]=normal[0];
		normalCarasHerramienta[1]=normal[1];
		normalCarasHerramienta[2]=normal[2];

	    calcularNormal(vertice2, vertice1, vertice3, normal);
		normalCarasHerramienta[3]=normal[0];
		normalCarasHerramienta[4]=normal[1];
		normalCarasHerramienta[5]=normal[2];

		calcularNormal(vertice1, vertice5, vertice3,normal);
		normalCarasHerramienta[6]=normal[0];
		normalCarasHerramienta[7]=normal[1];
		normalCarasHerramienta[8]=normal[2];

		
		calcularNormal(vertice3, vertice5, vertice7, normal);
		normalCarasHerramienta[9]=normal[0];
		normalCarasHerramienta[10]=normal[1];
		normalCarasHerramienta[11]=normal[2];

		calcularNormal(vertice0, vertice2, vertice4, normal);
		normalCarasHerramienta[12]=normal[0];
		normalCarasHerramienta[13]=normal[1];
		normalCarasHerramienta[14]=normal[2];

		calcularNormal(vertice2, vertice6, vertice4, normal);
		normalCarasHerramienta[15]=normal[0];
		normalCarasHerramienta[16]=normal[1];
		normalCarasHerramienta[17]=normal[2];

		calcularNormal(vertice0, vertice4, vertice1, normal);
		normalCarasHerramienta[18]=normal[0];
		normalCarasHerramienta[19]=normal[1];
		normalCarasHerramienta[20]=normal[2];

		calcularNormal(vertice1, vertice4, vertice5, normal);
		normalCarasHerramienta[21]=normal[0];
		normalCarasHerramienta[22]=normal[1];
		normalCarasHerramienta[23]=normal[2];

		calcularNormal(vertice2, vertice3, vertice6, normal);
		normalCarasHerramienta[24]=normal[0];
		normalCarasHerramienta[25]=normal[1];
		normalCarasHerramienta[26]=normal[2];

		calcularNormal(vertice6, vertice3, vertice7, normal);
		normalCarasHerramienta[27]=normal[0];
		normalCarasHerramienta[28]=normal[1];
		normalCarasHerramienta[29]=normal[2];

		calcularNormal(vertice5, vertice4, vertice6, normal);
		normalCarasHerramienta[30]=normal[0];
		normalCarasHerramienta[31]=normal[1];
		normalCarasHerramienta[32]=normal[2];

		calcularNormal(vertice5, vertice6, vertice7, normal);
		normalCarasHerramienta[33]=normal[0];
		normalCarasHerramienta[34]=normal[1];
		normalCarasHerramienta[35]=normal[2];
}

void vtkColDetect::obtenerVerticePlano(unsigned* verticesPlanoDestino, unsigned indicePlanoDestino){
	switch(indicePlanoDestino){

	case 0:
		verticesPlanoDestino[0]=0;
		verticesPlanoDestino[1]=1;
		verticesPlanoDestino[2]=2;
		break;

	case 1:
		verticesPlanoDestino[0]=2;
		verticesPlanoDestino[1]=1;
		verticesPlanoDestino[2]=3;
		break;
	case 2:
		verticesPlanoDestino[0]=1;
		verticesPlanoDestino[1]=5;
		verticesPlanoDestino[2]=3;
		break;

	case 3:
		verticesPlanoDestino[0]=3;
		verticesPlanoDestino[1]=5;
		verticesPlanoDestino[2]=7;
		break;
	case 4:
		verticesPlanoDestino[0]=0;
		verticesPlanoDestino[1]=2;
		verticesPlanoDestino[2]=4;
		break;
	case 5:
		verticesPlanoDestino[0]=2;
		verticesPlanoDestino[1]=6;
		verticesPlanoDestino[2]=4;
		break;
	case 6:
		verticesPlanoDestino[0]=0;
		verticesPlanoDestino[1]=4;
		verticesPlanoDestino[2]=1;
		break;
	case 7:
		verticesPlanoDestino[0]=1;
		verticesPlanoDestino[1]=4;
		verticesPlanoDestino[2]=5;
		break;
	case 8:
		verticesPlanoDestino[0]=2;
		verticesPlanoDestino[1]=3;
		verticesPlanoDestino[2]=6;
		break;
	case 9:
		verticesPlanoDestino[0]=6;
		verticesPlanoDestino[1]=3;
		verticesPlanoDestino[2]=7;
		break;
	case 10:
		verticesPlanoDestino[0]=5;
		verticesPlanoDestino[1]=4;
		verticesPlanoDestino[2]=6;
		break;
	case 11:
		verticesPlanoDestino[0]=5;
		verticesPlanoDestino[1]=6;
		verticesPlanoDestino[2]=7;
		break;
	default:
		break;
	}
}

unsigned vtkColDetect::identificarPenetracionDeslizamiento(double indicePenetracion, double *planoDestinoVertices, int numCarasColision, bool primeraPenetracion)
{

	unsigned indicePlanoDestino=0;
	if(fabs(indicePenetracion)<=UMBRALPENETRACION && !primeraPenetracion){

	// Calculamos el plano al que hay que mover los vertices del organo. 
	// El que minimiza el producto escalar entre la normal a la cara de la herramienta que colisiona
	// y la media de las normales de las caras del órgano (planoDestinoVertices(nh·no) lo más próximo a -1)
	
		double minimo=1;
		for(int i=0; i<numCarasColision; i++){
			if(planoDestinoVertices[i]<=minimo){
				minimo=planoDestinoVertices[i];
				indicePlanoDestino=i;
			}
		}
	}
	else if((indicePenetracion>UMBRALPENETRACION)||((indicePenetracion>0) && primeraPenetracion)){
	//PENETRACION SACAR
		// Calculamos el plano al que hay que mover los vertices del organo. 
		// Aquel que hace que el producto escalar entre la normal a la cara de la herramienta que colisiona
		// y el vector de movimiento (planoDestinoVertices) sea lo mas proximo a -1
		double minimo=1;
		for(int i=0; i<numCarasColision; i++){
			if(planoDestinoVertices[i]<=minimo){
				minimo=planoDestinoVertices[i];
				indicePlanoDestino=i;
			}
		}
	
	}else{
	//PENETRACIÓN, METER
		// Calculamos el plano al que hay que mover los vertices del organo. 
		// El que maximiza el producto escalar entre la normal a la cara de la herramienta que colisiona
		// y el vector de movimiento (planoDestinoVertices lo más próximo a 1)
	
		double maximo=-1;
		for(int i=0; i<numCarasColision; i++){
			if(planoDestinoVertices[i]>=maximo){
				maximo=planoDestinoVertices[i];
				indicePlanoDestino=i;
			}
		}
	}
	return indicePlanoDestino;
}// fin identificarPenetracionDeslizamiento()



void vtkColDetect::construirVectorColisionesPuntosFijos(std::vector<unsigned> carasColisionOrgano,unsigned idH, unsigned idCaja, unsigned idOrgano, vtkInfoColisionCollection* vColisiones, std::vector<unsigned> &listaVerticesOrgano) 
{
	vtkInfoColision *datosColision= vtkInfoColision::New();
	unsigned verticesCaraOrgano[3];
	unsigned id1=idDeformable(idOrgano);
	char i;
	for(i=0; i<carasColisionOrgano.size(); i++){
		obtenerVertices(id1, carasColisionOrgano[i], verticesCaraOrgano);
		bool verticeRepetido=false;
		unsigned sustituirVertice=0;
		char k;
		for(k=1; k<3; k++){
			// Comprobamos si el vertice está repetido.
			unsigned j;
			for(j=0; j<listaVerticesOrgano.size(); j++){
				if(listaVerticesOrgano[j]==verticesCaraOrgano[k]){
					verticeRepetido=true;
					break;
				}
	        }//fin for
			if(!verticeRepetido){
				datosColision->SetnHerramienta(idH);
				datosColision->SetnCaja(idCaja);
				datosColision->SetnDeformable(idOrgano);
				datosColision->SetnVertice(static_cast<int>(verticesCaraOrgano[k]));
				datosColision->SetnCara(static_cast<int>(carasColisionOrgano[i]));
	
				float posActualVertice[3];
				coordenadasVertices(id1, verticesCaraOrgano[k], posActualVertice);
				// Desplazamiento será la distancia del vértice del órgano al plano	destino
				datosColision->NuevaPosVertice[0]=posActualVertice[0];
				datosColision->NuevaPosVertice[1]=posActualVertice[1];
				datosColision->NuevaPosVertice[2]=posActualVertice[2];
				
				if(sustituirVertice==0){
					vColisiones->InsertarInfoColision(datosColision);
					listaVerticesOrgano.push_back(verticesCaraOrgano[k]);
				}
				else{
					vColisiones->ReplaceInfoColision(j,datosColision);
				}
			}//fin if
		}//fin for
	}//fin for
}// fin construirVectorColisionesPuntosFijos()




void vtkColDetect::construirVectorColisiones(float* normal, float* punto, std::vector<unsigned> carasColisionOrgano, 
										   unsigned idH, unsigned idCaja, unsigned idOrgano, 
										   vtkInfoColisionCollection *vColisiones, 
										   std::vector<unsigned> &listaVerticesOrgano, bool pinzamiento)
{
	double moduloCuadradoNormal;
	moduloCuadradoNormal=normal[0]*normal[0]+normal[1]*normal[1]+normal[2]*normal[2];
	vtkInfoColision *datosColision = vtkInfoColision::New();	
	unsigned verticesCaraOrgano[3];
	unsigned id1=idDeformable(idOrgano);
	
	for(int i=0; i<carasColisionOrgano.size(); i++){

		obtenerVertices(id1, carasColisionOrgano[i], verticesCaraOrgano);
		bool verticeRepetido=false;
		unsigned sustituirVertice=0;
		
				
		char k;
		for(k=1; k<3; k++){	 
		 // Comprobamos si el vertice está repetido.
			unsigned j;
			for(j=0; j<listaVerticesOrgano.size(); j++){
				if(listaVerticesOrgano[j]==verticesCaraOrgano[k]){
					verticeRepetido=true;
					sustituirVertice=j;
					break;
				}// fin for j
			}

		// Si el vertice no está repetido se añade la informacion de colisiones al vector.
		
				datosColision->SetnHerramienta(idH);
				datosColision->SetnCaja(idCaja);
				datosColision->SetnDeformable(idOrgano);
				datosColision->SetnVertice(static_cast<int>(verticesCaraOrgano[k]));
				datosColision->SetnCara(static_cast<int>(carasColisionOrgano[i]));
				
				float posActualVertice[3];
				coordenadasVertices(id1, verticesCaraOrgano[k], posActualVertice);
				// Desplazamiento será la distancia del vértice del órgano al plano	destino
				float desplazamiento; 
				//1. Producto escalar de vfd·n
				desplazamiento=normal[0]*(punto[0]-posActualVertice[0]);
				desplazamiento=desplazamiento+(normal[1]*(punto[1]-posActualVertice[1]));
				desplazamiento=desplazamiento+(normal[2]*(punto[2]-posActualVertice[2]));
				//desplazamiento=desplazamiento/moduloCuadradoNormal;
				
				//2. Se calcula |vfd·n|
				//desplazamiento = fabs(desplazamiento);
				//3. Se divide  |vfd·normal|/|n|
				desplazamiento = desplazamiento/sqrt(moduloCuadradoNormal);

			//	desplazamiento=desplazamiento-UMBRAL_SEGURIDAD;
				desplazamiento=0.9f*desplazamiento;

				
				//if(desplazamiento<0.1){
				if(fabs(desplazamiento)<0.001){
					desplazamiento=0.0;
				}
				
					
				if(!verticeRepetido){
		
				datosColision->NuevaPosVertice[0]=posActualVertice[0]+(normal[0]*desplazamiento);
				datosColision->NuevaPosVertice[1]=posActualVertice[1]+(normal[1]*desplazamiento);
				datosColision->NuevaPosVertice[2]=posActualVertice[2]+(normal[2]*desplazamiento);
				datosColision->SetRestMovimiento(-normal[0], -normal[1], -normal[2]);
				datosColision->SetposiblePinzamiento(pinzamiento);
				datosColision->desplazamiento=desplazamiento;
				vColisiones->InsertarInfoColision(datosColision);				
				listaVerticesOrgano.push_back(verticesCaraOrgano[k]);


				} else if (fabs(desplazamiento)<fabs(vColisiones->GetInfoColisionOnPosition(sustituirVertice)->desplazamiento)){

					datosColision->NuevaPosVertice[0]=posActualVertice[0]+(normal[0]*desplazamiento);
					datosColision->NuevaPosVertice[1]=posActualVertice[1]+(normal[1]*desplazamiento);
					datosColision->NuevaPosVertice[2]=posActualVertice[2]+(normal[2]*desplazamiento);
					datosColision->SetRestMovimiento(-normal[0], -normal[1], -normal[2]);
					datosColision->SetposiblePinzamiento(pinzamiento);
					datosColision->desplazamiento=desplazamiento;
					vColisiones->ReplaceInfoColision(j,datosColision);
				}
		
							
		} //fin for i
	} // fin construirVectorColisiones
}