LoginSignup
8
4

More than 5 years have passed since last update.

@runomee(天上の宴)

pythonista3を使ってmotionセンサのデータをsceneで描画

Last updated at Posted at 2017-05-03

pythonista3を使って、motionセンサのデータからiPadの回転に合わせて箱を画面上で回転させるようにしてみました。
sceneを使って描画させています。
IMG_0340.JPG

ソースは以下の通り。

motion_with_scene.py
#coding: utf-8
# For use in pythonista on iOS
#import ui
import motion
from scene import *
import math
import numpy as np
import time
scale =80  # scale raw accelerometer values to screen
W=2
L=1
H=.5/2
class MyScene (Scene):
    def setup(self):
            global scale
            scale=self.size.w/10
            #motion start
            motion.start_updates()
            #pitch,roll,yawメータの半径
            self.R=scale
            #Boxの定義 後の計算の為arrayにしておく
            self.Box=[[[W,L,-H]],[[-W,L,-H]],[[-W,-L,-H]],[[W,-L,-H]],[[W,L,H]],[[-W,L,H]],[[-W,-L,H]],[[W,-L,H]]]
            self.Box=np.array(self.Box)

    def draw(self):
        #Boxの中心
        self.cx =self.size.w * 0.5
        self.cy = self.size.h * 0.5
        #pitch,roll,yawメータの中心
        self.cx2 =self.size.w * 0.5
        self.cy2 = self.size.h * 0.5-scale*3.5
        time.sleep(0.1)
        #motionセンサの値更新
        ax,ay,az = motion.get_user_acceleration()
        gx,gy,gz= motion.get_gravity()
        gravity_vectors=motion.get_attitude()
        mx,my,mz,ma=motion.get_magnetic_field()
        pitch, roll, yaw = [x for x in gravity_vectors]
        #ラジアン→度
        pitch=-pitch*180/3.1415926
        roll=roll*180/3.1415926
        yaw=-yaw*180/3.1415926
        #redraw screen
        background(1, 1, 1)
        fill(1,1,1)
        stroke(0,0,0)
        stroke_weight(1)
        #pitch,roll,yaw描画
        ellipse(self.cx2-scale*3-self.R,self.cy2-self.R,self.R*2,self.R*2)
        ellipse(self.cx2-0.0-self.R,self.cy2-self.R,self.R*2,self.R*2)
        ellipse(self.cx2+scale*3-self.R,self.cy2-self.R,self.R*2,self.R*2)
        roll_sin=math.sin(math.radians(roll))
        roll_cos=math.cos(math.radians(roll))
        pitch_sin=math.sin(math.radians(pitch))
        pitch_cos=math.cos(math.radians(pitch))
        yaw_sin=math.sin(math.radians(yaw))
        yaw_cos=math.cos(math.radians(yaw))
        line(self.cx2-roll_cos*self.R-scale*3,self.cy2-roll_sin*self.R,self.cx2+roll_cos*self.R-scale*3,self.cy2+roll_sin*self.R)
        line(self.cx2-pitch_cos*self.R-0,self.cy2-pitch_sin*self.R,self.cx2+pitch_cos*self.R-0,self.cy2+pitch_sin*self.R)
        line(self.cx2-yaw_cos*self.R+scale*3,self.cy2-yaw_sin*self.R,self.cx2+yaw_cos*self.R+scale*3,self.cy2+yaw_sin*self.R)
        #回転マトリックスの計算
        yawMatrix = np.matrix([[yaw_cos, -yaw_sin, 0],[yaw_sin, yaw_cos, 0],[0, 0, 1]])
        pitchMatrix = np.matrix([[pitch_cos, 0, pitch_sin],[0, 1, 0],[-pitch_sin, 0, pitch_cos]])
        rollMatrix = np.matrix([[1, 0, 0],[0, roll_cos, -roll_sin],[0, roll_sin, roll_cos]])
        R = yawMatrix * pitchMatrix * rollMatrix
        R=np.array(R)
        x_3d,y_3d,z_3d=np.transpose(np.dot(self.Box,R),(2,0,1))
        #陰線処理のため1番奥の頂点を特定
        zmin=np.argmin(z_3d)
        #奥の頂点を含んでいない辺を描画
        if zmin!=0 and zmin!=1 :
            line(self.cx+x_3d[0]*scale,self.cy+y_3d[0]*scale,self.cx+x_3d[1]*scale,self.cy+y_3d[1]*scale)
        if zmin!=1 and zmin!=2 :
            line(self.cx+x_3d[1]*scale,self.cy+y_3d[1]*scale,self.cx+x_3d[2]*scale,self.cy+y_3d[2]*scale)
        if zmin!=2 and zmin!=3 :
            line(self.cx+x_3d[2]*scale,self.cy+y_3d[2]*scale,self.cx+x_3d[3]*scale,self.cy+y_3d[3]*scale)
        if zmin!=3 and zmin!=0 :
            line(self.cx+x_3d[3]*scale,self.cy+y_3d[3]*scale,self.cx+x_3d[0]*scale,self.cy+y_3d[0]*scale)

        if zmin!=4 and zmin!=5 :
            line(self.cx+x_3d[4]*scale,self.cy+y_3d[4]*scale,self.cx+x_3d[5]*scale,self.cy+y_3d[5]*scale)
        if zmin!=5 and zmin!=6 :
            line(self.cx+x_3d[5]*scale,self.cy+y_3d[5]*scale,self.cx+x_3d[6]*scale,self.cy+y_3d[6]*scale)
        if zmin!=6 and zmin!=7 :
            line(self.cx+x_3d[6]*scale,self.cy+y_3d[6]*scale,self.cx+x_3d[7]*scale,self.cy+y_3d[7]*scale)
        if zmin!=7 and zmin!=4 :
            line(self.cx+x_3d[7]*scale,self.cy+y_3d[7]*scale,self.cx+x_3d[4]*scale,self.cy+y_3d[4]*scale)

        if zmin!=0 and zmin!=4 :
            line(self.cx+x_3d[0]*scale,self.cy+y_3d[0]*scale,self.cx+x_3d[4]*scale,self.cy+y_3d[4]*scale)
        if zmin!=1 and zmin!=5 :
            line(self.cx+x_3d[1]*scale,self.cy+y_3d[1]*scale,self.cx+x_3d[5]*scale,self.cy+y_3d[5]*scale)
        if zmin!=2 and zmin!=6 :
            line(self.cx+x_3d[2]*scale,self.cy+y_3d[2]*scale,self.cx+x_3d[6]*scale,self.cy+y_3d[6]*scale)
        if zmin!=7 and zmin!=3 :
            line(self.cx+x_3d[3]*scale,self.cy+y_3d[3]*scale,self.cx+x_3d[7]*scale,self.cy+y_3d[7]*scale)
        #取得したmotionセンサのデータを画面上に印字
        tint(0,0,0,1)
        text('roll', font_name='Helvetica', font_size=16.0, x=self.cx2-scale*3, y=self.cy2+self.R+40, alignment=5)
        text(str(round(roll,2)), font_name='Helvetica', font_size=16.0, x=self.cx2-scale*3, y=self.cy2+self.R+20, alignment=5)
        text('pitch', font_name='Helvetica', font_size=16.0, x=self.cx2, y=self.cy2+self.R+40, alignment=5)
        text(str(round(pitch,2)), font_name='Helvetica', font_size=16.0, x=self.cx2, y=self.cy2+self.R+20, alignment=5)
        text('yaw', font_name='Helvetica', font_size=16.0, x=self.cx2+scale*3, y=self.cy2+self.R+40, alignment=5)
        text(str(round(yaw,2)), font_name='Helvetica', font_size=16.0, x=self.cx2+scale*3, y=self.cy2+self.R+20, alignment=5)

        text('ax=',font_name='Helvetica',font_size=16.0,x=20, y=self.cy*2-20, alignment=6)
        text(str(ax),font_name='Helvetica',font_size=16.0,x=60, y=self.cy*2-20, alignment=6)
        text('ay=',font_name='Helvetica',font_size=16.0,x=20, y=self.cy*2-40, alignment=6)
        text(str(ay),font_name='Helvetica',font_size=16.0,x=60, y=self.cy*2-40,alignment=6)
        text('az=',font_name='Helvetica',font_size=16.0,x=20, y=self.cy*2-60, alignment=6)
        text(str(az),font_name='Helvetica',font_size=16.0,x=60, y=self.cy*2-60, alignment=6)

        text('gx=',font_name='Helvetica',font_size=16.0,x=20, y=self.cy*2-80, alignment=6)
        text(str(gx),font_name='Helvetica',font_size=16.0,x=60, y=self.cy*2-80, alignment=6)
        text('gy=',font_name='Helvetica',font_size=16.0,x=20, y=self.cy*2-100, alignment=6)
        text(str(gy),font_name='Helvetica',font_size=16.0,x=60, y=self.cy*2-100,alignment=6)
        text('gz=',font_name='Helvetica',font_size=16.0,x=20, y=self.cy*2-120, alignment=6)
        text(str(gz),font_name='Helvetica',font_size=16.0,x=60, y=self.cy*2-120, alignment=6)
        text('magx=',font_name='Helvetica',font_size=16.0,x=20, y=self.cy*2-140, alignment=6)
        text(str(mx),font_name='Helvetica',font_size=16.0,x=80, y=self.cy*2-140, alignment=6)
        text('magy=',font_name='Helvetica',font_size=16.0,x=20, y=self.cy*2-160, alignment=6)
        text(str(my),font_name='Helvetica',font_size=16.0,x=80, y=self.cy*2-160,alignment=6)
        text('magz=',font_name='Helvetica',font_size=16.0,x=20, y=self.cy*2-180, alignment=6)
        text(str(mz),font_name='Helvetica',font_size=16.0,x=80, y=self.cy*2-180, alignment=6)
