SDC/A5_QD_CF_REVIEW.git

#include "StdAfx.h"
#include "CHReviewSetting/AlignCalibrator.h"
#include <math.h>
 
CAlignCalibrator::CAlignCalibrator()
{
    m_dDirectionX = 1.0;
    m_dDirectionY = 1.0;
 
    m_nResultCode = 0;
 
    memset(m_pMotorPos, 0, sizeof(SMarkPosition)*2);
    memset(m_pGlassPos, 0, sizeof(SMarkPosition)*2);
 
    m_sAlignResult.Reset();
}
 
CAlignCalibrator::~CAlignCalibrator(void)
{
 
}
 
void CAlignCalibrator::SetFirstMarkPos(double dMotorX, double dMotorY, double dGlassX, double dGlassY)
{
    m_pMotorPos[0].dPositionX = dMotorX;
    m_pMotorPos[0].dPositionY = dMotorY;
 
    m_pGlassPos[0].dPositionX = dGlassX;
    m_pGlassPos[0].dPositionY = dGlassY;
}
 
void CAlignCalibrator::SetSecondMarkPos(double dMotorX, double dMotorY, double dGlassX, double dGlassY)
{
    m_pMotorPos[1].dPositionX = dMotorX;
    m_pMotorPos[1].dPositionY = dMotorY;
 
    m_pGlassPos[1].dPositionX = dGlassX;
    m_pGlassPos[1].dPositionY = dGlassY;
}
 
/* A3 EA °Ë»ç±â
 
À§Ä¡    xÁÂÇ¥        yÁÂÇ¥
RT        3360.222    21.411
RB        3360.397    880.465
LT        1889.912    21.356
LB        1890.079    880.406
 
*/
 
BOOL CAlignCalibrator::CalculateAlignResult()
{
    m_nResultCode = 0;
 
    m_sAlignResult.Reset();
 
    double dx = fabs(m_pMotorPos[0].dPositionX - m_pMotorPos[1].dPositionX);
    double dy = fabs(m_pMotorPos[0].dPositionY - m_pMotorPos[1].dPositionY);
 
    if (dx > dy)
    {
        m_nResultCode = CalculateAxisX();
    }
    else
    {
        m_nResultCode = CalculateAxisY();
    }
 
    return (BOOL)m_nResultCode;
}
 
double EuclideanDistance(const SMarkPosition& pt1, const SMarkPosition& pt2)
{
    double dDistance = (pt1.dPositionX-pt2.dPositionX) * (pt1.dPositionX-pt2.dPositionX);
 
    dDistance += (pt1.dPositionY-pt2.dPositionY) * (pt1.dPositionY-pt2.dPositionY);
    
    return sqrt(dDistance);
}
 
