#include "StdAfx.h"
#include "VideoFrame.h"
#include "../include/cximage.h"
#include "ZImageUtil.h"

using namespace std;

CVideoFrame::CVideoFrame(void)
{
}

CVideoFrame::~CVideoFrame(void)
{
}

void CVideoFrame::LoadColorImage()
{
	CxImage image;

	image.Load(m_sImgName.c_str());

	CxImageToZImage(image,m_ColorImg);
}


void CVideoFrame::LoadDepthImg()
{
	FILE* fp = fopen(m_sDepthName.c_str(),"rb");

	if(fp != NULL){
		m_DspImg.Create(m_iWidth,m_iHeight);
		fread(m_DspImg.GetMap(),sizeof(BYTE),m_DspImg.GetSize(),fp);
		fclose(fp);
	}
}

void CVideoFrame::PrintParameters()
{
	printf("Frame %d\n",m_iNo);

	for(int i=0; i<3; ++i){
		for(int j=0; j<3; ++j){			
			printf("%.7f\t",m_K(i,j));
		}
		printf("\n");
	}

	for(int i=0; i<3; ++i){
		for(int j=0; j<3; ++j){
			printf("%.7f\t",m_R(i,j));
		}
		printf("\n");
	}

	for(int i=0; i<3; ++i){		
		printf("%f\t",m_T[i]);
	}
	printf("\n\n");
}

CVector3 CVideoFrame::CameraCoordToWorldCoord(CVector3& pt3d)
{
	CVector3 w_pt3d;

	for(int i=0; i<3; ++i){
		w_pt3d[i] = 0;
		for(int j=0; j<3; ++j){
			w_pt3d[i] += m_R(i,j) * pt3d[j];
		}

		w_pt3d[i] += m_T[i];
	}

	return w_pt3d;
}