content: package edu.stanford.rsl.conrad.numerics;
  
  import java.util.Collection;
  import java.util.Iterator;
  
  import Jama.EigenvalueDecomposition;
  import Jama.Matrix;
  import edu.stanford.rsl.conrad.numerics.SimpleMatrix.InversionType;
  import edu.stanford.rsl.conrad.utils.CONRAD;
  
  
  
  /**
   * @author Andreas Keil
   */
  public abstract class SimpleOperators {
  
  
  	// **************************************************************** //
  	// ******************* Vector/Vector operators ******************** //
  	// **************************************************************** //
  
  	/**
  	 * <p>Computes the sum of supplied vectors
  	 * <p> e.g. SimpleVector x = new SimpleVector(1,2,3);<br/>SimpleVector y = new SimpleVector(1,2,3);<br/> SimpleVector z = new SimpleVector(1,0,0);<br/>  SimpleOperators.add(x,y,z) returns [3,4,6];
  	 * @param addends is comma-separated list or an array of vectors.
  	 * @return the sum of supplied vectors.
  	 */
  	public static SimpleVector add(final SimpleVector... addends) {
  		assert (addends.length >= 1) : new IllegalArgumentException("Need at least one argument for addition!");
  		final SimpleVector result = addends[0].clone();
  		for (int i = 1; i < addends.length; ++i)
  			result.add(addends[i]);
  		return result;
  	}
  
  	/**
  	 * <p>subtracts v2 from v1
  	 */
  	public static SimpleVector subtract(final SimpleVector v1, final SimpleVector v2) {
  		assert (v1.getLen() == v2.getLen()) : new IllegalArgumentException("Vector lengths must match for summation!");
  		final SimpleVector result = new SimpleVector(v1.getLen());
  		for (int i = 0; i < v1.getLen(); ++i)
  			result.setElementValue(i, v1.getElement(i) - v2.getElement(i));
  		return result;
  	}
  	
  	/**
  	 * <p>Multiplies the supplied vectors element wise
  	 * <p> e.g. SimpleVector x = new SimpleVector(1,2,3);<br/>SimpleVector y = new SimpleVector(1,2,3);<br/> SimpleVector z = new SimpleVector(1,0,0);<br/>  SimpleOperators.multiplyElementWise(x,y,z) returns [1,0,0];
  	 * @param factors is a comma-separated list or an array of vectors.
  	 * @return element wise multiplication for supplied vectors
  	 */
  	public static SimpleVector multiplyElementWise(final SimpleVector... factors) {
  		final SimpleVector result = factors[0].clone();
  		for (int i = 1; i < factors.length; ++i)
  			result.multiplyElementWiseBy(factors[i]);
  		return result;
  	}
  	
  	/**
  	 * <p>Computes the element wise division of v1 by v2.
  	 * <p> e.g. SimpleVector x = new SimpleVector(1,2,3);<br/>SimpleVector y = new SimpleVector(2,10,5);<br/> SimpleOperations.divideElementWise(x,y) returns [0.5,0.2,0.6];
  	 * @param v1 is vector to be divided
  	 * @param v2 is divisor
  	 * @return a new vector containing the element-wise devision.
  	 */
  	public static SimpleVector divideElementWise(final SimpleVector v1, final SimpleVector v2) {
  		final SimpleVector result = v1.clone();
  		result.divideElementWiseBy(v2);
  		return result;
  	}
  	
  	/**
  	 * <p> Computes the inner product multiplication (dot product) of v1 and v2.
  	 * <p> SimpleVector x = new SimpleVector(1,2);<br/>SimpleVector y = new SimpleVector(3,10);<br/> SimpleOperations.multiplyInnerProd(x,y) = 1*3 + 2*10 = 23;
  	 * @param v1 is first vector
  	 * @param v2 is second vector
  	 * @return the inner product multiplication of v1 and v2
  	 */
  	public static double multiplyInnerProd(final SimpleVector v1, final SimpleVector v2) {
  		assert (v1.getLen() == v2.getLen()) : new IllegalArgumentException("Vector lengths must match for inner product calculation!");
  		double result = 0.0;
  		for (int i = 0; i < v1.getLen(); ++i)
  			result += v1.getElement(i) * v2.getElement(i);
  		return result;
  	}
  	
  	/**
  	 * Computes the outer product  multiplication of v1 and v2; i.e v1 x v2
  	 * @param v1 is first vector
  	 * @param v2 is second vector
  	 * @return matrix representing v1 x v2
  	 */
  	public static SimpleMatrix multiplyOuterProd(final SimpleVector v1, final SimpleVector v2) {
  		final SimpleMatrix result = new SimpleMatrix(v1.getLen(), v2.getLen());
  		for (int r = 0; r < v1.getLen(); ++r)
  			for (int c = 0; c < v2.getLen(); ++c)
  				result.setElementValue(r, c, v1.getElement(r)*v2.getElement(c));
  		return result;
  	}
  
  	/**
  	 * Creates a new vector which is composed of all input vectors, stacked over each other.  
  	 * @param parts  The vectors to concatenate.
  	 * @return  The vertically concatenated vector.
  	 */
  	public static SimpleVector concatenateVertically(SimpleVector... parts) {
  		final int noParts = parts.length;
  		assert noParts >= 1 : new IllegalArgumentException("Supply at least one vector to concatenate!");
  		final int[] lengths = new int[noParts];
  		final int[] accumulatedLengthsBefore = new int[noParts];
  		int totalLength = 0;
  		for (int i = 0; i < noParts; ++i) {
  			lengths[i] = parts[i].getLen();
  			accumulatedLengthsBefore[i] = (i == 0) ? 0 : (accumulatedLengthsBefore[i-1] + lengths[i-1]);
  			totalLength += lengths[i];
  		}
  		SimpleVector result = new SimpleVector(totalLength);
  		for (int i = 0; i < noParts; ++i) result.setSubVecValue(accumulatedLengthsBefore[i], parts[i]);
  		return result;
  	}
  
  	/**
  	 * Creates a new matrix which is composed of all input column vectors, stacked next to each other.  
  	 * @param columns  The vectors to stack.
  	 * @return  The horizontally concatenated matrix.
  	 */
  	public static SimpleMatrix concatenateHorizontally(SimpleVector... columns) {
  		final int cols = columns.length;
  		assert cols >= 1 : new IllegalArgumentException("Supply at least one vector to concatenate!");
  		final int rows = columns[0].getLen();
  		assert rows >= 1 : new IllegalArgumentException("Vectors have to contain at least one element each!");
  		SimpleMatrix result = new SimpleMatrix(rows, cols);
  		for (int c = 0; c < cols; ++c)
  			result.setColValue(c, columns[c]);
  		return result;
  	}
  
  	public static boolean equalElementWise(final SimpleVector v1, final SimpleVector v2, final double delta) {
  		if (v1.getLen() != v2.getLen()) throw new IllegalArgumentException("Vectors have different length!");
  		for (int i = 0; i < v1.getLen(); ++i)
  			if (Math.abs(v1.getElement(i) - v2.getElement(i)) > delta) return false;
  		return true;
  	}
  
  	/**
  	 * Computes and returns the element-wise maximum of all given vectors. 
  	 * @param vectors  A comma-separated list or an array of vectors.
  	 * @return  A new vector with the element-wise maximums of all given input vectors.
  	 */
  	public static SimpleVector max(final SimpleVector... vectors) {
  		assert vectors.length >= 1 : new IllegalArgumentException("Provide at least one vector!");
  		SimpleVector result = vectors[0].clone();
  		final int rows = vectors[0].getLen();
  		for (int i = 1; i < vectors.length; ++i) {
  			assert vectors[i].getLen() == rows : new IllegalArgumentException("All vectors must have the same length!");
  			for (int r = 0; r < rows; ++r)
  				if (vectors[i].getElement(r) > result.getElement(r)) result.setElementValue(r, vectors[i].getElement(r));
  		}
  		return result;
  	}
  
  	/**
  	 * Computes and returns the element-wise minimum of all given vectors. 
  	 * @param vectors  A comma-separated list or an array of vectors.
  	 * @return  A new vector with the element-wise minimums of all given input vectors.
  	 */
  	public static SimpleVector min(final SimpleVector... vectors) {
  		assert vectors.length >= 1 : new IllegalArgumentException("Provide at least one vector!");
  		SimpleVector result = vectors[0].clone();
  		final int rows = vectors[0].getLen();
  		for (int i = 1; i < vectors.length; ++i) {
  			assert vectors[i].getLen() == rows : new IllegalArgumentException("All vectors must have the same length!");
  			for (int r = 0; r < rows; ++r)
  				if (vectors[i].getElement(r) < result.getElement(r)) result.setElementValue(r, vectors[i].getElement(r));
  		}
  		return result;
  	}
  	
  	/**
  	 * 
  	 * @param in The vector to process
  	 * @param fct A generic function, e.g. the absolute value
  	 * @return a vector with the function applied to all elements
  	 */
  	public static SimpleVector elementWiseOperator(final SimpleVector in, DoubleFunction fct){
  		SimpleVector out = in.clone();
  		for (int i = 0; i < out.getLen(); i++) {
  			out.setElementValue(i, fct.f(out.getElement(i)));
  			
  		}
  		return out;
  	}
  	
  	// **************************************************************** //
  	// ******************* Matrix/Matrix operators ******************** //
  	// **************************************************************** //
  	/**
  	 * 
  	 * @param in The matrix to process
  	 * @param fct A generic function, e.g. the absolute value
  	 * @return a matrix with the function applied to all elements
  	 */
  	public static SimpleMatrix elementWiseOperator(final SimpleMatrix in, DoubleFunction fct){
  		SimpleMatrix out = in.clone();
  		for (int i = 0; i < out.getRows(); i++) {
  			for (int j = 0; j < out.getCols(); j++) {
  				out.setElementValue(i, j, fct.f(out.getElement(i, j)));
  			}
  		}
  		return out;
  	}
  	
  	/**
  	 * Computes the sum of provided matrices
  	 * @param addends  A comma-separated list or an array of matrices.
  	 * @return a matrix representing the sum of provided matrices
  	 */
  	public static SimpleMatrix add(final SimpleMatrix... addends) {
  		assert (addends.length >= 1) : new IllegalArgumentException("Need at least one argument for addition!");
  		final SimpleMatrix result = addends[0].clone();
  		for (int i = 1; i < addends.length; ++i)
  			result.add(addends[i]);
  		return result;
  	}
  	
  	/**
  	 * Subtracts M2 from M1
  	 * @param M1 
  	 * @param M2
  	 * @return matrix representing the subtraction of M2 from M1
  	 */
  	public static SimpleMatrix subtract(final SimpleMatrix M1, final SimpleMatrix M2) {
  		assert (M1.getRows() == M2.getRows() && M1.getCols() == M2.getCols()) : new IllegalArgumentException("Matrix sizes must match for summation!");
  		final SimpleMatrix result = new SimpleMatrix(M1.getRows(), M1.getCols());
  		for (int r = 0; r < result.getRows(); ++r)
  			for (int c = 0; c < result.getCols(); ++c)
  				result.setElementValue(r, c, M1.getElement(r, c) - M2.getElement(r, c));
  		return result;
  	}
  	
  	/**
  	 * Computes the product of two matrices
  	 * @param M1 is left matrix
  	 * @param M2 is right matrix
  	 * @return a matrix representing the product of provided matrices
  	 */
  	public static SimpleMatrix multiplyMatrixProd(final SimpleMatrix M1, final SimpleMatrix M2) {
  		assert (M1.getCols() == M2.getRows()) : new IllegalArgumentException("Matrices' column/row dimensions must match for multiplication!");
  		final SimpleMatrix result = new SimpleMatrix(M1.getRows(), M2.getCols());
  		for (int r = 0; r < M1.getRows(); ++r)
  			for (int c = 0; c < M2.getCols(); ++c)
  				result.setElementValue(r, c, SimpleOperators.multiplyInnerProd(M1.getRow(r), M2.getCol(c)));
  		return result;
  	}
  	
  	/**
  	 * Computes the product of matrices, e.g. multiplyMatrices(m1,m2,m3,m4,m5) -> computes m1*m2*m3*m4*m5 and
  	 * returns the result, if only one matrix is given, this method returns the matrix itself.
  	 * @param matrices: several matrices 
  	 * @return the matrix product of all given matrices
  	 */
  	public static SimpleMatrix multiplyMatrices(SimpleMatrix... matrices) {
  		int count = 0;
  		SimpleMatrix cur_matrix = null;
  		SimpleMatrix matrix_product = null;
  		for(SimpleMatrix matrix: matrices) {
  			if(count == 1) {
  				matrix_product = SimpleOperators.multiplyMatrixProd(cur_matrix, matrix);
  				count++;
  			}else if(count > 1){
  				matrix_product = SimpleOperators.multiplyMatrixProd(matrix_product, matrix);
  				count++;
  			}else if(count == 0){
  				count++;
  				cur_matrix = matrix;
  				matrix_product = cur_matrix;
  			}
  		}
  		return matrix_product;
  	}
  	
  	public static SimpleMatrix multiplyElementWise(final SimpleMatrix... factors) {
  		final SimpleMatrix result = factors[0].clone();
  		for (int i = 1; i < factors.length; ++i)
  			result.multiplyElementWiseBy(factors[i]);
  		return result;
  	}
  
  	public static SimpleMatrix divideElementWise(final SimpleMatrix M1, final SimpleMatrix M2) {
  		assert ((M1.getRows() == M2.getRows()) && (M1.getCols() == M2.getCols())) : new IllegalArgumentException("Matrices' dimensions must match for multiplication!");
  		final SimpleMatrix result = new SimpleMatrix(M1.getRows(), M1.getCols());
  		for (int r = 0; r < M1.getRows(); ++r)
  			for (int c = 0; c < M1.getCols(); ++c)
  				result.setElementValue(r, c, M1.getElement(r, c) / M2.getElement(r, c));
  		return result;
  	}
  
  	public static boolean equalElementWise(final SimpleMatrix M1, final SimpleMatrix M2, final double delta) {
  		if (M1.getRows() != M2.getRows()) throw new IllegalArgumentException("Matrices have different number of rows!");
  		if (M1.getCols() != M2.getCols()) throw new IllegalArgumentException("Matrices have different number of columns!");
  		for (int r = 0; r < M1.getRows(); ++r)
  			for (int c = 0; c < M1.getCols(); ++c)
  				if (Math.abs(M1.getElement(r, c) - M2.getElement(r, c)) > delta) return false;
  		return true;
  	}
  
  	/**
  	 * Computes and returns the element-wise maximum of all given matrices. 
  	 * @param matrices  A comma-separated list or an array of matrices.
  	 * @return  A new matrix with the element-wise maximums of all given input matrices.
  	 */
  	public static SimpleMatrix max(final SimpleMatrix... matrices) {
  		assert matrices.length >= 1 : new IllegalArgumentException("Provide at least one vector!");
  		SimpleMatrix result = matrices[0].clone();
  		final int rows = matrices[0].getRows();
  		final int cols = matrices[0].getCols();
  		for (int i = 1; i < matrices.length; ++i) {
  			assert (matrices[i].getRows() == rows) && (matrices[i].getCols() == cols) : new IllegalArgumentException("All matrices must have the same size!");
  			for (int r = 0; r < rows; ++r)
  				for (int c = 0; c < cols; ++c)
  					if (matrices[i].getElement(r, c) > result.getElement(r, c)) result.setElementValue(r, c, matrices[i].getElement(r, c));
  		}
  		return result;
  	}
  
  	/**
  	 * Computes and returns the element-wise minimum of all given matrices. 
  	 * @param matrices  A comma-separated list or an array of matrices.
  	 * @return  A new matrix with the element-wise minimums of all given input matrices.
  	 */
  	public static SimpleMatrix min(final SimpleMatrix... matrices) {
  		assert matrices.length >= 1 : new IllegalArgumentException("Provide at least one vector!");
  		SimpleMatrix result = matrices[0].clone();
  		final int rows = matrices[0].getRows();
  		final int cols = matrices[0].getCols();
  		for (int i = 1; i < matrices.length; ++i) {
  			assert (matrices[i].getRows() == rows) && (matrices[i].getCols() == cols) : new IllegalArgumentException("All matrices must have the same size!");
  			for (int r = 0; r < rows; ++r)
  				for (int c = 0; c < cols; ++c)
  					if (matrices[i].getElement(r, c) < result.getElement(r, c)) result.setElementValue(r, c, matrices[i].getElement(r, c));
  		}
  		return result;
  	}
  	
  	/**
  	 * Creates a new matrix which is composed of all input matrices, stacked on top of each other. 
  	 * All input matrices need to have the same number of columns.
  	 * @param parts  The matrices to concatenate.
  	 * @return  The vertically concatenated matrix.
  	 */
  	public static SimpleMatrix concatenateVertically(SimpleMatrix... parts) {
  		final int noParts = parts.length;
  		assert noParts >= 1 : new IllegalArgumentException("Supply at least one matrix to concatenate!");
  		final int[] noRows = new int[noParts];
  		final int noCols = parts[0].getCols();
  		final int[] accumulatedRowPosition = new int[noParts];
  		int totalLength = 0;
  		for (int i = 0; i < noParts; ++i) {
  			assert noCols != parts[i].getCols() : new IllegalArgumentException("Number of columns in all provided matrices needs to be the save for vertical concatenation!");
  			
  			noRows[i] = parts[i].getRows();
  			accumulatedRowPosition[i] = (i == 0) ? 0 : (accumulatedRowPosition[i-1] + noRows[i-1]);
  			totalLength += noRows[i];
  		}
  		SimpleMatrix result = new SimpleMatrix(totalLength, noCols);
  		for (int i = 0; i < noParts; ++i) result.setSubMatrixValue(accumulatedRowPosition[i],0,parts[i]);
  		return result;
  	}
  	
  	/**
  	 * Creates a new matrix which is composed of all input matrices, positioned next to each other. 
  	 * All input matrices need to have the same number of rows.
  	 * @param parts  The matrices to concatenate.
  	 * @return  The vertically concatenated matrix.
  	 */
  	public static SimpleMatrix concatenateHorizontally(SimpleMatrix... parts) {
  		final int noParts = parts.length;
  		assert noParts >= 1 : new IllegalArgumentException("Supply at least one matrix to concatenate!");
  		final int[] noCols = new int[noParts];
  		final int noRows = parts[0].getRows();
  		final int[] accumulatedColPosition = new int[noParts];
  		int totalLength = 0;
  		for (int i = 0; i < noParts; ++i) {
  			assert noRows != parts[i].getRows() : new IllegalArgumentException("Number of columns in all provided matrices needs to be the save for vertical concatenation!");
  			
  			noCols[i] = parts[i].getCols();
  			accumulatedColPosition[i] = (i == 0) ? 0 : (accumulatedColPosition[i-1] + noCols[i-1]);
  			totalLength += noCols[i];
  		}
  		SimpleMatrix result = new SimpleMatrix(totalLength, noRows);
  		for (int i = 0; i < noParts; ++i) result.setSubMatrixValue(0,accumulatedColPosition[i],parts[i]);
  		return result;
  	}
  	
  	
  	// **************************************************************** //
  	// ******************* Matrix/Vector operators ******************** //
  	// **************************************************************** //
  
  	/**
  	 * Performs a standard matrix-vector product.
  	 * @param M  A matrix, used as first factor.
  	 * @param v  A vector, used as second factor.
  	 * @return  The matrix-vector product {@latex.inline $\\mathbf{M} \\cdot \\mathbf{v}$}.
  	 */
  	public static SimpleVector multiply(final SimpleMatrix M, final SimpleVector v) {
  		assert (M.getCols() == v.getLen()) : new IllegalArgumentException("Matrix and Vector dimensions must match for multiplication!");
  		final SimpleVector result = new SimpleVector(M.getRows());
  		for (int r = 0; r < M.getRows(); ++r)
  			result.setElementValue(r, SimpleOperators.multiplyInnerProd(M.getRow(r), v));
  		return result;
  	}
  
  	/**
  	 * Performs a vector-matrix product, assuming a row vector.
  	 * @param v  A vector, assumed to be a row vector, used as first factor.
  	 * @param M  A matrix, used as second factor.
  	 * @return  The vector-matrix product {@latex.inline $\\mathbf{v}^T \\cdot \\mathbf{M}$}.
  	 */
  	public static SimpleVector multiply(final SimpleVector v, final SimpleMatrix M) {
  		assert (v.getLen() == M.getRows()) : new IllegalArgumentException("Matrix and Vector dimensions must match for multiplication!");
  		final SimpleVector result = new SimpleVector(M.getCols());
  		for (int c = 0; c < M.getCols(); ++c)
  			result.setElementValue(c, SimpleOperators.multiplyInnerProd(v, M.getCol(c)));
  		return result;
  	}
  	
  	/**
  	 * Performs an interpolation between two rigid transformations (rotation and translation) 
  	 * matrices, represented as 4x4 affine matrices.
  	 * 
  	 * @param A First 3D Rigid transform matrix of size 4x4
  	 * @param B Second 3D Rigid transform matrix of size 4x4
  	 * @param weightA weight for first rigid transform
  	 * @param weightB weight for second transform
  	 * 
  	 * @return interpolated rigid transform matrix of size 4x4
  	 */
  	public static SimpleMatrix interpolateRigidTransforms3D(SimpleMatrix A, SimpleMatrix B, double weightA, double weightB){
  		assert(A.isRigidMotion3D(CONRAD.DOUBLE_EPSILON) && B.isRigidMotion3D(CONRAD.DOUBLE_EPSILON)) : new IllegalArgumentException("Matrix interpolation requires 3D rigid motion matrices (rotation + translatio only) of size 4x4!");
  		
  		boolean considerTranslationOnly = A.getSubMatrix(0, 0, 3, 3).isIdentity(CONRAD.DOUBLE_EPSILON);
  		considerTranslationOnly &= B.getSubMatrix(0, 0, 3, 3).isIdentity(CONRAD.DOUBLE_EPSILON);
  		
  		// make sure weights add up to 1
  		double sum = (weightA+weightB);
  		weightA /= sum;
  		weightB /= sum;
  		
  		SimpleMatrix outR = null;
  		if(!considerTranslationOnly){
  			SimpleMatrix firstRpart = A.getSubMatrix(0,0,3,3);
  			SimpleMatrix scndRpart = B.getSubMatrix(0,0,3,3);
  			SimpleMatrix firstInverseRpart = firstRpart.inverse(InversionType.INVERT_SVD);
  			SimpleMatrix T = SimpleOperators.multiplyMatrixProd(firstInverseRpart,scndRpart);
  			Matrix jamT = new Matrix(T.copyAsDoubleArray());
  			EigenvalueDecomposition evd = jamT.eig();
  			double[] real = evd.getRealEigenvalues();
  			double[] imag = evd.getImagEigenvalues();
  			for (int i = 0; i < 3; i++) {
  				double angle = Math.atan2(imag[i], real[i]);
  				angle*=weightB;
  				real[i] = Math.cos(angle);
  				imag[i] = Math.sin(angle);
  			}
  			Matrix newR = evd.getV().times(evd.getD()).times(evd.getV().inverse());
  			outR = SimpleOperators.multiplyMatrixProd(firstRpart, new SimpleMatrix(newR.getArrayCopy()));
  		}
  		
  		SimpleVector outCol = A.getSubCol(0, 3, 3).multipliedBy(weightA);
  		outCol.add(B.getSubCol(0, 3, 3).multipliedBy(weightB));
  		SimpleMatrix out = new SimpleMatrix(4,4);
  		out.identity();
  		out.setSubColValue(0, 3, outCol);
  		if(!considerTranslationOnly){
  			out.setSubMatrixValue(0, 0, outR);
  		}
  		return out;
  	}
  	
  	/**
  	 * Performs an interpolation between two 2D rigid transformations (rotation and translation)
  	 * matrices, represented as 3x3 affine matrices.
  	 * 
  	 * @param A First 2D Rigid transform matrix of size 3x3
  	 * @param B Second 2D Rigid transform matrix of size 3x3
  	 * @param weightA weight for first rigid transform
  	 * @param weightB weight for second transform
  	 * 
  	 * @return interpolated rigid transform matrix of size 3x3
  	 */
  	public static SimpleMatrix interpolateRigidTransforms2D(SimpleMatrix A, SimpleMatrix B, double weightA, double weightB){
  		assert(A.isRigidMotion2D(CONRAD.DOUBLE_EPSILON) && B.isRigidMotion2D(CONRAD.DOUBLE_EPSILON)) : new IllegalArgumentException("Matrix interpolation requires 2D rigid motion matrices (rotation + translatio only) of size 3x3!");
  		
  		boolean considerTranslationOnly = A.getSubMatrix(0, 0, 2, 2).isIdentity(CONRAD.DOUBLE_EPSILON);
  		considerTranslationOnly &= B.getSubMatrix(0, 0, 2, 2).isIdentity(CONRAD.DOUBLE_EPSILON);
  		
  		// make sure weights add up to 1
  		double sum = (weightA+weightB);
  		weightA /= sum;
  		weightB /= sum;
  		
  		SimpleMatrix outR = null;
  		if(!considerTranslationOnly){
  			SimpleMatrix firstRpart = A.getSubMatrix(0,0,2,2);
  			SimpleMatrix scndRpart = B.getSubMatrix(0,0,2,2);
  			SimpleMatrix firstInverseRpart = firstRpart.inverse(InversionType.INVERT_SVD);
  			SimpleMatrix T = SimpleOperators.multiplyMatrixProd(firstInverseRpart,scndRpart);
  			Matrix jamT = new Matrix(T.copyAsDoubleArray());
  			EigenvalueDecomposition evd = jamT.eig();
  			double[] real = evd.getRealEigenvalues();
  			double[] imag = evd.getImagEigenvalues();
  			for (int i = 0; i < 2; i++) {
  				double angle = Math.atan2(imag[i], real[i]);
  				angle*=weightB;
  				real[i] = Math.cos(angle);
  				imag[i] = Math.sin(angle);
  			}
  			Matrix newR = evd.getV().times(evd.getD()).times(evd.getV().inverse());
  			outR = SimpleOperators.multiplyMatrixProd(firstRpart, new SimpleMatrix(newR.getArrayCopy()));
  		}
  		
  		SimpleVector outCol = A.getSubCol(0, 2, 2).multipliedBy(weightA);
  		outCol.add(B.getSubCol(0, 2, 2).multipliedBy(weightB));
  		SimpleMatrix out = new SimpleMatrix(3,3);
  		out.identity();
  		out.setSubColValue(0, 2, outCol);
  		if(!considerTranslationOnly){
  			out.setSubMatrixValue(0, 0, outR);
  		}
  		return out;
  	}
  	
  	/**
  	 * Computes the mean rigid transform of a 3D transformation
  	 * @param inputTransforms A collection of rigid transform matrices of size 4x4
  	 * @return The mean rigid transform of the collections transform
  	 */
  	public static SimpleMatrix getMeanRigidTransform3D(Iterable<SimpleMatrix> inputTransforms){
  		Iterator<SimpleMatrix> iter = inputTransforms.iterator();
  		SimpleMatrix compareOut = SimpleMatrix.I_4.clone();
  		int ctr = 0;
  		while(iter.hasNext()){
  			compareOut = SimpleOperators.interpolateRigidTransforms3D(
  					iter.next(),
  					compareOut,
  					1, ctr);
  			ctr++;
  		}
  		return compareOut;
  	}
  	
  	
  	/**
  	 * Computes the mean rigid transform of a collection of 2D transformation
  	 * @param inputTransforms A collection of 2D rigid transform matrices of size 3x3
  	 * @return The mean rigid transform of the collections transform
  	 */
  	public static SimpleMatrix getMeanRigidTransform2D(Iterable<SimpleMatrix> inputTransforms){
  		Iterator<SimpleMatrix> iter = inputTransforms.iterator();
  		SimpleMatrix compareOut = SimpleMatrix.I_3.clone();
  		int ctr = 0;
  		while(iter.hasNext()){
  			compareOut = SimpleOperators.interpolateRigidTransforms2D(
  					iter.next(),
  					compareOut,
  					1, ctr);
  			ctr++;
  		}
  		return compareOut;
  	}
  	
  	/**
  	 * method to compute the Pluecker dual coordinates of a vector L
  	 * @param L: SimpleVector having 6 elements
  	 * @return: SimpleMatrix of size 4x4
  	 */
  	public static SimpleMatrix getPlueckerMatrixDual(SimpleVector L) {
  		
  		SimpleMatrix L_out = new SimpleMatrix(4, 4);
  		// first row
  		L_out.setElementValue(0, 1, +L.getElement(5));
  		L_out.setElementValue(0, 2, -L.getElement(4));
  		L_out.setElementValue(0, 3, +L.getElement(3));
  		
  		// second row
  		L_out.setElementValue(1, 0, -L.getElement(5));
  		L_out.setElementValue(1, 2, +L.getElement(2));
  		L_out.setElementValue(1, 3, -L.getElement(1));
  		
  		// third row
  		L_out.setElementValue(2, 0, +L.getElement(4));
  		L_out.setElementValue(2, 1, -L.getElement(2));
  		L_out.setElementValue(2, 3, +L.getElement(0));
  		
  		// last row
  		L_out.setElementValue(3, 0, -L.getElement(3));
  		L_out.setElementValue(3, 1, +L.getElement(1));
  		L_out.setElementValue(3, 2, -L.getElement(0));
  		
  		return L_out;
  		
  	}
  
  
  	/**
  	 * method to compute the Pluecker join of a line L and a point X
  	 * @param L: line L as SimpleVector (6 entries)
  	 * @param X: point X as SimpleVector (4 entries)
  	 * @return: SimpleVector of size 4x1 representing a plane
  	 */
  	public static SimpleVector getPlueckerJoin(SimpleVector L, SimpleVector X) {
  		
  		//* calculate plane E (4x1) from [~L]x * X *//
  		double v1 = + X.getElement(1)*L.getElement(5) - X.getElement(2)*L.getElement(4) + X.getElement(3)*L.getElement(3);
  		double v2 = - X.getElement(0)*L.getElement(5) + X.getElement(2)*L.getElement(2) - X.getElement(3)*L.getElement(1);
  		double v3 = + X.getElement(0)*L.getElement(4) - X.getElement(1)*L.getElement(2) + X.getElement(3)*L.getElement(0);
  		double v4 = - X.getElement(0)*L.getElement(3) + X.getElement(1)*L.getElement(1) - X.getElement(2)*L.getElement(0);
  		
  		return new SimpleVector(v1, v2, v3, v4);
  		
  	}
  
  
  }
  /*
   * Copyright (C) 2010-2014 Andreas Keil
   * CONRAD is developed as an Open Source project under the GNU General Public License (GPL).
  */
  