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