if __name__ == "__main__":
   scene = run(MyScene())

簡単に解説します。
まず、motion,scene,math,numpyとtimeをimportします。
motionがimportできなかったらomzさんのmotion.pyをsite-packeagesに放り込みます。omzさん感謝。
後は、boxを描画するときの幅、長さ、高さを設定。scaleは後で変えちゃうので仮の値。

import motion
from scene import *
import math
import numpy as np
import time
scale =80  # scale raw accelerometer values to screen
W=2
L=1
H=.5/2

次に、MySceneを設定して行きます。
def setup(self):で初期設定します。
scaleを画面サイズに合わせます。
motion.start_updates()でモーションセンサを読み取れるようスタートさせておきます。
最後にBoxとpitch,roll,yawのメータ用の半径Rを定義して、初期設定は終わりです。

class MyScene (Scene):
    def setup(self):
            global scale
            scale=self.size.w/10
            #motion start
            motion.start_updates()
            #pitch,roll,yawメータの半径
            self.R=scale
            #Boxの定義 後の計算の為arrayにしておく
            self.Box=[[[W,L,-H]],[[-W,L,-H]],[[-W,-L,-H]],[[W,-L,-H]],[[W,L,H]],[[-W,L,H]],[[-W,-L,H]],[[W,-L,H]]]
            self.Box=np.array(self.Box)

