読者です 読者をやめる 読者になる 読者になる

たけし備忘録

自分の好奇心の赴くままに勉強メモ LL系が大好き Python bash Julia C

ボールの自由落下(衝突判定有り)

先日アップした の続きで、ボールの自由落下に衝突判定を加えたバージョンになっています。 物理シミュレーションは衝突判定ができてなんぼです
衝突判定があると見た目的にもすごく面白いです。
では以下よりコードと結果を見てみましょう。

目次

[FreeFall_addCollision.py]

# -*- coding:utf-8 -*-
import visual as vs
import numpy as np
import ode

vs.scene.center = (0, 2, 0)

"""  ODE環境の設定  """
# ODEワールドの作成 + ワールドの重力設定
world = ode.World()
world.setGravity(np.array((0, -9.81, 0)))

# 剛体の作成 + 質量特性の設定
Ball_Body = ode.Body(world) 
M = ode.Mass()              
M.setSphere(250.0, 0.5)   
Ball_Body.setMass(M)        
Ball_Body.setPosition((0, 4, 0))

# 衝突検出の設定
space = ode.Space()

Floor_Geom = ode.GeomPlane(space, (0, 1, 0), 0)
Ball_Geom = ode.GeomSphere(space, radius=0.5)
Ball_Geom.setBody(Ball_Body)
contactgroup = ode.JointGroup()

def Collision_Callback(args, geom1, geom2):
    contacts = ode.collide(geom1, geom2)
    world, contactgroup = args
    for c in contacts:
        c.setBounce(1) # 反発係数
        c.setMu(5000)  # クーロン摩擦係数
        j = ode.ContactJoint(world, contactgroup, c)
        j.attach(geom1.getBody(), geom2.getBody())


""" VPython環境の設定 """
Floor_VS = vs.box(length=4, height=0.2, width=4)
Floor_VS.pos = vs.vector(0, 0, 0)

Ball_VS = vs.sphere(pos=Ball_Body.getPosition(),
                       radius=0.5,
                       color=vs.color.red)

if __name__=="__main__":
    dt = 0.01
    total_time = 0.0
    n = 2
    while True:
        """
      for _ in xrange(2):
          space.collide((world, contactgroup), Collision_Callback)
          world.step(dt/n)
          contactgroup.empty()
      """

        space.collide((world, contactgroup), Collision_Callback)
        """ ODEによってVPythonのボールの位置を更新 """
        Ball_VS.pos =  Ball_Body.getPosition()
        vs.rate(1/dt)  # VPythonのアニメーションを進める
        world.step(dt)  # ODEの計算を進める
        contactgroup.empty()
        total_time += dt
    vs.exit()

f:id:takeshiD:20160605162804g:plain

今回追加した部分

今回追加した部分は衝突判定に関する部分のみです。
コードの中段あたりの

# 衝突検出の設定
space = ode.Space()

Floor_Geom = ode.GeomPlane(space, (0, 1, 0), 0)
Ball_Geom = ode.GeomSphere(space, radius=0.5)
Ball_Geom.setBody(Ball_Body)
contactgroup = ode.JointGroup()

def Collision_Callback(args, geom1, geom2):
    contacts = ode.collide(geom1, geom2)
    world, contactgroup = args
    for c in contacts:
        c.setBounce(1) # 反発係数
        c.setMu(5000)  # クーロン摩擦係数
        j = ode.ContactJoint(world, contactgroup, c)
        j.attach(geom1.getBody(), geom2.getBody())

の部分と mainの部分の

while True:
    space.collide((world, contactgroup), Collision_Callback)
    Ball_VS.pos =  Ball_Body.getPosition()
    vs.rate(1/dt)  # VPythonのアニメーションを進める
    world.step(dt)  # ODEの計算を進める
    contactgroup.empty()
    total_time += dt

の一部のみです。
他部分は前回と変わりません。
ではさっそく解説していきましょう。

コードの解説~衝突検出の設定~

# 衝突検出の設定
space = ode.Space()

Floor_Geom = ode.GeomPlane(space, (0, 1, 0), 0)
Ball_Geom = ode.GeomSphere(space, radius=0.5)
Ball_Geom.setBody(Ball_Body)
contactgroup = ode.JointGroup()

def Collision_Callback(args, geom1, geom2):
    contacts = ode.collide(geom1, geom2)
    world, contactgroup = args
    for c in contacts:
        c.setBounce(1) # 反発係数
        c.setMu(5000)  # クーロン摩擦係数
        j = ode.ContactJoint(world, contactgroup, c)
        j.attach(geom1.getBody(), geom2.getBody())

まずは衝突判定を管理する空間であるSpaceオブジェクトを作成します。

space = ode.Space()

この中に形状オブジェクトであるGeomオブジェクトを詰め込んでいきます。

Geomオブジェクト:床とボールの作成

