#include "stdafx.h"
#include "SO3Operations.h"
#include "nr.h"

void	so3projection(Affine3d &aff, Matrix3d &rot)
{
	Vec_DP w(3);
	Mat_DP a(3,3),u(3,3),v(3,3);

	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			a[i][j] = aff[j+1][i];
		}
	}
	for (int k=0; k<3; k++)
	{
		for (int l=0; l<3; l++) 
		{
			u[k][l]=a[k][l];
		}
	}
	NR::svdcmp(u,w,v);

	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			rot[j][i] = u[i][0]*v[j][0]+u[i][1]*v[j][1]+u[i][2]*v[j][2];
		}
	}
}

void	so3projection(Matrix3d &aff, Matrix3d &rot, Vector3d &diag)
{
	Vec_DP w(3);
	Mat_DP a(3,3),u(3,3),v(3,3);

	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			a[i][j] = aff[j][i];
		}
	}
	for (int k=0; k<3; k++)
	{
		for (int l=0; l<3; l++) 
		{
			u[k][l]=a[k][l];
		}
	}
	NR::svdcmp(u,w,v);

	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			rot[j][i] = u[i][0]*v[j][0]+u[i][1]*v[j][1]+u[i][2]*v[j][2];
		}
		w[i] = sqrt(w[i]);
	}
	diag[0] = max(max(w[0], w[1]), w[2]);
	diag[2] = min(min(w[0], w[1]), w[2]);
	diag[1] = w[0] + w[1] + w[2] - diag[0] - diag[2];
}

float	icpKnownCorres(vector<Vector3f> &sourcePositions, 
		vector<Vector3f> &targetPositions,vector<float> &fcWeights, 
		Affine3d &affine, Vector3d &diag)
{
	Matrix3d	buf, rot;
	Vector3f	centers, centert;
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			buf[i][j] = 0.0;
		}
		centers[i] = centert[i] = 0.0;
	}
	
	
	int	nFC = int(fcWeights.size());
	float	sumOfWeight = 0.f;
	for (int fcId = 0; fcId < nFC; fcId++)
	{
		Vector3f	&s = sourcePositions[fcId];
		Vector3f	&t = targetPositions[fcId];
		float		&w = fcWeights[fcId];
		centers += s*w;
		centert += t*w;
		sumOfWeight += w;
	}
	centers /= sumOfWeight;	centert /= sumOfWeight;

	for (int fcId = 0; fcId < nFC; fcId++)
	{
		Vector3f	coords = sourcePositions[fcId] - centers;
		Vector3f	coordt = targetPositions[fcId] - centert;
		float		&w = fcWeights[fcId];
		for (int i = 0; i < 3; i++)
		{
			for (int j = 0; j < 3; j++)
			{
				buf[i][j] += w*coords[i]*coordt[j];
			}
		}
	}
	so3projection(buf, rot, diag);
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			affine[i+1][j] = rot[i][j];
		}
	}
	for (int j = 0; j < 3; j++)
	{
		affine[0][j] = centert[j] - rot[0][j]*centers[0]
		- rot[1][j]*centers[1] - rot[2][j]*centers[2];
	}

	double	res = 0.f;
	for (int fcId = 0; fcId < nFC; fcId++)
	{
		Vector3f	coords = sourcePositions[fcId];
		Vector3f	coordt = targetPositions[fcId];
		float		&w = fcWeights[fcId];

		Vector3d	coords_trans = affine[0] + affine[1]*coords[0]
		+ affine[2]*coords[1] + affine[3]*coords[2];
		for (int j = 0; j < 3; j++)
		{
			double	dis = coords_trans[j] - coordt[j];
			res += dis*dis;
		}
	}
	res /= sumOfWeight;
	res = sqrt(res);
	return float(res);
}