public void FindSensorPosition(string direction, DeviceAngleInfo angleInfo) { if (direction == "right") { //계산시작(기본단위는 Radian으로 합시다) angleInfo.rightDevice_thetaR = angleInfo.rightDevice_thetaR2 - angleInfo.rightDevice_thetaR1; angleInfo.rightDevice_thetaL = angleInfo.rightDevice_thetaR3 - angleInfo.rightDevice_thetaR2; // a, b 변수 설정 double a = angleInfo.p[1].Y - angleInfo.p[0].Y; double b = angleInfo.p[2].Y - angleInfo.p[1].Y; double A = angleInfo.rightDevice_thetaR + angleInfo.rightDevice_thetaL; // 공식대입 angleInfo.rightDevice_thetaM = Math.Atan2((a * Math.Tan(angleInfo.rightDevice_thetaR)) + (b * Math.Tan(angleInfo.rightDevice_thetaR)) - (a * Math.Tan(A)), b * Math.Tan(A) * Math.Tan(angleInfo.rightDevice_thetaR)); double x = a * (1 - Math.Tan(angleInfo.rightDevice_thetaM) * Math.Tan(angleInfo.rightDevice_thetaR)) / (Math.Tan(angleInfo.rightDevice_thetaR) * (Math.Tan(angleInfo.rightDevice_thetaM) * Math.Tan(angleInfo.rightDevice_thetaM) + 1)); double y = Math.Tan(angleInfo.rightDevice_thetaM) * x; // 답 출력 answerPoint.X = x + angleInfo.p[0].X; // x값 출력 answerPoint.Y = y; } else if (direction == "left") { //계산시작(기본단위는 Radian으로 합시다) angleInfo.leftDevice_thetaR = angleInfo.leftDevice_thetaR2 - angleInfo.leftDevice_thetaR1; angleInfo.leftDevice_thetaL = angleInfo.leftDevice_thetaR3 - angleInfo.leftDevice_thetaR2; // a, b 변수 설정 double a = angleInfo.p[1].Y - angleInfo.p[0].Y; double b = angleInfo.p[2].Y - angleInfo.p[1].Y; double A = angleInfo.leftDevice_thetaR + angleInfo.leftDevice_thetaL; // 공식대입 angleInfo.leftDevice_thetaM = Math.Atan2((a * Math.Tan(angleInfo.leftDevice_thetaR)) + (b * Math.Tan(angleInfo.leftDevice_thetaR)) - (a * Math.Tan(A)), b * Math.Tan(A) * Math.Tan(angleInfo.leftDevice_thetaR)); double x = a * (1 - Math.Tan(angleInfo.leftDevice_thetaM) * Math.Tan(angleInfo.leftDevice_thetaR)) / (Math.Tan(angleInfo.leftDevice_thetaR) * (Math.Tan(angleInfo.leftDevice_thetaM) * Math.Tan(angleInfo.leftDevice_thetaM) + 1)); double y = Math.Tan(angleInfo.leftDevice_thetaM) * x; // 답 출력 answerPoint.X = x + angleInfo.p[0].X; // x값 출력 answerPoint.Y = y; } }
public void FindSensorPosition(string direction, DeviceAngleInfo angleInfo) { if (direction == "right") { //계산시작(기본단위는 Radian으로 합시다) angleInfo.rightDevice_thetaR = angleInfo.rightDevice_thetaR2 - angleInfo.rightDevice_thetaR1; angleInfo.rightDevice_thetaL = angleInfo.rightDevice_thetaR3 - angleInfo.rightDevice_thetaR2; // a, b 변수 설정 double a = angleInfo.p[1].X - angleInfo.p[0].X; double b = angleInfo.p[2].X - angleInfo.p[1].X; // 공식대입 angleInfo.rightDevice_thetaM = Math.Atan2((a * Math.Tan(angleInfo.rightDevice_thetaL) - b * Math.Tan(angleInfo.rightDevice_thetaR)), ((a + b) * Math.Tan(angleInfo.rightDevice_thetaR) * Math.Tan(angleInfo.rightDevice_thetaL))); double y = a / (Math.Tan(angleInfo.rightDevice_thetaR + angleInfo.rightDevice_thetaM) - Math.Tan(angleInfo.rightDevice_thetaM)); double x = y * Math.Tan(angleInfo.rightDevice_thetaM); // 답 출력 answerPoint.X = x + angleInfo.p[1].X; // x값 출력 answerPoint.Y = y; } else if (direction == "left") { //계산시작(기본단위는 Radian으로 합시다) angleInfo.leftDevice_thetaR = angleInfo.leftDevice_thetaR2 - angleInfo.leftDevice_thetaR1; angleInfo.leftDevice_thetaL = angleInfo.leftDevice_thetaR3 - angleInfo.leftDevice_thetaR2; // a, b 변수 설정 double a = angleInfo.p[1].X - angleInfo.p[0].X; double b = angleInfo.p[2].X - angleInfo.p[1].X; // 공식대입 angleInfo.leftDevice_thetaM = Math.Atan2((a * Math.Tan(angleInfo.leftDevice_thetaL) - b * Math.Tan(angleInfo.leftDevice_thetaR)), ((a + b) * Math.Tan(angleInfo.leftDevice_thetaR) * Math.Tan(angleInfo.leftDevice_thetaL))); double y = a / (Math.Tan(angleInfo.leftDevice_thetaR + angleInfo.leftDevice_thetaM) - Math.Tan(angleInfo.leftDevice_thetaM)); double x = y * Math.Tan(angleInfo.leftDevice_thetaM); // 답 출력 answerPoint.X = x + angleInfo.p[1].X; // x값 출력 answerPoint.Y = y; } }
public void FindSensorPosition(string direction, DeviceAngleInfo angleInfo) { Boolean isRightSided = false; if (direction == "right") { //계산시작(기본단위는 Radian) angleInfo.rightDevice_thetaR = Math.Abs(angleInfo.rightDevice_thetaR2 - angleInfo.rightDevice_thetaR1); angleInfo.rightDevice_thetaL = Math.Abs(angleInfo.rightDevice_thetaR3 - angleInfo.rightDevice_thetaR2); if (angleInfo.p[1].X < angleInfo.p[2].X) { } /* p1 [p0] * p3 [p2] p2 [p1] * 꼴일경우... */ else { isRightSided = true; } // a, b 변수 설정 double a = Math.Abs(angleInfo.p[1].Y - angleInfo.p[0].Y); double b = Math.Abs(angleInfo.p[2].X - angleInfo.p[1].X); double thetaA = angleInfo.rightDevice_thetaR + angleInfo.rightDevice_thetaL; // 공식대입 angleInfo.rightDevice_thetaM = Math.Atan2((b * Math.Tan(thetaA) - (a * Math.Tan(thetaA) * Math.Tan(angleInfo.rightDevice_thetaL)) - (b * Math.Tan(angleInfo.rightDevice_thetaL))), a * Math.Tan(angleInfo.rightDevice_thetaL)); double y = ((a * Math.Tan(thetaA)) + (a * Math.Tan(angleInfo.rightDevice_thetaM)) - (Math.Tan(thetaA) * Math.Tan(angleInfo.rightDevice_thetaM) * b) + b) / (Math.Tan(thetaA) * (1 + Math.Tan(angleInfo.rightDevice_thetaM) * Math.Tan(angleInfo.rightDevice_thetaM))); double x = y * Math.Tan(angleInfo.rightDevice_thetaM); // 답 출력 answerPoint.X = (isRightSided == true) ? angleInfo.p[2].X - x : angleInfo.p[2].X + x; // x값 출력 answerPoint.Y = y; } else if (direction == "left") { //계산시작(기본단위는 Radian) angleInfo.leftDevice_thetaR = Math.Abs(angleInfo.leftDevice_thetaR2 - angleInfo.leftDevice_thetaR1); angleInfo.leftDevice_thetaL = Math.Abs(angleInfo.leftDevice_thetaR3 - angleInfo.leftDevice_thetaR2); /* p1 [p0] * p2 [p1] p3 [p2] * 꼴일경우... */ if (angleInfo.p[1].X < angleInfo.p[2].X) { } /* p1 [p0] * p3 [p2] p2 [p1] * 꼴일경우... */ else { isRightSided = true; } // a, b 변수 설정 double a = Math.Abs(angleInfo.p[1].Y - angleInfo.p[0].Y); double b = Math.Abs(angleInfo.p[2].X - angleInfo.p[1].X); double thetaA = angleInfo.leftDevice_thetaR + angleInfo.leftDevice_thetaL; //공식대입 4월 23일버전 angleInfo.leftDevice_thetaM = Math.Atan2((b * Math.Tan(thetaA) - (a * Math.Tan(thetaA) * Math.Tan(angleInfo.leftDevice_thetaL)) - (b * Math.Tan(angleInfo.leftDevice_thetaL))), a * Math.Tan(angleInfo.leftDevice_thetaL)); double y = ((a * Math.Tan(thetaA)) + (a * Math.Tan(angleInfo.leftDevice_thetaM)) - (Math.Tan(thetaA) * Math.Tan(angleInfo.leftDevice_thetaM) * b) + b) / (Math.Tan(thetaA) * (1 + Math.Tan(angleInfo.leftDevice_thetaM) * Math.Tan(angleInfo.leftDevice_thetaM))); double x = Math.Tan(angleInfo.leftDevice_thetaM) * y; // 답 출력 answerPoint.X = (isRightSided == true) ? angleInfo.p[2].X - x : angleInfo.p[2].X + x; // x값 출력 answerPoint.Y = y; } }