BOOL CAlignCalibrator::CalculateAxisX()
{
    double dx1 = m_pMotorPos[0].dPositionX - m_pMotorPos[1].dPositionX;
    double dy1 = m_pMotorPos[0].dPositionY - m_pMotorPos[1].dPositionY;
    double dx2 = m_pMotorPos[1].dPositionX - m_pMotorPos[0].dPositionX;
    double dy2 = m_pMotorPos[1].dPositionY - m_pMotorPos[0].dPositionY;
 
    // È¸Àü°¢ °è»ê
//    if(m_pGlassPos[0].dPositionX < m_pMotorPos[1].dPositionX)
        m_sAlignResult.dRadian = atan( dy1 / dx1 );
//    else
//        m_sAlignResult.dRadian = -atan( dy1 / dx1 );
    m_sAlignResult.dDegree = ToDegree(m_sAlignResult.dRadian);
    m_sAlignResult.dSinValue = sin(m_sAlignResult.dRadian);
    m_sAlignResult.dCosValue = cos(m_sAlignResult.dRadian);
 
    // ±âÁØ ¾ó¶óÀÎ ¸¶Å©ÀÇ ¸ðÅÍ À§Ä¡¸¦ È¸ÀüÁß½ÉÀ¸·Î Á¤/¿ª È¸Àüº¯È¯ Çà·Ä °è»ê
    m_sAlignResult.SetRotateMatrix(m_pMotorPos[0].dPositionX, m_pMotorPos[0].dPositionY);
 
    // ±âÁØ ¾ó¶óÀÎ ¸¶Å©ÀÇ ¸ðÅÍ À§Ä¡¿Í ±Û¶ó½º À§Ä¡·Î ±Û¶ó½º ¿øÁ¡ÀÇ ¸ðÅÍ À§Ä¡ ¼³Á¤
    m_sAlignResult.dOriginX = m_pMotorPos[0].dPositionX - (m_pGlassPos[0].dPositionX * m_dDirectionX);
    m_sAlignResult.dOriginY = m_pMotorPos[0].dPositionY - (m_pGlassPos[0].dPositionY * m_dDirectionY);
 
    // ¸¶Å©°£ °Å¸® °è»ê
    double dMotorDist = EuclideanDistance(m_pMotorPos[0], m_pMotorPos[1]);
    double dGalssDist = EuclideanDistance(m_pGlassPos[0], m_pGlassPos[1]);
    double dDistThres = 2.0 * fabs(dMotorDist-dGalssDist);
 
    double tx, ty;
    double sx, sy;
 
    // ¾ó¶óÀÎ ¸¶Å© ¼³°èÀ§Ä¡¸¦ Á¤¹æÇâ È¸ÀüÇßÀ»¶§ ¸ðÅÍÀ§Ä¡¿ÍÀÇ Â÷ÀÌ°ª °è»ê
    tx = ty = 0.0;
    for (int i=0; i<2; i++)
    {
        TransformGlass2MotorTest(m_pGlassPos[i].dPositionX, m_pGlassPos[i].dPositionY, sx, sy);
        tx += fabs(m_pMotorPos[i].dPositionX - sx);
        ty += fabs(m_pMotorPos[i].dPositionY - sy);
    }
 
    if (tx>dDistThres || ty>dDistThres)
    {
        return FALSE;
    }
 
    // ¾ó¶óÀÎ ¸¶Å© ¸ðÅÍÀ§Ä¡¸¦ ¿ª¹æÇâ È¸ÀüÇßÀ»¶§ ¼³°èÀ§Ä¡¿ÍÀÇ Â÷ÀÌ°ª °è»ê
    tx = ty = 0.0;
    for (int i=0; i<2; i++)
    {
        TransformMotor2GlassTest(m_pMotorPos[i].dPositionX, m_pMotorPos[i].dPositionY, sx, sy);
        tx += fabs(m_pGlassPos[i].dPositionX - sx);
        ty += fabs(m_pGlassPos[i].dPositionY - sy);
    }
 
    if (tx>dDistThres || ty>dDistThres)
    {
        return FALSE;
    }
 
    return TRUE;
}
 
BOOL CAlignCalibrator::CalculateAxisY()
{
    double dx1 = m_pMotorPos[0].dPositionX - m_pMotorPos[1].dPositionX;
    double dy1 = m_pMotorPos[0].dPositionY - m_pMotorPos[1].dPositionY;
    double dx2 = m_pMotorPos[1].dPositionX - m_pMotorPos[0].dPositionX;
    double dy2 = m_pMotorPos[1].dPositionY - m_pMotorPos[0].dPositionY;
 
    // È¸Àü°¢ °è»ê
    m_sAlignResult.dRadian = -1.0 * atan( dx1 / dy1 );
    m_sAlignResult.dDegree = ToDegree(m_sAlignResult.dRadian);
    m_sAlignResult.dSinValue = sin(m_sAlignResult.dRadian);
    m_sAlignResult.dCosValue = cos(m_sAlignResult.dRadian);
 
    // ±âÁØ ¾ó¶óÀÎ ¸¶Å©ÀÇ ¸ðÅÍ À§Ä¡¸¦ È¸ÀüÁß½ÉÀ¸·Î Á¤/¿ª È¸Àüº¯È¯ Çà·Ä °è»ê
    m_sAlignResult.SetRotateMatrix(m_pMotorPos[0].dPositionX, m_pMotorPos[0].dPositionY);
 
    // ±âÁØ ¾ó¶óÀÎ ¸¶Å©ÀÇ ¸ðÅÍ À§Ä¡¿Í ±Û¶ó½º À§Ä¡·Î ±Û¶ó½º ¿øÁ¡ÀÇ ¸ðÅÍ À§Ä¡ ¼³Á¤
    m_sAlignResult.dOriginX = m_pMotorPos[0].dPositionX - (m_pGlassPos[0].dPositionX * m_dDirectionX);
    m_sAlignResult.dOriginY = m_pMotorPos[0].dPositionY - (m_pGlassPos[0].dPositionY * m_dDirectionY);
 
    // ¸¶Å©°£ °Å¸® °è»ê
    double dMotorDist = EuclideanDistance(m_pMotorPos[0], m_pMotorPos[1]);
    double dGalssDist = EuclideanDistance(m_pGlassPos[0], m_pGlassPos[1]);
    double dDistThres = 2.0 * fabs(dMotorDist-dGalssDist);
 
    double tx, ty;
    double sx, sy;
 
    tx = ty = 0.0;
 
    sx = sy = 0.0;
    for (int i=0; i<2; i++)
    {
        TransformGlass2MotorTest(m_pGlassPos[i].dPositionX, m_pGlassPos[i].dPositionY, sx, sy);
        tx += fabs(m_pMotorPos[i].dPositionX - sx);
        ty += fabs(m_pMotorPos[i].dPositionY - sy);
    }
 
    if (tx>dDistThres || ty>dDistThres)
    {
        return FALSE;
    }
 
    tx = ty = 0.0;
    sx = sy = 0.0;
 
    for (int i=0; i<2; i++)
    {
        TransformMotor2GlassTest(m_pMotorPos[i].dPositionX, m_pMotorPos[i].dPositionY, sx, sy);
        tx += fabs(m_pGlassPos[i].dPositionX - sx);
        ty += fabs(m_pGlassPos[i].dPositionY - sy);
    }
 
    if (tx>dDistThres || ty>dDistThres)
    {
        return FALSE;
    }
 
    return TRUE;
}
 
