回転後の座標について 5軸加工機

質問

今年中に、ＯＫＫの縦型マシニング機械(三菱Neomatic)に日研のＮＣインデックス(角度と回転ができる仕様)を載せる予定です。
使いやすくするため、テーブル回転後の座標を、変数で計算ができるプログラムを作っているのですが、うまくいかず頭をかかえてる毎日であります。

そこで、皆様の力をお願いしたく質問させていただきます。

私の希望としては、
＃101＝回転させたいＧ54からのＸ座標(+ＯＲ-)
＃102＝回転させたいＧ54からのＹ座標(+ＯＲ-)
＃103＝回転させたいＧ54からのＺ座標(+ＯＲ-)
＃104＝回転Ｃ(+ＯＲ-)
＃105＝角度Ａ(+ＯＲ-)

上記、Ｇ54座標中心からずれた数値(＃101から＃103)から、回転＆角度(＃104、＃105)の数値を変更するだけで、ひねった後の座標を計算し、その答えをＧ55の座標系に入れるプログラムを作っているのですが、いくつか問題がある為うまくいきません。

◎Ｇ54のワーク座標がテーブル回転中心ではない。
◎Ｇ54のワーク座標の中心位置から、さらにずらした位置を回転させたい。
◎Ｘ，Ｙ，Ｚ及び、回転軸Ｃ、角度軸Ａ　共にプラス数値とマイナス数値があり計算が困難。
◎回転軸Ｃ、角度軸Ａ　共に90度以上回る。

この様な原因があり、電卓で計算した数値と変数で計算させた数値が違い、困った次第であります。

機械の特徴としては、
◎機械座標はマイナス数値
◎テーブル回転中心座標は＃501＝Ｘ　＃502＝Ｙ　＃503＝Ｚ　に登録済み。

★☆まとめ☆★

＃5221＝Ｇ54Ｘ座標
＃5222＝Ｇ54Ｙ座標
＃5223＝Ｇ54Ｘ座標　を使い

＃101＝回転させたいＧ54からのＸ座標(+ＯＲ-)
＃102＝回転させたいＧ54からのＹ座標(+ＯＲ-)
＃103＝回転させたいＧ54からのＺ座標(+ＯＲ-)
＃104＝回転Ｃ(+ＯＲ-)
＃105＝角度Ａ(+ＯＲ-)
で移動させた数値の答えを

＃5221＝Ｇ54Ｘ座標
＃5222＝Ｇ54Ｙ座標
＃5223＝Ｇ54Ｚ座標　に登録したい。

文章が下手で申し訳ありませんが、どなたか変数を使い回転後の座標が計算できるプログラムをお教え下さい。

不明な点等ございましたらお教え下さい。

大変困っているので宜しくお願いします。

まとめの部分を下記の様に修正いたします。

★☆まとめ☆★

＃5221＝Ｇ54Ｘ座標
＃5222＝Ｇ54Ｙ座標
＃5223＝Ｇ54Ｘ座標　を使い

＃101＝回転させたいＧ54からのＸ座標(+ＯＲ-)
＃102＝回転させたいＧ54からのＹ座標(+ＯＲ-)
＃103＝回転させたいＧ54からのＺ座標(+ＯＲ-)
＃104＝回転Ｃ(+ＯＲ-)
＃105＝角度Ａ(+ＯＲ-)
で移動させた数値の答えを

＃5241＝Ｇ55Ｘ座標
＃5242＝Ｇ55Ｙ座標
＃5243＝Ｇ55Ｚ座標　に登録したい。

回答No.1

noname#230359

下記例が参考になれば幸いです。
一般に、C軸回転中心YとA軸回転中心Yが違うものがあるので、変数を増やしてあります。
----------
#5221=-500.000
#5222=-400.000
#5223=-300.000
#101=50.000(G54からシフトX)
#102=40.000(G54からシフトY)
#103=30.000(G54からシフトZ)
#104=80.000(C軸回りの座標系の回転角度･Z+方向からみて反時計回りが+)
#105=110.000(A軸回りの座標系の回転角度･X+方向からみて反時計回りが+)
#501=-600.000(C軸回転中心X)
#502=-500.000(C軸回転中心Y)
#512=-600.000(A軸回転中心Y)
#513=-400.000(A軸回転中心Z)

#5241=[#101+#5221-#501]*COS[#104]-[#102+#5222-#502]*SIN[#104]+#501
#1=[#101+#5221-#501]*SIN[#104]+[#102+#5222-#502]*COS[#104]+#502
#5242=[#1-#512]*COS[#105]-[#103+#5223-#513]*SIN[#105]+#512
#5243=[#1-#512]*SIN[#105]+[#103+#5223-#513]*COS[#105]+#513
----------
上記にて
#5241=-711.826
#5242=-815.200
#5243=-188.836
となり、CADで求めたものと合っているのを確認しました。

#104と#105のプラスマイナスを逆にすれば良いです。
----------
#5241=[#101+#5221-#501]*COS[-#104]-[#102+#5222-#502]*SIN[-#104]+#501
#1=[#101+#5221-#501]*SIN[-#104]+[#102+#5222-#502]*COS[-#104]+#502
#5242=[#1-#512]*COS[-#105]-[#103+#5223-#513]*SIN[-#105]+#512
#5243=[#1-#512]*SIN[-#105]+[#103+#5223-#513]*COS[-#105]+#513
----------

補足

有難うございます。
やっと確認ができる事ができました。
自分で作った変数は何行もあり長ったらしく、
それに比べシンプルかつ正確です。(笑)
とても感謝しています。

１つだけ問題があるのですが、A軸
及びC軸の表記がtokuhonさんが言われた＋－が下記条件とは逆
#104=(C軸回りの座標系の回転角度･Z+方向からみて反時計回りが+)
#105=(A軸回りの座標系の回転角度･X+方向からみて反時計回りが+)
の場合、どこの計算式を変更すれば宜しいでしょうか？

宜しくお願いします。