// KinematicsBasicMapping.cpp: implementation of the CKinematicsBasicMapping class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "KinematicsBasicMapping.h"


//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CKinematicsBasicMapping::CKinematicsBasicMapping()
{
}


CKinematicsBasicMapping::~CKinematicsBasicMapping()
{

}


int CKinematicsBasicMapping::TransformCADtoActuators(double x, double y, double z, double a, double b, double c, double *Acts, bool NoGeo)
{

	double x1 = x;
	double y1 = y;
	double z1 = z;
	double a1 = a;
	double b1 = b;
	double c1 = c;


	Acts[0] = x1*m_MotionParams.CountsPerInchX;
	Acts[1] = y1*m_MotionParams.CountsPerInchY;
	Acts[2] = z1*m_MotionParams.CountsPerInchZ;

	Acts[3] = a1*m_MotionParams.CountsPerInchA;
	Acts[4] = b1*m_MotionParams.CountsPerInchB;
	Acts[5] = c1*m_MotionParams.CountsPerInchC;

	return 0;
}


// perform Inversion to go the other way

int CKinematicsBasicMapping::TransformActuatorstoCAD(double *Acts, double *xr, double *yr, double *zr, double *ar, double *br, double *cr, bool NoGeo)
{
	return InvertTransformCADtoActuators(Acts, xr, yr, zr, ar, br, cr);
}