//This is the last copy of 2D transforms that I attempted before giving them up for 
//one dimenstional arrays.  It seems thats what the computer uses and
//doesn't like having to emulate anything else.

#include "stdAfx.h"
#include "PointGroup.h"
#include "PointClass.h"
#include "Mesh.h"
#include "Transform2D.h"
#include "VectorMatrixMath.h"
#include <math.h>

#define PI 3.14159
#define DEGPERRAD 57.2958 //Degrees per radian r 'radians' = 360/(2*PI)'degrees'
//Row column emulation for a 3x3 matrix using a 1D array
#define M3X3  9
#define ROW0 3*0
#define ROW1 3*1
#define ROW2 3*2
#define COL0 0
#define COL1 1
#define COL2 2

//rotate PointGroup pointCloud about p1
void Transform2D::RotateAtP1(/*in*/point3D p1,/*in*/PointGroup *inPointCloud,
							 /*out*/PointGroup *outPointCloud,/*in*/double theta,/*in*/double *oldMatrix) 
{
	//PROCEEDURE DESCRIPTION
	//1. Translate such that p1 is at the origin
	//2. Rotate / Transform
	//3.Translate such that the point at the origin returns to p1
	//[cos(theta)   -sin(theta)		x1(1-cos(theta)+y1sin(theta)]
	//[sin(theta)	cos(theta)		y1(1-cos(theta)-x1sin(theta)]
	//[0			0				1							]
	//Matrix emulation with a 1d array(pointer problem with 2D)
	//row = (row size)*(row number) range 0 thru n-1 where n is the #rows
	//column = (column number) range 0 thru n-1 where n is the #columns



	//INITIALIZATION OF VARIABLES
	int numberPoints = 0; 
	double cosineTheta = 0;
	double sinTheta = 0;
	double newMatrix[M3X3];   //New Matrix based on rotate of theta, emulate a 3x3 matrix
	double resultMatrix[M3X3];
	point3D *transitionPoint; //
	point3D toOutPointCloud;  //
	VectorMatrixMath moveIt;	

	// make this function part of a more general matrix-api(tm)
	//Make sure newMatrix is full of 0s

	for(int i = 0; i < 9; i++) {
		newMatrix[i] = 0;
	}

	//1. TRANSLATE p1 to origin

	numberPoints = inPointCloud->GetNumPoints();//Get the number of points for use in translation and transformation

	moveIt.Translate(p1, inPointCloud, outPointCloud, numberPoints);

	//2. ROTATE / TRANSFORM

	//Convert theta into radians for sin/cos transformation
	theta = theta/DEGPERRAD;

	//calculate sin and cosine for rotation
	cosineTheta = cos(theta);
	sinTheta = sin(theta);

	//Initailize newMatrix to the new values.
	newMatrix[ROW0+COL0] = cosineTheta;
	newMatrix[ROW0+COL1] = -sinTheta;
	newMatrix[ROW0+COL2] = (p1.x * (1 - cosineTheta)) + (p1.y * (sinTheta));
	newMatrix[ROW1+COL0] = sinTheta;
	newMatrix[ROW1+COL1] = cosineTheta; //3*1 takes care of the 
	newMatrix[ROW1+COL2] = (p1.y * (1 - cosineTheta)) - (p1.x * (sinTheta));
	newMatrix[ROW2+COL0] = 0;
	newMatrix[ROW2+COL1] = 0;
	newMatrix[ROW2+COL2] = 1;

	//Create the transform matrix by putting the new and the old matricies together
	
	moveIt.MatrixMultiply3x3(oldMatrix, newMatrix, resultMatrix);
	for(i = 0; i < M3X3; i++) {
		oldMatrix[i] = resultMatrix[i];
	}
		
	//Modify this routine to use outPoint cloud as both the inPut and output of this for statment
	//Concept.  
	//Get the point into transition point from outPoint cloud.
	for(i = 0; i < numberPoints; i++) {
		transitionPoint = outPointCloud->GetPoint(i); //Get the current point for transformation
		toOutPointCloud.x = (transitionPoint->x * oldMatrix[ROW0+COL0]) + (transitionPoint->y * oldMatrix[ROW0+COL1]) +
			(transitionPoint->z * oldMatrix[ROW0+COL2]);
		toOutPointCloud.y = ((transitionPoint->x * oldMatrix[ROW1+COL0]) + (transitionPoint->y * oldMatrix[ROW1+COL1]) +
			(transitionPoint->z * oldMatrix[ROW1+COL2]));
		toOutPointCloud.z = ((transitionPoint->x * oldMatrix[ROW2+COL0]) + (transitionPoint->y * oldMatrix[ROW2+COL1]) +
			(transitionPoint->z * oldMatrix[ROW2+COL2]));
		outPointCloud->SetPoint(i, toOutPointCloud.x, toOutPointCloud.y, toOutPointCloud.z);
	}

	//TRANSLATE p1 back to where it belongs
	//use the toOutPointCloud variable to translate the object back to it's original location
	toOutPointCloud.x = -p1.x;
	toOutPointCloud.y = -p1.y;
	toOutPointCloud.z = -p1.z;
	moveIt.Translate(toOutPointCloud, outPointCloud, numberPoints);
}

void Transform2D::Rotate3D( /*in*/  float fYaw,
						    /*in*/  float fRoll,
							/*in*/  float fPitch,
						    /*in*/  PointGroup *pObjectCoordGroup,
							/*out*/ PointGroup *pWorldCoordGroup
						   ) 
{
	// calculated world-coordinate points

	float fX, fY, fZ;
	float fTemp;

	point3D *pPoint;


	// loop through all the points in the group
	// these points are in object-coordinate space

	for ( int i = 0; i < pObjectCoordGroup->GetNumPoints(); i++ ) 
	{
		pPoint = pObjectCoordGroup->GetPoint( i );

		fX = pPoint->x * cos( fRoll ) + pPoint->y * sin( fRoll );
		fY = pPoint->x * sin( fRoll ) - pPoint->y * cos( fRoll );

		fTemp = fX;

		fX = fTemp * cos( fYaw ) + pPoint->z * sin( fYaw );
		fZ = fTemp * sin( fYaw ) - pPoint->z * cos( fYaw );

		fTemp = fZ;

		fZ = fTemp * cos( fPitch ) + fY * sin( fPitch );
		fY = fTemp * sin( fPitch ) - fY * cos( fPitch );

		pWorldCoordGroup->SetPoint( i, fX, fY, fZ );
	}
}

void Transform2D::WorldToViewport( /*in*/  float fWidth,
		    	                   /*in*/  float fHeight,
				                   /*in*/  float fDis,
				                   /*in*/  PointGroup *pWorldCoordGroup,
				                   /*out*/ PointGroup *pViewportCoordGroup
				                 )
{
	// calculated world coordinate points

	float fViewX, fViewY;

	point3D *pPoint;

	// loop through all the points in the group
	// these points are in world-coordinate space

	for ( int i = 0; i < pWorldCoordGroup->GetNumPoints(); i++ ) 
	{
		pPoint = pWorldCoordGroup->GetPoint( i );

		fViewX = fDis * pPoint->x / pPoint->z;
		fViewY = fDis * pPoint->y / pPoint->z;

		pViewportCoordGroup->SetPoint( i, fViewX, fViewY, fDis );
	}
}