描画するBoxとpitch,roll,yawメータの中心を設定します。
タイマtime.sleep(0.1)はなくてもよいのですが、良く落ちるので気休めに入れてみました。
sceneのdrawメソッドは1秒に60回の頻度でレンダリングループしてくれます。
この機能を使って、motionデータを読み込んで、ipadの回転に合わせて直方体をグルグルさせていきます。

def draw(self):
        #Boxの中心
        self.cx =self.size.w * 0.5
        self.cy = self.size.h * 0.5
        #pitch,roll,yawメータの中心
        self.cx2 =self.size.w * 0.5
        self.cy2 = self.size.h * 0.5-scale*3.5
        time.sleep(0.1)

それでは、次に画面を描画していきます。
Motionセンサの値を取り込みます。pitch, roll, yawはラジアンで取り込まれます。
画面の背景は白、塗りつぶしの色は白、、線の色は白、線の太さは1にしました。色はr、g、bで設定します。

        #motionセンサの値更新
        ax,ay,az = motion.get_user_acceleration()
        gx,gy,gz= motion.get_gravity()
        gravity_vectors=motion.get_attitude()
        mx,my,mz,ma=motion.get_magnetic_field()
        pitch, roll, yaw = [x for x in gravity_vectors]
        #ラジアン→度
        pitch=-pitch*180/3.1415926
        roll=roll*180/3.1415926
        yaw=-yaw*180/3.1415926
        #redraw screen
        background(1, 1, 1)
        fill(1,1,1)
        stroke(0,0,0)
        stroke_weight(1)

