// 2016.11.25 static
public static void ConvertFrameInverse(ASDef._stRobotShift stFrame, ASDef._stXYZ stSource, ref ASDef._stXYZ stDest, _emRotationOrder emRotationOrder)
{
// 2016.07.07 by khWoo
// **********************************************************************
// [ stDest.dX ] [ stSource.dX ] [ stFrame.dX ]
// | stDest.dY | = RevM ( | stSource.dY | - | stFrame.dY | )
// [ stDest.dZ ] [ stSource.dZ ] [ stFrame.dZ ]
// **********************************************************************
double[,] RevM = new double[3, 3];
double RX = stFrame.dAX * Math.PI / 180;
double RY = stFrame.dAY * Math.PI / 180;
double RZ = stFrame.dAZ * Math.PI / 180;
double dX = stSource.dX - stFrame.dX;
double dY = stSource.dY - stFrame.dY;
double dZ = stSource.dZ - stFrame.dZ;
switch (emRotationOrder)
{
case _emRotationOrder.ZYX:
RevM[0, 0] = Math.Cos(RZ) * Math.Cos(RY);
RevM[0, 1] = Math.Sin(RZ) * Math.Cos(RY);
RevM[0, 2] = -Math.Sin(RY);
RevM[1, 0] = -Math.Sin(RZ) * Math.Cos(RX) + Math.Sin(RX) * Math.Cos(RZ) * Math.Sin(RY);
RevM[1, 1] = Math.Cos(RX) * Math.Cos(RZ) + Math.Sin(RX) * Math.Sin(RY) * Math.Sin(RZ);
RevM[1, 2] = Math.Sin(RX) * Math.Cos(RY);
RevM[2, 0] = Math.Sin(RX) * Math.Sin(RZ) + Math.Cos(RX) * Math.Cos(RZ) * Math.Sin(RY);
RevM[2, 1] = -Math.Sin(RX) * Math.Cos(RZ) + Math.Sin(RY) * Math.Sin(RZ) * Math.Cos(RX);
RevM[2, 2] = Math.Cos(RX) * Math.Cos(RY);
break;
case _emRotationOrder.ZYZ:
RevM[0, 0] = Math.Cos(RZ) * Math.Cos(RX) * Math.Cos(RY) - Math.Sin(RZ) * Math.Sin(RX);
RevM[0, 1] = Math.Cos(RZ) * Math.Sin(RX) * Math.Cos(RY) + Math.Sin(RZ) * Math.Cos(RX);
RevM[0, 2] = -Math.Sin(RY) * Math.Cos(RZ);
RevM[1, 0] = -Math.Sin(RZ) * Math.Cos(RX) * Math.Cos(RY) - Math.Cos(RZ) * Math.Sin(RX);
RevM[1, 1] = -Math.Sin(RZ) * Math.Sin(RX) * Math.Cos(RY) + Math.Cos(RZ) * Math.Cos(RX);
RevM[1, 2] = Math.Sin(RZ) * Math.Sin(RY);
RevM[2, 0] = Math.Cos(RX) * Math.Sin(RY);
RevM[2, 1] = Math.Sin(RX) * Math.Sin(RY);
RevM[2, 2] = Math.Cos(RY);
break;
}
stDest.dX = RevM[0, 0] * dX + RevM[0, 1] * dY + RevM[0, 2] * dZ;
stDest.dY = RevM[1, 0] * dX + RevM[1, 1] * dY + RevM[1, 2] * dZ;
stDest.dZ = RevM[2, 0] * dX + RevM[2, 1] * dY + RevM[2, 2] * dZ;
}
// 2016.11.25 static
public static void ConvertFrame(ASDef._stRobotShift stFrame, ASDef._stXYZ stSource, ref ASDef._stXYZ stDest, _emRotationOrder emRotationOrder)
{
// 2016.07.07 by khWoo
// *****************************************************
// [ stDest.dX ] [ stSource.dX ] [ stFrame.dX ]
// | stDest.dY | = M | stSource.dY | + | stFrame.dY |
// [ stDest.dZ ] [ stSource.dZ ] [ stFrame.dZ ]
// *****************************************************
double[,] M = new double[3, 3];
double RX = stFrame.dAX * Math.PI / 180;
double RY = stFrame.dAY * Math.PI / 180;
double RZ = stFrame.dAZ * Math.PI / 180;
switch (emRotationOrder)
{
case _emRotationOrder.ZYX:
M[0, 0] = Math.Cos(RZ) * Math.Cos(RY);
M[0, 1] = -Math.Sin(RZ) * Math.Cos(RX) + Math.Sin(RX) * Math.Cos(RZ) * Math.Sin(RY);
M[0, 2] = Math.Sin(RX) * Math.Sin(RZ) + Math.Cos(RX) * Math.Cos(RZ) * Math.Sin(RY);
M[1, 0] = Math.Sin(RZ) * Math.Cos(RY);
M[1, 1] = Math.Cos(RX) * Math.Cos(RZ) + Math.Sin(RX) * Math.Sin(RY) * Math.Sin(RZ);
M[1, 2] = -Math.Sin(RX) * Math.Cos(RZ) + Math.Sin(RY) * Math.Sin(RZ) * Math.Cos(RX);
M[2, 0] = -Math.Sin(RY);
M[2, 1] = Math.Sin(RX) * Math.Cos(RY);
M[2, 2] = Math.Cos(RX) * Math.Cos(RY);
break;
case _emRotationOrder.ZYZ:
M[0, 0] = Math.Cos(RZ) * Math.Cos(RX) * Math.Cos(RY) - Math.Sin(RZ) * Math.Sin(RX);
M[0, 1] = -Math.Sin(RZ) * Math.Cos(RX) * Math.Cos(RY) - Math.Cos(RZ) * Math.Sin(RX);
M[0, 2] = Math.Cos(RX) * Math.Sin(RY);
M[1, 0] = Math.Cos(RZ) * Math.Sin(RX) * Math.Cos(RY) + Math.Sin(RZ) * Math.Cos(RX);
M[1, 1] = -Math.Sin(RZ) * Math.Sin(RX) * Math.Cos(RY) + Math.Cos(RZ) * Math.Cos(RX);
M[1, 2] = Math.Sin(RX) * Math.Sin(RY);
M[2, 0] = -Math.Sin(RY) * Math.Cos(RZ);
M[2, 1] = Math.Sin(RZ) * Math.Sin(RY);
M[2, 2] = Math.Cos(RY);
break;
}
stDest.dX = M[0, 0] * stSource.dX + M[0, 1] * stSource.dY + M[0, 2] * stSource.dZ + stFrame.dX;
stDest.dY = M[1, 0] * stSource.dX + M[1, 1] * stSource.dY + M[1, 2] * stSource.dZ + stFrame.dY;
stDest.dZ = M[2, 0] * stSource.dX + M[2, 1] * stSource.dY + M[2, 2] * stSource.dZ + stFrame.dZ;
}
//--------------------------
/*
* 3점으로 좌표계 생성
* pstP[0] : ORG
* pstP[1] : XX
* pstP[2] : XY
*
* 주의 : Y축 +-90도 이상 , X축 +-90도 이상 계산 못함
*
*
*/
public static void MakeFrame(ASDef._stXYZ[] pstP, ref ASDef._stRobotShift stFrame)
{
ASDef._stXYZ stPa, stPb, stPn;
/*
* |n
* |
* |
* |
* | b
* / ---------------
* /
* /
* /
* a
*/
stPa = default(ASDef._stXYZ);
stPb = default(ASDef._stXYZ);
stPn = default(ASDef._stXYZ);
SubXYZ(pstP[1], pstP[0], ref stPa);
SubXYZ(pstP[2], pstP[0], ref stPb);
MulXYZ(stPa, stPb, ref stPn); // stPa X stPb
// 2016.07.07 by khWoo
//MulXYZ(stPn, stPa, ref stPb); // (stPa X stPb) X stPa
stFrame.dX = pstP[0].dX;
stFrame.dY = pstP[0].dY;
stFrame.dZ = pstP[0].dZ;
// 2016.07.21 by khWoo
// Vector Normalize
double dTmp = Math.Sqrt(Math.Pow(stPa.dX, 2) + Math.Pow(stPa.dY, 2) + Math.Pow(stPa.dZ, 2));
stPa.dX /= dTmp;
stPa.dY /= dTmp;
stPa.dZ /= dTmp;
dTmp = Math.Sqrt(Math.Pow(stPn.dX, 2) + Math.Pow(stPn.dY, 2) + Math.Pow(stPn.dZ, 2));
stPn.dX /= dTmp;
stPn.dY /= dTmp;
stPn.dZ /= dTmp;
/*
* // 2011.08.09
* if (stPn.dY == 0)
* stFrame.dAX = Math.Acos(stPn.dZ / Math.Sqrt(Math.Pow(stPn.dZ, 2) + Math.Pow(stPn.dX, 2)));
* else
* stFrame.dAX = (stPn.dY / Math.Abs(stPn.dY)) * Math.Acos(stPn.dZ / Math.Sqrt(Math.Pow(stPn.dZ, 2) + Math.Pow(stPn.dX, 2)));
*
* if (stPn.dX == 0)
* stFrame.dAY = Math.Acos(stPn.dZ / Math.Sqrt(Math.Pow(stPn.dZ, 2) + Math.Pow(stPn.dY, 2)));
* else
* stFrame.dAY = (stPn.dX / Math.Abs(stPn.dX)) * Math.Acos(stPn.dZ / Math.Sqrt(Math.Pow(stPn.dZ, 2) + Math.Pow(stPn.dY, 2)));
*
* if (stPa.dY == 0)
* stFrame.dAZ = Math.Acos(stPa.dX / Math.Sqrt(Math.Pow(stPa.dX, 2) + Math.Pow(stPa.dY, 2)));
* else
* stFrame.dAZ = (stPa.dY / Math.Abs(stPa.dY)) * Math.Acos(stPa.dX / Math.Sqrt(Math.Pow(stPa.dX, 2) + Math.Pow(stPa.dY, 2)));
* stFrame.dAX *= 180 / Math.PI;
* stFrame.dAY *= 180 / Math.PI;
* stFrame.dAZ *= 180 / Math.PI;
*/
// 2011.08.11
/*
* if (stPa.dY == 0)
* stFrame.dAZ = Math.Acos(stPa.dX / Math.Sqrt(Math.Pow(stPa.dX, 2) + Math.Pow(stPa.dY, 2)));
* else
* stFrame.dAZ = (stPa.dY / Math.Abs(stPa.dY)) * Math.Acos(stPa.dX / Math.Sqrt(Math.Pow(stPa.dX, 2) + Math.Pow(stPa.dY, 2)));
*/
/*// 2011.08.15 Z축 회전 구하는 루틴 수정
* if (stPa.dX >= 0)
* {
* if (stPa.dX == 0 && stPa.dY == 0)
* stFrame.dAZ = 0;
* else
* stFrame.dAZ = Math.Asin(stPa.dY / Math.Sqrt(Math.Pow(stPa.dX, 2) + Math.Pow(stPa.dY, 2)));
* }
* else
* {
* if (stPa.dY == 0)
* {
* stFrame.dAZ = Math.PI; // Math.Acos(stPa.dX / Math.Sqrt(Math.Pow(stPa.dX, 2) + Math.Pow(stPa.dY, 2)));
* }
* else
* {
* stFrame.dAZ = (stPa.dY / Math.Abs(stPa.dY)) * Math.Acos(stPa.dX / Math.Sqrt(Math.Pow(stPa.dX, 2) + Math.Pow(stPa.dY, 2)));
* }
* }
*
* stFrame.dAZ = stFrame.dAZ * 180 / Math.PI;
*
* ASDef._stXYZ stPnz, stPny;
* stPnz = default(ASDef._stXYZ);
* RotateZ(-stFrame.dAZ, stPn, ref stPnz);
*
* if (stPnz.dX == 0)
* stFrame.dAY = Math.Acos(stPnz.dZ / Math.Sqrt(Math.Pow(stPnz.dZ, 2) + Math.Pow(stPnz.dX, 2)));
* else
* stFrame.dAY = (stPnz.dX / Math.Abs(stPnz.dX)) * Math.Acos(stPnz.dZ / Math.Sqrt(Math.Pow(stPnz.dZ, 2) + Math.Pow(stPnz.dX, 2)));
* stFrame.dAY = stFrame.dAY * 180 / Math.PI;
*
* stPny = default(ASDef._stXYZ);
* RotateY(-stFrame.dAY, stPnz, ref stPny);
*
*
* if (stPny.dY == 0)
* stFrame.dAX = Math.Acos(stPny.dZ / Math.Sqrt(Math.Pow(stPny.dZ, 2) + Math.Pow(stPny.dY, 2)));
* else
* stFrame.dAX = -(stPny.dY / Math.Abs(stPny.dY)) * Math.Acos(stPny.dZ / Math.Sqrt(Math.Pow(stPny.dZ, 2) + Math.Pow(stPny.dY, 2)));
* stFrame.dAX = stFrame.dAX * 180 / Math.PI;
*/
// 2016.07.07 by khWoo
//------ z축 중심으로 회전
if (stPa.dX >= 0) // (stPa.dX, stPa.dY) <-- x축이 기준축
{
if (stPa.dX == 0 && stPa.dY == 0) // origin
{
stFrame.dAZ = 0;
}
else // -90 ~ +90 (1사분면, 4사분면)
{
stFrame.dAZ = Math.Asin(stPa.dY / Math.Sqrt(Math.Pow(stPa.dX, 2) + Math.Pow(stPa.dY, 2)));
}
}
else
{
if (stPa.dY == 0) // -x축 위
{
stFrame.dAZ = Math.PI; // Math.Acos(stPa.dX / Math.Sqrt(Math.Pow(stPa.dX, 2) + Math.Pow(stPa.dY, 2)));
}
else // 2사분면, 3사분면
{
stFrame.dAZ = (stPa.dY / Math.Abs(stPa.dY)) * Math.Acos(stPa.dX / Math.Sqrt(Math.Pow(stPa.dX, 2) + Math.Pow(stPa.dY, 2)));
}
}
stFrame.dAZ = stFrame.dAZ * 180 / Math.PI;
/*
* ASDef._stXYZ stPnz, stPbz; //, stPaz;
* stPnz = default(ASDef._stXYZ);
* //stPaz = default(ASDef._stXYZ);
* stPbz = default(ASDef._stXYZ);
* RotateZ(-stFrame.dAZ, stPn, ref stPnz);
* //RotateZ(-stFrame.dAZ, stPa, ref stPaz);
* RotateZ(-stFrame.dAZ, stPb, ref stPbz); // stPa, stPb, stPn을 dAZ만큼 z축 중심으로 회전
*/
ASDef._stXYZ stPnz, stPaz;
stPnz = default(ASDef._stXYZ);
stPaz = default(ASDef._stXYZ);
RotateZ(-stFrame.dAZ, stPn, ref stPnz); // stPn을 -dAZ만큼 z축 중심으로 회전
RotateZ(-stFrame.dAZ, stPa, ref stPaz); // stPa을 -dAZ만큼 z축 중심으로 회전
// 2016.07.21 bby khWoo
//------ y축 중심으로 회전
if (stPaz.dX >= 0) // (stPaz.dX, stPaz.dZ) <-- x축이 기준축
{
if (stPaz.dZ == 0 && stPaz.dX == 0) // origin
{
stFrame.dAY = 0;
}
else // -90 ~ +90 (1사분면, 4사분면)
{
stFrame.dAY = Math.Asin(stPaz.dZ / Math.Sqrt(Math.Pow(stPaz.dX, 2) + Math.Pow(stPaz.dZ, 2)));
}
}
else
{
if (stPaz.dZ == 0) // -x축 위
{
stFrame.dAY = Math.PI;
}
else
{
stFrame.dAY = (stPaz.dZ / Math.Abs(stPaz.dZ)) * Math.Acos(stPaz.dX / Math.Sqrt(Math.Pow(stPaz.dX, 2) + Math.Pow(stPaz.dZ, 2)));
}
}
stFrame.dAY = -stFrame.dAY * 180 / Math.PI;
ASDef._stXYZ stPny;
stPny = default(ASDef._stXYZ);
RotateY(-stFrame.dAY, stPnz, ref stPny); // stPnz을 dAY만큼 y축 중심으로 회전
// 2016.07.21 bby khWoo
//------ x축 중심으로 회전
if (stPny.dZ >= 0) // (stPby.dZ, stPby.dY) <-- z축이 기준축
{
if (stPny.dY == 0 && stPny.dZ == 0)
{
stFrame.dAX = 0; // origin
}
else // -90 ~ +90 (1사분면, 4사분면)
{
stFrame.dAX = Math.Asin(stPny.dY / Math.Sqrt(Math.Pow(stPny.dY, 2) + Math.Pow(stPny.dZ, 2)));
}
}
else
{
if (stPny.dY == 0) // -z축 위
{
stFrame.dAX = Math.PI;
}
else
{
stFrame.dAX = (stPny.dY / Math.Abs(stPny.dY)) * Math.Acos(stPny.dZ / Math.Sqrt(Math.Pow(stPny.dY, 2) + Math.Pow(stPny.dZ, 2)));
}
}
stFrame.dAX = -stFrame.dAX * 180 / Math.PI;
}
public void SetFrameCommon(ASDef._stRobotShift stFrame)
{
m_stFrameCommon = stFrame;
}
public void SetFramePose(ASDef._stRobotShift stFrame)
{
m_stFramePose = stFrame;
}
