//#####################################################################
// Copyright 2004-2005, Eftychios Sifakis.
// This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt.
//#####################################################################
// Class QUASI_RIGID_TRANSFORM_3D
//#####################################################################
#ifndef __QUASI_RIGID_TRANSFORM_3D__
#define __QUASI_RIGID_TRANSFORM_3D__

#include "../../Public_Library/Constitutive_Models/DIAGONALIZED_ISOTROPIC_STRESS_DERIVATIVE_3D.h"

namespace PhysBAM
{

template <class T>
class QUASI_RIGID_TRANSFORM_3D
{
public:
	MATRIX_3X3<T> affine_transform;
	VECTOR_3D<T> translation;

	QUASI_RIGID_TRANSFORM_3D()
		: affine_transform (MATRIX_3X3<T>::Identity_Matrix())
	{}

	QUASI_RIGID_TRANSFORM_3D (const MATRIX_3X3<T>& affine_transform_input, const VECTOR_3D<T>& translation_input)
		: affine_transform (affine_transform_input), translation (translation_input)
	{}

	T operator() (const int i) const
	{
		assert (1 <= i && i <= 12);
		return i <= 9 ? affine_transform.x[i - 1] : translation[i - 9];
	}

	T& operator() (const int i)
	{
		assert (1 <= i && i <= 12);
		return i <= 9 ? affine_transform.x[i - 1] : translation[i - 9];
	}

	T Rigidity_Penalty() const
	{
		SYMMETRIC_MATRIX_3X3<T> strain = affine_transform.Normal_Equations_Matrix() - 1;
		return strain.Frobenius_Norm_Squared();
	}

	T Rigidity_Penalty_Gradient (const int i) const
	{
		assert (1 <= i && i <= 12);

		if (i > 9) return 0;

		MATRIX_3X3<T> gradient = affine_transform * (affine_transform.Normal_Equations_Matrix() - 1);
		return (T) 4 * gradient.x[i - 1];
	}

	T Ridigity_Penalty_Hessian_Definite_Part (const int i, const int j) const
	{
		assert (1 <= i && i <= 12 && 1 <= j && j <= 12);

		if (i > 9 || j > 9) return 0;

		MATRIX_3X3<T> U, V;
		DIAGONAL_MATRIX_3X3<T> Sigma;
		affine_transform.Fast_Singular_Value_Decomposition (U, Sigma, V);
		DIAGONALIZED_ISOTROPIC_STRESS_DERIVATIVE_3D<T> rotated_hessian;
		rotated_hessian.x1111 = (T) 12 * sqr (Sigma.x11) - (T) 4;
		rotated_hessian.x2222 = (T) 12 * sqr (Sigma.x22) - (T) 4;
		rotated_hessian.x3333 = (T) 12 * sqr (Sigma.x33) - (T) 4;
		rotated_hessian.x2211 = 0;
		rotated_hessian.x3311 = 0;
		rotated_hessian.x3322 = 0;
		rotated_hessian.x2121 = (T) 4 * (sqr (Sigma.x11) + sqr (Sigma.x22) - (T) 1);
		rotated_hessian.x3131 = (T) 4 * (sqr (Sigma.x11) + sqr (Sigma.x33) - (T) 1);
		rotated_hessian.x3232 = (T) 4 * (sqr (Sigma.x22) + sqr (Sigma.x33) - (T) 1);
		rotated_hessian.x2112 = (T) 4 * Sigma.x11 * Sigma.x22;
		rotated_hessian.x3113 = (T) 4 * Sigma.x11 * Sigma.x33;
		rotated_hessian.x3223 = (T) 4 * Sigma.x22 * Sigma.x33;
		rotated_hessian.Fix_Indefinite_Blocks();
		MATRIX_3X3<T> dF1, dF2;
		dF1.x[i - 1] = (T) 1;
		dF2.x[j - 1] = (T) 1;
		dF1 = U.Transpose_Times (dF1) * V;
		dF2 = U.Transpose_Times (dF2) * V;
		return dF1.Transpose_Times (rotated_hessian.Differential (dF2)).Trace();
	}

	static QUASI_RIGID_TRANSFORM_3D<T> Incremental_Transform (const QUASI_RIGID_TRANSFORM_3D<T>& target_transform, const QUASI_RIGID_TRANSFORM_3D<T>& initial_transform)
	{
		MATRIX_3X3<T> affine_transform_incremental = target_transform.affine_transform * initial_transform.affine_transform.Inverse();
		VECTOR_3D<T> translation_incremental = target_transform.translation - affine_transform_incremental * initial_transform.translation;
		return QUASI_RIGID_TRANSFORM_3D<T> (affine_transform_incremental, translation_incremental);
	}

	static QUASI_RIGID_TRANSFORM_3D<T> Composite_Transform (const QUASI_RIGID_TRANSFORM_3D<T>& master_transform, const QUASI_RIGID_TRANSFORM_3D<T>& slave_transform)
	{
		MATRIX_3X3<T> affine_transform_composite = master_transform.affine_transform * slave_transform.affine_transform;
		VECTOR_3D<T> translation_composite = master_transform.affine_transform * slave_transform.translation + master_transform.translation;
		return QUASI_RIGID_TRANSFORM_3D<T> (affine_transform_composite, translation_composite);
	}

	FRAME<T> Frame()
	{
		MATRIX_3X3<T> U, V;
		DIAGONAL_MATRIX_3X3<T> Sigma;
		affine_transform.Fast_Singular_Value_Decomposition (U, Sigma, V);
		return FRAME<T> (translation, U.Multiply_With_Transpose (V));
	}

	static QUASI_RIGID_TRANSFORM_3D<T> Interpolate (const QUASI_RIGID_TRANSFORM_3D<T>& initial, const QUASI_RIGID_TRANSFORM_3D<T>& final, const T interpolation_fraction)
	{
		QUASI_RIGID_TRANSFORM_3D<T> result;
		result.translation = LINEAR_INTERPOLATION<T, VECTOR_3D<T> >::Linear (initial.translation, final.translation, interpolation_fraction);
		QUATERNION<T> initial_rotation (initial.affine_transform), final_rotation (final.affine_transform);
		result.affine_transform = QUATERNION<T>::Spherical_Linear_Interpolation (initial_rotation, final_rotation, interpolation_fraction).Matrix_3X3();
		//result.affine_transform=LINEAR_INTERPOLATION<T,MATRIX_3X3<T> >::Linear(initial.affine_transform,final.affine_transform,interpolation_fraction);result.Make_Rigid();
		return result;
	}

	void Make_Rigid()
	{
		MATRIX_3X3<T> U, V;
		DIAGONAL_MATRIX_3X3<T> Sigma;
		affine_transform.Fast_Singular_Value_Decomposition (U, Sigma, V);
		affine_transform = U.Multiply_With_Transpose (V);
	}

	void Print_Diagnostics (std::ostream& output = std::cout) const
	{
		output << "Transformation matrix :" << std::endl << affine_transform;
		MATRIX_3X3<T> U, V;
		DIAGONAL_MATRIX_3X3<T> Sigma;
		affine_transform.Fast_Singular_Value_Decomposition (U, Sigma, V);
		output << "Singular values : " << Sigma.x11 << " , " << Sigma.x22 << " , " << Sigma.x33 << std::endl;
		output << "Translation : " << translation << std::endl;
		output << "Rigidity penalty at current configuration : " << Rigidity_Penalty() << std::endl;
	}

//#####################################################################
};
}
#endif