#pragma once
|
|
#include <vector>
|
#include <math.h>
|
|
enum CoordinateDirection { COORDINATE_DIRECTION_FORWARD = 1, COORDINATE_DIRECTION_BACKWARD = -1 };
|
|
#define M_PI 3.14159265358979323846
|
#define ToRadian(degree) ( (degree) * (M_PI/180.0) )
|
#define ToDegree(radian) ( (radian) * (180.0/M_PI) )
|
|
struct SMarkPosition
|
{
|
SMarkPosition()
|
{
|
Reset();
|
}
|
|
void Reset()
|
{
|
dPositionX = 0.;
|
dPositionY = 0.;
|
}
|
|
double dPositionX;
|
double dPositionY;
|
};
|
typedef std::vector<SMarkPosition> VectorMarkPosition;
|
typedef std::vector<SMarkPosition>::iterator VectorMarkPositionIt;
|
|
struct SAlignResult
|
{
|
SAlignResult()
|
{
|
Reset();
|
}
|
|
void Reset()
|
{
|
dDegree = 0.;
|
dRadian = 0.;
|
dOriginX = 0.;
|
dOriginY = 0.;
|
dSinValue = 0.;
|
dCosValue = 0.;
|
|
memset(dRotateMatrix, 0, sizeof(dRotateMatrix));
|
memset(dRotateMatrixInv, 0, sizeof(dRotateMatrixInv));
|
}
|
|
void RotatePos(double& dX, double& dY) const
|
{
|
double tx = dX;
|
double ty = dY;
|
|
dX = dRotateMatrix[0]*tx + dRotateMatrix[1]*ty + dRotateMatrix[2];
|
dY = dRotateMatrix[3]*tx + dRotateMatrix[4]*ty + dRotateMatrix[5];
|
}
|
|
void InvRotatePos(double& dX, double& dY) const
|
{
|
double tx = dX;
|
double ty = dY;
|
|
dX = dRotateMatrixInv[0]*tx + dRotateMatrixInv[1]*ty + dRotateMatrixInv[2];
|
dY = dRotateMatrixInv[3]*tx + dRotateMatrixInv[4]*ty + dRotateMatrixInv[5];
|
}
|
|
void SetRotateMatrix(double dCenterX, double dCenterY)
|
{
|
double tx = dCenterX;
|
double ty = dCenterY;
|
double tc = cos(dRadian);
|
double ts = sin(dRadian);
|
|
dRotateMatrix[0] = tc;
|
dRotateMatrix[1] = -ts;
|
dRotateMatrix[2] = tx-(tc*tx)+(ts*ty);
|
dRotateMatrix[3] = ts;
|
dRotateMatrix[4] = tc;
|
dRotateMatrix[5] = ty-(ts*tx)-(tc*ty);
|
dRotateMatrix[6] = 0.0;
|
dRotateMatrix[7] = 0.0;
|
dRotateMatrix[8] = 1.0;
|
|
tc = cos(-dRadian);
|
ts = sin(-dRadian);
|
dRotateMatrixInv[0] = tc;
|
dRotateMatrixInv[1] = -ts;
|
dRotateMatrixInv[2] = tx-(tc*tx)+(ts*ty);
|
dRotateMatrixInv[3] = ts;
|
dRotateMatrixInv[4] = tc;
|
dRotateMatrixInv[5] = ty-(ts*tx)-(tc*ty);
|
dRotateMatrixInv[6] = 0.0;
|
dRotateMatrixInv[7] = 0.0;
|
dRotateMatrixInv[8] = 1.0;
|
}
|
|
double dRadian;
|
double dDegree;
|
double dOriginX;
|
double dOriginY;
|
double dSinValue;
|
double dCosValue;
|
|
double dRotateMatrix[9];
|
double dRotateMatrixInv[9];
|
};
|
|
|
class AFX_EXT_CLASS CAlignCalibrator
|
{
|
public:
|
CAlignCalibrator();
|
virtual ~CAlignCalibrator(void);
|
|
void SetDirection(int nXDir, int nYDir) { m_dDirectionX=nXDir; m_dDirectionY=nYDir; }
|
void SetFirstMarkPos(double dMotorX, double dMotorY, double dGlassX, double dGlassY);
|
void SetSecondMarkPos(double dMotorX, double dMotorY, double dGlassX, double dGlassY);
|
|
void GetDirection(int& nXDir, int& nYDir) const { nXDir=int(m_dDirectionX); nYDir=int(m_dDirectionY); }
|
SAlignResult GetAlignResult() const { return m_sAlignResult; }
|
void SetAlignResult(SAlignResult sAlignResult) { m_sAlignResult = sAlignResult; }
|
|
BOOL CalculateAlignResult();
|
|
protected:
|
BOOL TransformGlass2Motor(double dGlassX, double dGlassY, double& dMotorX, double& dMotorY) const;
|
BOOL TransformMotor2Glass(double dMotorX, double dMotorY, double& dGlassX, double& dGlassY) const;
|
|
BOOL TransformGlass2MotorTest(double dGlassX, double dGlassY, double& dMotorX, double& dMotorY);
|
BOOL TransformMotor2GlassTest(double dMotorX, double dMotorY, double& dGlassX, double& dGlassY);
|
|
BOOL CalculateAxisX();
|
BOOL CalculateAxisY();
|
|
int m_nResultCode;
|
SAlignResult m_sAlignResult;
|
|
double m_dDirectionX;
|
double m_dDirectionY;
|
|
SMarkPosition m_pMotorPos[2];
|
SMarkPosition m_pGlassPos[2];
|
|
};
|
