#pragma once
|
|
#include <vector>
|
#include "SPosition2D.h"
|
|
inline double DOT(const SPosition2D& pt1, const SPosition2D& pt2)
|
{
|
return (pt1.dPositionX*pt2.dPositionX)+(pt1.dPositionY*pt2.dPositionY);
|
}
|
|
struct SCalibrateResult
|
{
|
SCalibrateResult()
|
{
|
Reset();
|
}
|
|
void Reset()
|
{
|
sStartPosition.Reset();
|
sEndPosition.Reset();
|
vectorRegion.clear();
|
memset(dCoefficient, 9, sizeof(double)*9);
|
}
|
|
bool InRect(double dPosX, double dPosY)
|
{
|
if (dPosX<sStartPosition.dPositionX || dPosX>sEndPosition.dPositionX) return false;
|
if (dPosY<sStartPosition.dPositionY || dPosY>sEndPosition.dPositionY) return false;
|
return true;
|
}
|
|
bool is_inside_polygon2D(double x, double y)
|
{
|
int i, j;
|
bool c = false;
|
|
int N = (int) vectorRegion.size();
|
|
for(i=0, j=N-1; i<N; j=i++)
|
{
|
//xp[i] = vectorRegion[i].dPositionX;
|
//yp[i] = vectorRegion[i].dPositionY;
|
|
if ( (((vectorRegion[i].dPositionY<=y) && (y<vectorRegion[j].dPositionY)) || ((vectorRegion[j].dPositionY<=y) && (y<vectorRegion[i].dPositionY))) //(1)&&(2)&&(3)
|
&&
|
(x < (vectorRegion[j].dPositionX - vectorRegion[i].dPositionX) * (y - vectorRegion[i].dPositionY) / (vectorRegion[j].dPositionY - vectorRegion[i].dPositionY) + vectorRegion[i].dPositionX) //(4)
|
)
|
{
|
c =!c ;
|
}
|
}
|
|
return (c) ;
|
}
|
|
|
/* third source */
|
|
//Á¡ P°¡ (V2-V1)ÀÇ ¹æÇ⺤Å͸¦ °®´Â Á÷¼±ÀÇ ¿ÞÂÊÀÎÁö, Á÷¼±»óÀÎÁö, ¾Æ´Ï¸é
|
|
//¿À¸¥ÂÊÀÎÁö¸¦ ¾Ë·ÁÁÖ´Â ÁöÇ¥ÀÓ
|
|
// >0 : ¿ÞÂÊ¿¡ ³õ¿©ÀÖÀ½;
|
|
// =0 : Á÷¼±»ó¿¡ ³õ¿©ÀÖÀ½;
|
|
// <0 : ¿À¸¥ÂÊ¿¡ ³õ¿©ÀÖÀ½;
|
|
#define LEFTTEST(V1,V2,x,y) ((V2.dPositionX-V1.dPositionX)*(y-V1.dPositionY)-(V2.dPositionY-V1.dPositionY)*(x-V1.dPositionX))
|
|
int find_winding_number_polygon2D(double dPosX, double dPosY)
|
{
|
int N = (int)vectorRegion.size();
|
int n = (int)vectorRegion.size();
|
|
for (int i=0; i<n; i++)
|
{
|
if (vectorRegion[i].dPositionX==dPosX && vectorRegion[i].dPositionY==dPosY)
|
{
|
return 1;
|
}
|
}
|
|
int wn = 0;
|
// loop through all edges of the polygon
|
for (int i=0; i<n; i++)
|
{
|
//edge from V[i] to V[i+1];
|
int j=(i+1)%N ;
|
|
if (vectorRegion[i].dPositionY <= dPosY)
|
{ // start y <= dPosY
|
if(vectorRegion[j].dPositionY > dPosY) // an upward crossing
|
if(LEFTTEST(vectorRegion[i], vectorRegion[j], dPosX, dPosY)>0)//P left of edge
|
++wn; // have a valid up intersect
|
}
|
else
|
{ // start y>dPosY (no test needed)
|
if(vectorRegion[j].dPositionY <= dPosY) //a downward crossing
|
if(LEFTTEST(vectorRegion[i], vectorRegion[j], dPosX, dPosY)<0)//P right of edge
|
--wn; // have a valid down intersect
|
}
|
}
|
|
//wn=0;P is outside;
|
|
//wn=1;P is inside;
|
|
//wn=2;P is in the non-simple region;
|
return wn;
|
}
|
|
|
|
|
bool InRegion(double dPosX, double dPosY)
|
{
|
bool inside = false;
|
|
SPosition2D newPoint, oldPoint;
|
SPosition2D left, right;
|
|
int nCount = (int)vectorRegion.size();
|
|
oldPoint = vectorRegion[nCount-1];
|
|
for (int i=0; i<nCount; i++)
|
{
|
newPoint = vectorRegion[i];
|
|
if (newPoint.dPositionX > oldPoint.dPositionX)
|
{
|
left = oldPoint;
|
right = newPoint;
|
}
|
else
|
{
|
left = newPoint;
|
right = oldPoint;
|
}
|
|
if ( (newPoint.dPositionX<dPosX)==(dPosX<oldPoint.dPositionX) &&
|
(dPosY-left.dPositionY)*(right.dPositionX-left.dPositionX) <
|
(right.dPositionY-left.dPositionY)*(dPosX-left.dPositionX) )
|
{
|
inside = !inside;
|
}
|
|
oldPoint = newPoint;
|
}
|
|
return inside;
|
}
|
|
double dCoefficient[9];
|
VectorPosition2D vectorRegion;
|
SPosition2D sStartPosition;
|
SPosition2D sEndPosition;
|
};
|
typedef std::vector<SCalibrateResult> VectorCalibrateResult;
|
typedef std::vector<SCalibrateResult>::iterator VectorCalibrateResultIt;
|
typedef std::vector<SCalibrateResult>::const_iterator constVectorCalibrateResultIt;
|
|
typedef std::vector<double> VectorDouble;
|
typedef std::vector<double>::iterator VectorDoubleIt;
|
typedef std::vector<double>::const_iterator constVectorDoubleIt;
|
|
class AFX_EXT_CLASS CMotorCalibrator
|
{
|
public:
|
CMotorCalibrator(int nIndex=-1);
|
virtual ~CMotorCalibrator(void);
|
void Reset();
|
|
int GetIndex() { return m_nIndex; }
|
|
BOOL LoadDesignPosition(const CString& strFilename);
|
BOOL SaveDesignPosition(const CString& strFilename);
|
|
BOOL LoadMotorPosition(const CString& strFilename);
|
BOOL SaveMotorPosition(const CString& strFilename);
|
|
BOOL LoadCalibrateResultF(const CString& strFilename);
|
BOOL SaveCalibrateResultF(const CString& strFilename);
|
|
BOOL LoadCalibrateResultB(const CString& strFilename);
|
BOOL SaveCalibrateResultB(const CString& strFilename);
|
|
void SetDesignPosition(const VectorDouble& vectorRows, const VectorDouble& vectorCols);
|
void SetDesignPosition(const VectorPosition2D& vectorDesignPos) { m_vectorDesignPos = vectorDesignPos; }
|
void SetMotorPosition(const VectorPosition2D& vectorMotorPos) { m_vectorMotorPos = vectorMotorPos; }
|
const VectorPosition2D* GetDesignPosition() { return &m_vectorDesignPos; }
|
const VectorPosition2D* GetMotorPosition() { return &m_vectorMotorPos; }
|
void SetMotorPosition(int nRow, int nCol, const SPosition2D& sMotorPos);
|
void SetMotorPosition(int nIndex, const SPosition2D& sMotorPos);
|
|
int CalculateCoefficient();
|
BOOL FCalibratePosition(double& dPosX, double& dPosY);
|
BOOL BCalibratePosition(double& dPosX, double& dPosY);
|
|
static int CalculatePerspectiveTransform(const VectorPosition2D& vectorSrc, const VectorPosition2D& vectorDst, SCalibrateResult& result);
|
|
static BOOL LoadPosition(const CString& strFilename, VectorPosition2D& vectorPos, int& nRows, int& nCols);
|
static BOOL SavePosition(const CString& strFilename, const VectorPosition2D& vectorPos, int nRows, int nCols);
|
|
static BOOL LoadCalibrateResult(const CString& strFilename, VectorCalibrateResult& vectorPos);
|
static BOOL SaveCalibrateResult(const CString& strFilename, const VectorCalibrateResult& vectorPos);
|
|
protected:
|
int m_nIndex;
|
int m_nRows;
|
int m_nCols;
|
VectorPosition2D m_vectorDesignPos;
|
VectorPosition2D m_vectorMotorPos;
|
VectorCalibrateResult m_vectorCaliResultF;
|
VectorCalibrateResult m_vectorCaliResultB;
|
};
|
