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; }