いよいよ、描画です。
飛行機が飛ぶときに機体の状態を示すメータをイメージして、pitch、roll、yawの角度を円のなかに角度分傾いた直線を引いて表します。
ellipse(cx,cy,R1,R2)で円を描いた後でlineで直線を引いています。

        #pitch,roll,yaw描画
        ellipse(self.cx2-scale*3-self.R,self.cy2-self.R,self.R*2,self.R*2)
        ellipse(self.cx2-0.0-self.R,self.cy2-self.R,self.R*2,self.R*2)
        ellipse(self.cx2+scale*3-self.R,self.cy2-self.R,self.R*2,self.R*2)
        roll_sin=math.sin(math.radians(roll))
        roll_cos=math.cos(math.radians(roll))
        pitch_sin=math.sin(math.radians(pitch))
        pitch_cos=math.cos(math.radians(pitch))
        yaw_sin=math.sin(math.radians(yaw))
        yaw_cos=math.cos(math.radians(yaw))
        line(self.cx2-roll_cos*self.R-scale*3,self.cy2-roll_sin*self.R,self.cx2+roll_cos*self.R-scale*3,self.cy2+roll_sin*self.R)
        line(self.cx2-pitch_cos*self.R-0,self.cy2-pitch_sin*self.R,self.cx2+pitch_cos*self.R-0,self.cy2+pitch_sin*self.R)
        line(self.cx2-yaw_cos*self.R+scale*3,self.cy2-yaw_sin*self.R,self.cx2+yaw_cos*self.R+scale*3,self.cy2+yaw_sin*self.R)

次に直方体をぐるぐるさせます。
そのため、まず、pitch、roll、とyawから回転マトリックスを作って、回転マトリックスとbox配列とで内積をとって、回転後の座標を計算します。

        #回転マトリックスの計算
        yawMatrix = np.matrix([[yaw_cos, -yaw_sin, 0],[yaw_sin, yaw_cos, 0],[0, 0, 1]])
        pitchMatrix = np.matrix([[pitch_cos, 0, pitch_sin],[0, 1, 0],[-pitch_sin, 0, pitch_cos]])
        rollMatrix = np.matrix([[1, 0, 0],[0, roll_cos, -roll_sin],[0, roll_sin, roll_cos]])
        R = yawMatrix * pitchMatrix * rollMatrix
        R=np.array(R)
        x_3d,y_3d,z_3d=np.transpose(np.dot(self.Box,R),(2,0,1))

求めた座標を使って、ワイヤフレームを描画します。
陰線処理をするため、z軸上の1番奥の頂点を求め、その頂点と結ばれていない辺だけを描画します。
なんちゃって陰線処理ですが、意外とうまくいきます。np.argmin関数が1個だけ返してくれるかとかいろいろ不安がありましたがまあ動きます。

        #陰線処理のため1番奥の頂点を特定
        zmin=np.argmin(z_3d)
        #奥の頂点を含んでいない辺を描画
        if zmin!=0 and zmin!=1 :
            line(self.cx+x_3d[0]*scale,self.cy+y_3d[0]*scale,self.cx+x_3d[1]*scale,self.cy+y_3d[1]*scale)
        ＃後は11本。頂点を変えながら、ベタに書いています。

最後にテキストデータを印字します。
tint(0,0,0,1)で文字の色、太さを指定し、text関数で、フォントや表示位置を指定して、データを印字していきます。

        #取得したmotionセンサのデータを画面上に印字
        tint(0,0,0,1)
        text('roll', font_name='Helvetica', font_size=16.0, x=self.cx2-scale*3, y=self.cy2+self.R+40, alignment=5)
        text(str(round(roll,2)), font_name='Helvetica', font_size=16.0, x=self.cx2-scale*3, y=self.cy2+self.R+20, alignment=5)

