WioTerminalでドーナツをぐりぐりしてみた。

Wio Terminal でドーナツの形を立体表示させて、グリグリと動かしてみた。

ドーナツの形は、トーラスと言って、平面に描いた円を軸の周りにぐるっと一周させるとできます。
これを数式で表すと以下のように書けます。

     x = (a + r*cos(θ) ) * cos(φ)
     y = (a + r*cos(θ) ) * sin(φ)
     z = r*sin(θ)

とりあえず、どんな形になるか見てみるには、Pythonでちゃちゃっとやると確認できます。
こんな感じですね。

from matplotlib import cm
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import numpy as np

fig = plt.figure(figsize=(6,6))
ax = fig.add_subplot(111, projection='3d')

u=np.linspace(0,2*np.pi,100)
v=np.linspace(0,2*np.pi,100)
u,v=np.meshgrid(u,v)
a = 2
b = 9
X = (b + a*np.cos(u)) * np.cos(v)
Y = (b + a*np.cos(u)) * np.sin(v)
Z = a * np.sin(u)

ax.set_xlim(-7,7)
ax.set_ylim(-7,7)
ax.set_zlim(-7,7)

ax.plot_surface(X, Y, Z,alpha=0.8, cmap=cm.Wistia)

plt.show()

グリグリするだけなら、このままマウスで動かせば、これで十分ですが、せっかくなので、Wio Terminalでやってみました。
Pythonですと、matprotlib や、numpy がちゃんと処理してくれるのですが、Wioでやるには、視点を設定したり、３D座標から２D座標に投影したり、ちょっと、手間をかけなければなりません。
で、いろいろ、調べながら、以下のように仕上がりました。
Wioでグリグリするには、
A, B, C, ボタンを押しながら、5ボタンの上下を押すと、X軸、Y軸、Z軸、まわりにドーナツが回転します。
A, B, C, ボタンを押さずに、5ボタンの上下を押すと、視点の仰角が変わります。また、この時、左右を押すと視点が回ります。
また、5ボタンを押すと視点がドーナツに近づいていきます。そのまま、ドーナツを通り過ぎると、遠ざかっていきます。
Cボタンを押しながら、5ボタンを押すと、逆に、ズーム、パンします。
ま、説明するよりやってみた方が早いですね。

#include <LovyanGFX.hpp>
#include <math.h>

static LGFX lcd;
static LGFX_Sprite torus(&lcd);

float pi=3.141592;
static auto transpalette = 0;          

float r = 2.0;    // ドーナツの太さ
float a = 5.0;    // ドーナツの大きさ
float x, y, z, x01, y01, z01, x02, y02, xv, yv,zv;
float xa,ya,za,xb,yb,zb,xg,yg,zg;
float phi = 0.0 / 180.0 * pi;   // 仰角
float theta = 0.0 / 180.0 * pi; // 回転角
float distance = 100.0;         // 視点までの距離
float alpha = 0.0, beta = 0.0, gmma = 0.0;   //ドーナツの傾き
float dg_p = 0.0, dg_t = 0.0;   // 仰角と回転角の度数（deg)

void disp_shape() {
  xv = distance * cos(phi) * cos(theta);
  yv = distance * cos(phi) * sin(theta);
  zv = distance * sin(phi) ;

  //Serial.printf("xv:%3.2f, yv:%3.2f, zv:%3.2f \n", xv,yv,zv);
  torus.clear(); 
  for (float i = 0.0; i<5*pi; i+=0.1) {    
    for (float j = 0.0; j<2*pi; j+=0.1) {

      //torus
    x = (a + r*cos(j) ) * cos(i)+2;
    y = (a + r*cos(j) ) * sin(i);
    z = r*sin(j);

      //x軸傾き
      xa = x;
      ya = y*cos(alpha) - z*sin(alpha);
      za = y*sin(alpha) + z*cos(alpha);

      //y軸傾き
      xb = xa*cos(beta) + za*sin(beta);
      yb = ya;
      zb = -xa*sin(beta) + za*cos(beta);

      //z軸傾き
      xg = xb*cos(gmma) - yb*sin(gmma);
      yg = xb*sin(gmma) + yb*cos(gmma);
      zg = zb;

      //
      x = xg;
      y = yg;
      z = zg;
      
      x01 = -sin(theta)*(x-xv) + cos(theta)*(y-yv);
      y01 = -sin(phi)*cos(theta)*(x-xv) - sin(phi)*sin(theta)*(y-yv) + cos(phi)*(z-zv);
      z01 = -cos(phi)*cos(theta)*(x-xv) - cos(phi)*sin(theta)*(y-yv) - sin(phi)*(z-zv);
      x02 = x01*100.0/z01;
      y02 = y01*100.0/z01;
     
      torus.drawPixel(x02*10+160,y02*10+120,1);
    }
  }
  
  torus.pushSprite(0,0);

  
}

