#include "CameraFrame.h"

namespace DFF_VideoMat
{

CCameraFrame::CCameraFrame():m_K(Eigen::Matrix3d::Identity()),
m_objAbsRT(Eigen::Matrix4d::Identity()),
m_viewAbsRT(Eigen::Matrix4d::Identity())
{
}

CCameraFrame::~CCameraFrame(){}

Eigen::Matrix3d CCameraFrame::GetObjAbsRot() const
{
	return m_objAbsRT.block<3,3>(0,0);
}
Eigen::Matrix3d CCameraFrame::GetViewAbsRot() const
{
	return m_viewAbsRT.block<3,3>(0,0);
}

Eigen::Vector3d CCameraFrame::GetObjAbsT() const
{
	return m_objAbsRT.block<3,1>(0,3);
}
Eigen::Vector3d CCameraFrame::GetViewAbsT() const
{
	return m_viewAbsRT.block<3,1>(0,3);
}

Eigen::Matrix3d CCameraFrame::GetK() const
{
	return m_K;
}

Eigen::Matrix4d CCameraFrame::ConvertObjViewAbsRTF(const Eigen::Matrix4d& objAbsRT)
{
	Eigen::Matrix3d Rot = objAbsRT.block<3,3>(0,0);
	Eigen::Vector3d T = objAbsRT.block<3,1>(0,3);

	Rot.transposeInPlace();
	T = -Rot*T;

	Eigen::Matrix4d viewAbsRT;
	viewAbsRT.setIdentity();
	viewAbsRT.block<3,3>(0,0) = Rot;
	viewAbsRT.block<3,1>(0,3) = T;

	return viewAbsRT;
}

bool CCameraFrame::SetK(double fx,double fy,double cx,double cy,double skew)
{
	m_K.setIdentity();
	m_K(0,0) = fx;
	m_K(1,1) = fy;
	m_K(0,2) = cx;
	m_K(1,2) = cy;
	m_K(0,1) = skew;

	return true;
}
bool CCameraFrame::SetObjAbsRT(const Eigen::Matrix4d& objAbsRT)
{
	m_objAbsRT = objAbsRT;
	m_viewAbsRT = ConvertObjViewAbsRTF(objAbsRT);
	return true;
}
bool CCameraFrame::SetViewAbsRT(const Eigen::Matrix4d& viewAbsRT)
{
	m_viewAbsRT = viewAbsRT;
	m_objAbsRT = ConvertObjViewAbsRTF(viewAbsRT);
	return true;
}
}