最後の最後に run(MyScene())で走らせればiPadの傾きに対応して直方体がグルグルします。
地磁気センサもあるので方位磁石を自分で作ったりできます。
陰線処理をせずに直方体を表示させるだけなら50行もいりません。

import motion
from scene import *
import math
import numpy as np
W,L,H=2,1,0.25
class MyScene (Scene):
    def setup(self):
        global scale
        scale=self.size.w/10
        motion.start_updates()
        self.R=scale
        self.Box=[[[W,L,-H]],[[-W,L,-H]],[[-W,-L,-H]],[[W,-L,-H]],[[W,L,H]],[[-W,L,H]],[[-W,-L,H]],[[W,-L,H]]]
        self.Box=np.array(self.Box)
    def draw(self):
        #get motion data
        pitch, roll, yaw =motion.get_attitude()
        #draw screen
        background(1, 1, 1)
        stroke(0,0,0)
        stroke_weight(1)
        roll_sin=math.sin(roll)
        roll_cos=math.cos(roll)
        pitch_sin=math.sin(pitch)
        pitch_cos=math.cos(pitch)
        yaw_sin=math.sin(yaw)
        yaw_cos=math.cos(yaw)
        yawMatrix = np.matrix([[yaw_cos, -yaw_sin, 0],[yaw_sin, yaw_cos, 0],[0, 0, 1]])
        pitchMatrix = np.matrix([[pitch_cos, 0, pitch_sin],[0, 1, 0],[-pitch_sin, 0, pitch_cos]])
        rollMatrix = np.matrix([[1, 0, 0],[0, roll_cos, -roll_sin],[0, roll_sin, roll_cos]])
        R = yawMatrix * pitchMatrix * rollMatrix
        R=np.array(R)
        x_3d,y_3d,z_3d=np.transpose(np.dot(self.Box,R),(2,0,1))
        self.cx =self.size.w * 0.5
        self.cy = self.size.h * 0.5
        line(self.cx+x_3d[0]*scale,self.cy+y_3d[0]*scale,self.cx+x_3d[1]*scale,self.cy+y_3d[1]*scale)
        line(self.cx+x_3d[1]*scale,self.cy+y_3d[1]*scale,self.cx+x_3d[2]*scale,self.cy+y_3d[2]*scale)
        line(self.cx+x_3d[2]*scale,self.cy+y_3d[2]*scale,self.cx+x_3d[3]*scale,self.cy+y_3d[3]*scale)
        line(self.cx+x_3d[3]*scale,self.cy+y_3d[3]*scale,self.cx+x_3d[0]*scale,self.cy+y_3d[0]*scale)
        line(self.cx+x_3d[4]*scale,self.cy+y_3d[4]*scale,self.cx+x_3d[5]*scale,self.cy+y_3d[5]*scale)
        line(self.cx+x_3d[5]*scale,self.cy+y_3d[5]*scale,self.cx+x_3d[6]*scale,self.cy+y_3d[6]*scale)
        line(self.cx+x_3d[6]*scale,self.cy+y_3d[6]*scale,self.cx+x_3d[7]*scale,self.cy+y_3d[7]*scale)
        line(self.cx+x_3d[7]*scale,self.cy+y_3d[7]*scale,self.cx+x_3d[4]*scale,self.cy+y_3d[4]*scale)
        line(self.cx+x_3d[0]*scale,self.cy+y_3d[0]*scale,self.cx+x_3d[4]*scale,self.cy+y_3d[4]*scale)
        line(self.cx+x_3d[1]*scale,self.cy+y_3d[1]*scale,self.cx+x_3d[5]*scale,self.cy+y_3d[5]*scale)
        line(self.cx+x_3d[2]*scale,self.cy+y_3d[2]*scale,self.cx+x_3d[6]*scale,self.cy+y_3d[6]*scale)
        line(self.cx+x_3d[3]*scale,self.cy+y_3d[3]*scale,self.cx+x_3d[7]*scale,self.cy+y_3d[7]*scale)
run(MyScene())

でもいちいち3次元処理を一から組み立てるのはしんどいので次はscenekitを使ってみます。

8
4
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
8
4