BOOL CAlignCalibrator::TransformGlass2MotorTest(double dGlassX, double dGlassY, double& dMotorX, double& dMotorY)
{
    // ±Û¶ó½º ÁÂÇ¥¸¦ ¸ðÅÍÁÂÇ¥·Î º¯È¯(¿øÁ¡±âÁØ)
    double motor_x = (dGlassX * m_dDirectionX) + m_sAlignResult.dOriginX;
    double motor_y = (dGlassY * m_dDirectionY) + m_sAlignResult.dOriginY;
 
    // ¸ðÅÍÁÂÇ¥¸¦ radian ¹æÇâÀ¸·Î È¸ÀüÇÑ ¸ðÅÍÁÂÇ¥¸¦ °è»ê
    m_sAlignResult.RotatePos(motor_x, motor_y);
    dMotorX = motor_x;
    dMotorY = motor_y;
 
    return TRUE;
}
 
BOOL CAlignCalibrator::TransformGlass2Motor(double dGlassX, double dGlassY, double& dMotorX, double& dMotorY) const
{
    // ±Û¶ó½º ÁÂÇ¥¸¦ ¸ðÅÍÁÂÇ¥·Î º¯È¯(¿øÁ¡±âÁØ)
    double motor_x = (dGlassX * m_dDirectionX) + m_sAlignResult.dOriginX;
    double motor_y = (dGlassY * m_dDirectionY) + m_sAlignResult.dOriginY;
 
    // ¸ðÅÍÁÂÇ¥¸¦ radian ¹æÇâÀ¸·Î È¸ÀüÇÑ ¸ðÅÍÁÂÇ¥¸¦ °è»ê
    if (m_nResultCode)
    {
        m_sAlignResult.RotatePos(motor_x, motor_y);
    }
 
    dMotorX = motor_x;
    dMotorY = motor_y;
 
    return TRUE;
}
 
BOOL CAlignCalibrator::TransformMotor2GlassTest(double dMotorX, double dMotorY, double& dGlassX, double& dGlassY)
{
    double motor_x = dMotorX;
    double motor_y = dMotorY;
 
    // ¸ðÅÍÁÂÇ¥¸¦ -radian ¹æÇâÀ¸·Î È¸ÀüÇÑ ¸ðÅÍÁÂÇ¥ °è»ê
    m_sAlignResult.InvRotatePos(motor_x, motor_y);
 
    // È¸ÀüÇÑ ¸ðÅÍÁÂÇ¥¸¦ ±Û¶ó½º ÁÂÇ¥·Î º¯È¯(¿øÁ¡±âÁØ)
    dGlassX = (motor_x - m_sAlignResult.dOriginX) * m_dDirectionX;
    dGlassY = (motor_y - m_sAlignResult.dOriginY) * m_dDirectionY;
 
    return TRUE;
}
 
BOOL CAlignCalibrator::TransformMotor2Glass(double dMotorX, double dMotorY, double& dGlassX, double& dGlassY) const
{
    double motor_x = dMotorX;
    double motor_y = dMotorY;
 
    // ¸ðÅÍÁÂÇ¥¸¦ -radian ¹æÇâÀ¸·Î È¸ÀüÇÑ ¸ðÅÍÁÂÇ¥ °è»ê
    if (m_nResultCode)
    {
        m_sAlignResult.InvRotatePos(motor_x, motor_y);
    }
 
    // È¸ÀüÇÑ ¸ðÅÍÁÂÇ¥¸¦ ±Û¶ó½º ÁÂÇ¥·Î º¯È¯
    dGlassX = (motor_x - m_sAlignResult.dOriginX) * m_dDirectionX;
    dGlassY = (motor_y - m_sAlignResult.dOriginY) * m_dDirectionY;
 
    return TRUE;
}