public static void RotateX(double dRotate, ASDef._stXYZ stSource, ref ASDef._stXYZ stDest)
{
double[,] M = new double[4, 4];
double dA = dRotate * Math.PI / 180;
M[0, 0] = 1;
M[0, 1] = 0;
M[0, 2] = 0;
M[0, 3] = 0;
M[1, 0] = 0;
M[1, 1] = Math.Cos(dA);
M[1, 2] = -Math.Sin(dA);
M[1, 3] = 0;
M[2, 0] = 0;
M[2, 1] = Math.Sin(dA);
M[2, 2] = Math.Cos(dA);
M[2, 3] = 0;
M[3, 0] = 0;
M[3, 1] = 0;
M[3, 2] = 0;
M[3, 3] = 1;
stDest.dX = M[0, 0] * stSource.dX + M[0, 1] * stSource.dY + M[0, 2] * stSource.dZ + M[0, 3] * 1;
stDest.dY = M[1, 0] * stSource.dX + M[1, 1] * stSource.dY + M[1, 2] * stSource.dZ + M[1, 3] * 1;
stDest.dZ = M[2, 0] * stSource.dX + M[2, 1] * stSource.dY + M[2, 2] * stSource.dZ + M[2, 3] * 1;
}
/* 2016.06.17
* public void PoseToBase(ASDef._stXYZ stSource, ref ASDef._stXYZ stDest, _emRotationOrder emRotationOrder)
* {
* // **********************************************************
* // [ stDest.dX ] [ stSource.dX ] [ m_stFramePose.dX ]
* // | stDest.dY | = M | stSource.dY | + | m_stFramePose.dY |
* // [ stDest.dZ ] [ stSource.dZ ] [ m_stFramePose.dZ ]
* // **********************************************************
*
* double[,] M = new double[3,3];
* double RX = m_stFramePose.dAX * Math.PI / 180;
* double RY = m_stFramePose.dAY * Math.PI / 180;
* double RZ = m_stFramePose.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);
* // 2010.07.12
* 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 + m_stFramePose.dX;
* stDest.dY = M[1,0] * stSource.dX + M[1,1] * stSource.dY + M[1,2] * stSource.dZ + m_stFramePose.dY;
* stDest.dZ = M[2,0] * stSource.dX + M[2,1] * stSource.dY + M[2,2] * stSource.dZ + m_stFramePose.dZ;
* }
*/
public void BaseToCommon(ASDef._stXYZ stSource, ref ASDef._stXYZ stDest, _emRotationOrder emRotationOrder)
{
// 2016.07.07 by khWoo
// **********************************************************************
// [ stDest.dX ] [ stSource.dX ] [ m_stFrameCommon.dX ]
// | stDest.dY | = RevM ( | stSource.dY | - | m_stFrameCommon.dY | )
// [ stDest.dZ ] [ stSource.dZ ] [ m_stFrameCommon.dZ ]
//
// RevM = Transpose of M
// **********************************************************************
double[,] RevM = new double[3, 3];
double RX = m_stFrameCommon.dAX * Math.PI / 180;
double RY = m_stFrameCommon.dAY * Math.PI / 180;
double RZ = m_stFrameCommon.dAZ * Math.PI / 180;
double dX = stSource.dX - m_stFrameCommon.dX;
double dY = stSource.dY - m_stFrameCommon.dY;
double dZ = stSource.dZ - m_stFrameCommon.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 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;
}
// 다른 좌표계의 값을 기준 좌표계 값으로 변환
public static void FrameToBase(ASDef._stXYZ stSource, ref ASDef._stXYZ stDest, _emRotationOrder emRotationOrder, ASDef._stRobotShift stFrame)
{
// 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;
}
/*
* 기준 4포인트에서 이동후 1포인트의 이동값을 모르는 경우 찾는
* 3 --------------4
* | |
* | |
* | |
* | |
* 2---------------1
* 0 : 4번을 모르는 경우 2:Org 1:xx 3:xy
* 1 : 1번을 모르는 경우 3:Org 2:xx 4:xy
* 2 : 3번을 모르는 경우 1:Org 4:xx 2:xy
* 3 : 2번을 모르는 경우 4:Org 3:xx 1:xy
*/
public static void FindLost1Point(ASDef._stXYZ[] pstSource, ASDef._stXYZ[] pstMove, ref ASDef._stXYZ stLost1Point, int nCase)
{
ASDef._stRobotShift stFrame;
stFrame = default(ASDef._stRobotShift);
ASDef._stXYZ[] pst3Point = new ASDef._stXYZ[3];
ASDef._stXYZ st1Point;
st1Point = default(ASDef._stXYZ);
switch (nCase)
{
// 0 : 4번을 모르는 경우 2:Org 1:xx 3:xy
case 0:
{
pst3Point[0] = pstSource[1]; // ORG
pst3Point[1] = pstSource[0]; // XX
pst3Point[2] = pstSource[2]; // XY
MakeFrame(pst3Point, ref stFrame);
BaseToFrame(pstSource[3], ref st1Point, _emRotationOrder.ZYX, stFrame);
pst3Point[0] = pstMove[1]; // ORG
pst3Point[1] = pstMove[0]; // XX
pst3Point[2] = pstMove[2]; // XY
MakeFrame(pst3Point, ref stFrame);
FrameToBase(st1Point, ref stLost1Point, _emRotationOrder.ZYX, stFrame);
}
break;
// 2011.08.16 4<->2
// 1 : 1번을 모르는 경우 3:Org 2:xx 4:xy
case 1:
{
pst3Point[0] = pstSource[2]; // ORG
pst3Point[1] = pstSource[1]; // XX
pst3Point[2] = pstSource[3]; // XY
MakeFrame(pst3Point, ref stFrame);
BaseToFrame(pstSource[0], ref st1Point, _emRotationOrder.ZYX, stFrame);
pst3Point[0] = pstMove[2]; // ORG
pst3Point[1] = pstMove[1]; // XX
pst3Point[2] = pstMove[3]; // XY
MakeFrame(pst3Point, ref stFrame);
FrameToBase(st1Point, ref stLost1Point, _emRotationOrder.ZYX, stFrame);
}
break;
// 2011.08.16 4<->2
// 2 : 3번을 모르는 경우 1:Org 4:xx 2:xy
case 2:
{
pst3Point[0] = pstSource[0]; // ORG
pst3Point[1] = pstSource[3]; // XX
pst3Point[2] = pstSource[1]; // XY
MakeFrame(pst3Point, ref stFrame);
BaseToFrame(pstSource[2], ref st1Point, _emRotationOrder.ZYX, stFrame);
pst3Point[0] = pstMove[0]; // ORG
pst3Point[1] = pstMove[3]; // XX
pst3Point[2] = pstMove[1]; // XY
MakeFrame(pst3Point, ref stFrame);
FrameToBase(st1Point, ref stLost1Point, _emRotationOrder.ZYX, stFrame);
}
break;
// 3 : 2번을 모르는 경우 4:Org 3:xx 1:xy
case 3:
{
pst3Point[0] = pstSource[3]; // ORG
pst3Point[1] = pstSource[2]; // XX
pst3Point[2] = pstSource[0]; // XY
MakeFrame(pst3Point, ref stFrame);
BaseToFrame(pstSource[1], ref st1Point, _emRotationOrder.ZYX, stFrame);
pst3Point[0] = pstMove[3]; // ORG
pst3Point[1] = pstMove[2]; // XX
pst3Point[2] = pstMove[0]; // XY
MakeFrame(pst3Point, ref stFrame);
FrameToBase(st1Point, ref stLost1Point, _emRotationOrder.ZYX, stFrame);
}
break;
}
}
public static double LengthXYZ(ASDef._stXYZ stSource)
{
return(Math.Sqrt(Math.Pow(stSource.dX, 2) + Math.Pow(stSource.dY, 2) + Math.Pow(stSource.dZ, 2)));
}
// 외적
public static void MulXYZ(ASDef._stXYZ stSource1, ASDef._stXYZ stSource2, ref ASDef._stXYZ stDest)
{
stDest.dX = stSource1.dY * stSource2.dZ - stSource1.dZ * stSource2.dY;
stDest.dY = stSource1.dZ * stSource2.dX - stSource1.dX * stSource2.dZ;
stDest.dZ = stSource1.dX * stSource2.dY - stSource1.dY * stSource2.dX;
}
//--------------------------
// 2011.08.14
public static void SubXYZ(ASDef._stXYZ stSource1, ASDef._stXYZ stSource2, ref ASDef._stXYZ stDest)
{
stDest.dX = stSource1.dX - stSource2.dX;
stDest.dY = stSource1.dY - stSource2.dY;
stDest.dZ = stSource1.dZ - stSource2.dZ;
}