sword_rotate_center.ipynb

{
  "nbformat": 4,
  "nbformat_minor": 0,
  "metadata": {
    "colab": {
      "name": "sword_rotate_center.ipynb",
      "provenance": [],
      "collapsed_sections": []
    },
    "kernelspec": {
      "name": "python3",
      "display_name": "Python 3"
    }
  },
  "cells": [
    {
      "cell_type": "code",
      "metadata": {
        "id": "VPb9b_4lo7Di",
        "colab_type": "code",
        "cellView": "form",
        "colab": {}
      },
      "source": [
        "#@markdown ■■■■■■■■■■■■■■■■■■\n",
        "\n",
        "#@markdown 【セル①】　準備\n",
        "\n",
        "#@markdown ■■■■■■■■■■■■■■■■■■\n",
        "\n",
        "# qt5\n",
        "\n",
        "! sudo apt-get install python3-pyqt5  \n",
        "! sudo apt-get install pyqt5-dev-tools\n",
        "! sudo apt-get install qttools5-dev-tools\n",
        "\n",
        "\n",
        "print(\"■■■■■■■■■■■■■■■■■■\")\n",
        "print(\"■　準備が完了しました。\")\n",
        "print(\"■■■■■■■■■■■■■■■■■■\")"
      ],
      "execution_count": 0,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "LQuwEMhUqdQU",
        "colab_type": "code",
        "cellView": "form",
        "colab": {}
      },
      "source": [
        "from math import atan2, acos, asin, cos, sin, degrees, isnan, isclose, sqrt, pi, isinf, radians\n",
        "from PyQt5.QtGui import QQuaternion, QVector3D, QVector2D, QMatrix4x4, QVector4D\n",
        "import traceback\n",
        "\n",
        "#@markdown ■■■■■■■■■■■■■■■■■■\n",
        "\n",
        "#@markdown 【セル②】　計算\n",
        "\n",
        "#@markdown ■■■■■■■■■■■■■■■■■■\n",
        "\n",
        "# 指定された3点と半径を通る球の中心点を求める\n",
        "# https://oshiete.goo.ne.jp/qa/195295.html\n",
        "# https://okwave.jp/qa/q9467739.html\n",
        "def calc_sphere_center(pv, wv, nv, r):\n",
        "    x1 = pv.x()\n",
        "    y1 = pv.y()\n",
        "    z1 = pv.z()\n",
        "    x2 = wv.x()\n",
        "    y2 = wv.y()\n",
        "    z2 = wv.z()\n",
        "    x3 = nv.x()\n",
        "    y3 = nv.y()\n",
        "    z3 = nv.z()\n",
        "\n",
        "    m = (pv + wv + nv) / 3\n",
        "\n",
        "    try:\n",
        "        tm01=x1**2-x2**2+y1**2-y2**2+z1**2-z2**2\n",
        "        tm02=x1**2-x3**2+y1**2-y3**2+z1**2-z3**2\n",
        "        tm11=-2*(x1-x2)*(z1-z3)+2*(x1-x3)*(z1-z2)\n",
        "        tm12=-2*(y1-y2)*(z1-z3)+2*(y1-y3)*(z1-z2)\n",
        "        tm13=tm01*(z1-z3)-tm02*(z1-z2)\n",
        "        tm21=-2*(x1-x2)*(y1-y3)+2*(x1-x3)*(y1-y2)\n",
        "        tm22=-2*(z1-z2)*(y1-y3)+2*(z1-z3)*(y1-y2)\n",
        "        tm23=tm01*(y1-y3)-tm02*(y1-y2)\n",
        "        tma=1+tm11**2/tm12**2+tm21**2/tm22**2\n",
        "        tmb=-2*x1+2*(y1+tm13/tm12)*tm11/tm12+2*(z1+tm23/tm22)*tm21/tm22\n",
        "        tmc=x1**2+(y1+tm13/tm12)**2+(z1+tm23/tm22)**2-r**2\n",
        "        xq1=(-tmb+sqrt(abs(tmb**2-4*tma*tmc)))/2/tma\n",
        "        xq2=(-tmb-sqrt(abs(tmb**2-4*tma*tmc)))/2/tma\n",
        "        yq1=-tm13/tm12-tm11/tm12*xq1\n",
        "        yq2=-tm13/tm12-tm11/tm12*xq2\n",
        "        zq1=-tm23/tm22-tm21/tm22*xq1\n",
        "        zq2=-tm23/tm22-tm21/tm22*xq2\n",
        "\n",
        "        c1 = QVector3D(xq1, yq1, zq1)\n",
        "        c2 = QVector3D(xq2, yq2, zq2)\n",
        "\n",
        "        if c1 == c2:\n",
        "            # 重解\n",
        "            return c1, r\n",
        "\n",
        "        if c1.distanceToPoint(m) < c2.distanceToPoint(m):\n",
        "            # 3点の中間に近い方を返す\n",
        "            return c1, r\n",
        "\n",
        "        return c2, r\n",
        "    except ZeroDivisionError as e:\n",
        "        # print(\"ゼロ割エラー1\")\n",
        "        # print(traceback.format_exc())\n",
        "\n",
        "        if round(x1,1) == round(x2,1) == round(x3,1):\n",
        "            # 同じ値の場合、2次元円として求める\n",
        "            cx, cy, r = calc_circle_center(y1, z1, y2, z2, y3, z3)\n",
        "            return QVector3D(x1, cx, cy), r\n",
        "        \n",
        "        if round(y1,1) == round(y2,1) == round(y3,1):\n",
        "            cx, cy, r = calc_circle_center(x1, z1, x2, z2, x3, z3)\n",
        "            return QVector3D(cx, y1, cy), r\n",
        "        \n",
        "        if round(z1,1) == round(z2,1) == round(z3,1):\n",
        "            cx, cy, r = calc_circle_center(x1, y1, x2, y2, x3, y3)\n",
        "            return QVector3D(cx, cy, z1), r\n",
        "    \n",
        "    return QVector3D(), r\n",
        "\n",
        "\n",
        "# http://www.iot-kyoto.com/satoh/2016/01/29/tangent-003/\n",
        "# http://nobutina.blog86.fc2.com/blog-entry-674.html\n",
        "def calc_circle_center(x1, y1, x2, y2, x3, y3):\n",
        "\n",
        "    G=( y2*x1-y1*x2 +y3*x2-y2*x3 +y1*x3-y3*x1 )\n",
        "\n",
        "    try:\n",
        "        Xc= ((x1*x1+y1*y1)*(y2-y3)+(x2*x2+y2*y2)*(y3-y1)+(x3*x3+y3*y3)*(y1-y2))/(2*G)\n",
        "        Yc=-((x1*x1+y1*y1)*(x2-x3)+(x2*x2+y2*y2)*(x3-x1)+(x3*x3+y3*y3)*(x1-x2))/(2*G)\n",
        "\n",
        "        Xd=(((x1*x1+y1*y1)-(x2*x2+y2*y2))*(y2-y3)-((x2*x2+y2*y2)-(x3*x3+y3*y3))*(y1-y2))/(2*((x1-x2)*(y2-y3)-(x2-x3)*(y1-y2)))\n",
        "        Yd=(((y1*y1+x1*x1)-(y2*y2+x2*x2))*(x2-x3)-((y2*y2+x2*x2)-(y3*y3+x3*x3))*(x1-x2))/(2*((y1-y2)*(x2-x3)-(y2-y3)*(x1-x2)))\n",
        "\n",
        "        G=2 * sqrt( (x1 - Xc) * (x1 - Xc) + (y1 - Yc) * (y1 - Yc) )\n",
        "\n",
        "        return Xd, Yd, G/2\n",
        "    except ZeroDivisionError as e:\n",
        "        # print(\"ゼロ割エラー2\")\n",
        "        # print(traceback.format_exc())\n",
        "\n",
        "        if round(x1,1) == round(x2,1) == round(x3,1):\n",
        "            G = sqrt((y1 + y2 + y3) ** 2)\n",
        "            return (x1 + x2 + x3) / 3, (y1 + y2 + y3) / 3, G\n",
        "\n",
        "        G = sqrt((x1 + x2 + x3) ** 2)\n",
        "        return (x1 + x2 + x3) / 3, (y1 + y2 + y3) / 3, G\n",
        "\n",
        "    return 0, 0, 0\n",
        "\n",
        "\n",
        "\n",
        "#@markdown 三点の位置のXYZをそれぞれの欄に入力して、セルを実行してください。\n",
        "#@markdown \n",
        "#@markdown 回転支点の位置とその軸制限が求められます。\n",
        "#@markdown \n",
        "#@markdown 三点の位置は、刀を真上から見た時の中心線を通る頂点を選んでください\n",
        "#@markdown \n",
        "#@markdown 大体の場合、XYZのどれかがとても大きな値（打刀で30以上くらい）になります。\n",
        "#@markdown \n",
        "#@markdown ---\n",
        "\n",
        "#@markdown ### 刀の刃区（根元）のXYZ\n",
        "\n",
        "point1_x = 3.321042  #@param {type: \"number\"}\n",
        "point1_y = 5.73096  #@param {type: \"number\"}\n",
        "point1_z = -3.896624  #@param {type: \"number\"}\n",
        "\n",
        "#@markdown ---\n",
        "\n",
        "#@markdown ### 刃区と切っ先の中間くらいのXYZ\n",
        "\n",
        "point2_x = -0.4420419  #@param {type: \"number\"}\n",
        "point2_y = 5.700311  #@param {type: \"number\"}\n",
        "point2_z = -3.902588  #@param {type: \"number\"}\n",
        "\n",
        "#@markdown ---\n",
        "\n",
        "#@markdown ### 刀の切っ先（先端）のXYZ\n",
        "\n",
        "point3_x = -4.205778  #@param {type: \"number\"}\n",
        "point3_y = 5.968236  #@param {type: \"number\"}\n",
        "point3_z = -3.908525  #@param {type: \"number\"}\n",
        "\n",
        "point1_vec = QVector3D(point1_x, point1_y, point1_z)\n",
        "point2_vec = QVector3D(point2_x, point2_y, point2_z)\n",
        "point3_vec = QVector3D(point3_x, point3_y, point3_z)\n",
        "\n",
        "print(\"point1 %s\" % (point1_vec))\n",
        "print(\"point2 %s\" % (point2_vec))\n",
        "print(\"point3 %s\" % (point3_vec))\n",
        "\n",
        "# 半径は3点間の距離の最長の半分\n",
        "r = max(point1_vec.distanceToPoint(point2_vec), point1_vec.distanceToPoint(point3_vec),  point2_vec.distanceToPoint(point3_vec)) / 2\n",
        "\n",
        "# 半径と原点を求める\n",
        "center, radius = calc_sphere_center(point1_vec, point2_vec, point3_vec, r)\n",
        "\n",
        "# 鞘回転支点の軸制限\n",
        "fixed_axis1 = QVector3D.crossProduct(center, (point2_vec - point1_vec).normalized())\n",
        "\n",
        "# 刀回転支点の軸制限\n",
        "fixed_axis2 = QVector3D.crossProduct(center, (point2_vec - point3_vec).normalized())\n",
        "\n",
        "print(\"■■■■■■■■■■■■■■■■■■\")\n",
        "print(\"■　回転支点の位置\")\n",
        "print(\"■　x: %s\" % (center.x()))\n",
        "print(\"■　y: %s\" % (center.y()))\n",
        "print(\"■　z: %s\" % (center.z()))\n",
        "# print(\"■　--------\")\n",
        "# print(\"■　鞘回転支点の軸制限\")\n",
        "# print(\"■　x: %s\" % (fixed_axis1.x()))\n",
        "# print(\"■　y: %s\" % (fixed_axis1.y()))\n",
        "# print(\"■　z: %s\" % (fixed_axis1.z()))\n",
        "# print(\"■　--------\")\n",
        "# print(\"■　刀回転支点の軸制限\")\n",
        "# print(\"■　x: %s\" % (fixed_axis2.x()))\n",
        "# print(\"■　y: %s\" % (fixed_axis2.y()))\n",
        "# print(\"■　z: %s\" % (fixed_axis2.z()))\n",
        "print(\"■■■■■■■■■■■■■■■■■■\")\n"
      ],
      "execution_count": 0,
      "outputs": []
    }
  ]
}