今回は床とボールの2つのオブジェクトの衝突を考えるのでそれぞれFloor_GeomBall_Geomとしてオブジェクトを作りましょう。

Floor_Geom = ode.GeomPlane(space, (0, 1, 0), 0)
Ball_Geom = ode.GeomSphere(space, radius=0.5)

Ball_Geomはわかりやすいと思います。 ode.GeomSphere(space, radius=0.5)とすることで半径0.5の球状のGeomオブジェクトをspaceに作成しています。

一方でFloor_Geomは少々分かりにくいです。
Floor_Geomを生成している関数GeomPlaneは厚みの無い平面を作ります。 定義としては ode.GeomPlame(space, normal, dist)となっています。それぞれを見ていきましょう。
引数である(0, 1, 0)は平面の法線ベクトル、最後の0は平面の方程式の定数です。
数式で表すと

という平面の方程式で

という形で対応しています。
ですので今回のGeom_Plameは y =0 という平面になります。

GeomオブジェクトにBodyオブジェクトをセット + 接触グループの取得

Ball_Geom.setBody(Ball_Body)
contactgroup = ode.JointGroup()

Ball_GeomにBall_Bodyをセットします。
今回は床は動力学計算させないのでFloor_GeomにはBodyはセットしません。

次にcontactgroupですが、これは衝突するオブジェクトのグループを示すプロパティです。
衝突判定のコールバック関数の引数に用います。
とりあえず取っておけば問題無いと思えば大丈夫だと思います。

衝突検出のコールバック関数

衝突検出nためのコールバック関数について説明していきます。

def Collision_Callback(args, geom1, geom2):
    contacts = ode.collide(geom1, geom2)
    world, contactgroup = args
    for c in contacts:
        c.setBounce(1) # 反発係数
        c.setMu(5000)  # クーロン摩擦係数
        j = ode.ContactJoint(world, contactgroup, c)
        j.attach(geom1.getBody(), geom2.getBody())

まず、ode.collide(geom1, geom2)によってgeom1とgeom2が衝突しているかの判定をします。
ode.collidespace.collideとは異なる関数ですので注意してください。
ode.collide(geom1, geom2)ode.Contactオブジェクトのリストを返します(衝突しない場合は空のリストを返します)。
Contactオブジェクトは動力学計算の中で使われる情報を全て含んでいます。例えば接触した座標、接触方向を示す法線ベクトルなどの位置情報に加えて摩擦係数、弾性係数などを含んでいます。

world, contactgroupについては、特に中身をいじることはしないのでとりあえず引数に入れておけば大丈夫です。

次にforループの中を見ていきましょう。

for c in contacts:
    c.setBounce(1) # 反発係数
    c.setMu(5000)  # クーロン摩擦係数
    j = ode.ContactJoint(world, contactgroup, c)
    j.attach(geom1.getBody(), geom2.getBody())

contactsはConatctオブジェクトのリストですので、cはContactオブジェクトです。
Conatctオブジェクトのメソッドなどは ode.Contact を見ればなんとなく分かるかと思います。

c.setBouceは、反発係数を設定します。反発係数は0~1で設定されますのでその範囲で設定します。今回は1ですので完全弾性衝突となります。

c.setMuは、静止摩擦係数μです。今回、μは5000ですので静止摩擦力Fは、垂直抗力Nに対してF=μNとなりますので、1[N]の垂直抗力しかなくとも5000[N]の静止摩擦力がかかることになります。もうほぼ動かないですね。
ちなみに動摩擦係数は_c.setSlip1で設定するようです。

次に

j = ode.ContactJoint(world, contactgroup, c)
j.attach(geom1.getBody(), geom2.getBody())

の部分についてです。

world, contactgroupについては無視します。
ode.ContactJointによって、ode.collideで取得したContactオブジェクトから接触ジョイントという衝突しているオブジェクトを結ぶジョイントを取得します。
j.attachによって接触ジョイントの両端につなぐBodyを接続します。

このような手続きをcontactsに入っている接触している点全てで行います。

コードの解説~シミュレーションループで衝突計算を反映させる~

while True:
    space.collide((world, contactgroup), Collision_Callback)
    Ball_VS.pos =  Ball_Body.getPosition()
    vs.rate(1/dt)  # VPythonのアニメーションを進める
    world.step(dt)  # ODEの計算を進める
    contactgroup.empty()

space.collideによって衝突判定を行います。
コールバック関数Collision_Callbackによって内部で、衝突しているGeomオブジェクトのBodyに位置情報などが渡され更新されます。
その後は通常と同じくVPythonのボールの位置を更新していきます。

最後にcontactgroup.empty()をしなければいけません。
これを行うことで、各ステップごとで衝突しているオブジェクトの情報が更新されます。
emptyしない場合、衝突計算が最初の状態でずっと続くのでおかしなことになってしまいます。