void setup() {
  lcd.init();
  lcd.setRotation(1);
  lcd.setBrightness(64);
  lcd.setColorDepth(16);
  lcd.clear(); 

  torus.setColorDepth(lgfx::palette_4bit);
  torus.createSprite(lcd.width(), lcd.height());
  torus.setPaletteColor(1, 0, 0, 255);
  
  pinMode(WIO_5S_UP, INPUT_PULLUP);
  pinMode(WIO_5S_DOWN, INPUT_PULLUP);
  pinMode(WIO_5S_LEFT, INPUT_PULLUP);
  pinMode(WIO_5S_RIGHT, INPUT_PULLUP);
  pinMode(WIO_5S_PRESS, INPUT_PULLUP);
  pinMode(WIO_KEY_A, INPUT_PULLUP);
  pinMode(WIO_KEY_B, INPUT_PULLUP);
  pinMode(WIO_KEY_C, INPUT_PULLUP);

  disp_shape();
}

void loop() {

   if ((digitalRead(WIO_5S_PRESS) == LOW)and (digitalRead(WIO_KEY_C) == HIGH)) {
    distance -= 10.0;
    disp_shape();   
   }

   else if ((digitalRead(WIO_5S_PRESS) == LOW) and (digitalRead(WIO_KEY_C) == LOW)) {
    distance += 10.0;
    disp_shape();   
   }

   else if ((digitalRead(WIO_5S_UP) == LOW) and (digitalRead(WIO_KEY_C) == LOW)) {
    alpha = alpha+5.0/180.0*pi;
    disp_shape();
   }

   else if ((digitalRead(WIO_5S_DOWN) == LOW) and (digitalRead(WIO_KEY_C) == LOW)) {
    alpha -= 5.0/180.0*pi;
    disp_shape();
   }

   else if ((digitalRead(WIO_5S_UP) == LOW) and (digitalRead(WIO_KEY_B) == LOW)) {
    beta += 5.0/180.0*pi;
    disp_shape();
   }

   else if ((digitalRead(WIO_5S_DOWN) == LOW) and (digitalRead(WIO_KEY_B) == LOW)) {
    beta -= 5.0/180.0*pi;
    disp_shape();
   }

   else if ((digitalRead(WIO_5S_UP) == LOW) and (digitalRead(WIO_KEY_A) == LOW)) {
    gmma += 5.0/180.0*pi;
    disp_shape();
   }

   else if ((digitalRead(WIO_5S_DOWN) == LOW) and (digitalRead(WIO_KEY_A) == LOW)) {
    gmma -= 5.0/180.0*pi;
    disp_shape();
   }
   else if ((digitalRead(WIO_5S_UP) == LOW) and (digitalRead(WIO_KEY_A) == HIGH) and (digitalRead(WIO_KEY_B) == HIGH) and (digitalRead(WIO_KEY_C) == HIGH)) {
    phi = float(dg_p+=10.0) / 180.0 * pi;
    if(dg_p>360)dg_p-=360;
    disp_shape();
   }
   
   else if ((digitalRead(WIO_5S_DOWN) == LOW) and (digitalRead(WIO_KEY_A) == HIGH) and (digitalRead(WIO_KEY_B) == HIGH) and (digitalRead(WIO_KEY_C) == HIGH)) {
    phi = float(dg_p-=10.0) / 180.0 * pi;
    disp_shape();
   }
   
   else if (digitalRead(WIO_5S_LEFT) == LOW) {
    theta = float(dg_t+=10.0) / 180.0 * pi;
    if(dg_t>360)dg_t-=360;
    disp_shape();
   }
   
   else if (digitalRead(WIO_5S_RIGHT) == LOW) {
    theta = float(dg_t-=10.0) / 180.0 * pi;
    disp_shape();
   }

}