使用している機能

本コードは、以下の機能を使用して「マウスでクリックした物体のオブジェクトを変更する機能」を実現しています。

レイ機能
色の変更
マウスイベントの取得

レイ機能

pybulletでは、rayTest関数を使用することで「レイキャスト」を実施することが出来ます。

hitResult = pybullet.rayTest(rayFrom, RayTo)

rayFrom：レイキャストの開始位置([x, y, z])
rayTo：レイキャストの終点（[x, y, z]）

レイキャストとは、下図のように「ある地点から特定の方向に線を引き、その線上に障害物があるかどうかを検出する」ものになります。

今回は、マウスでクリックした位置にレイキャストし、レイが衝突しているかどうかをチェックしています。

色の変更

pybulletでは、changeVisualShape関数を使用することで指定した物体の、指定したリンクの色を変更することができます。

pybullet.changeVisualShape(objectId, linkIdx, rgbaColor=color)

objectId：オブジェクトのID
linkIdx：リンクのインデックス（ベースリンクの場合、-1を指定）
rgbaColor：色を[赤、緑、青、透明度]のリストで指定

今回の場合、アヒルのオブジェクトの色を変更するのに使用しています（アヒルのオブジェクトはリンクが1つで定義されているので、オブジェクト全体の色が変更されます）。

マウスイベントの取得

pybulletでは、getMouseEvents関数を使用することでマウスのイベントを取得することができます

mouseEvents = pybullet.getMouseEvents()

今回は、左クリックされたかどうかを検出しています。

コメントをつけたサンプルコード

サンプルコードにコメントをつけたものが以下になります（もともとあった不要と思われるコメントについては削除しています）

import pybullet as p
import time
import math
import pybullet_data

# マウスのX座標とY座標を受け取り、始点と終点を計算する
def getRayFromTo(mouseX, mouseY):
  # 現在のカメラの情報（画面の幅と高さ、ビュー行列と投影行列、カメラの上方向ベクトル、前方向ベクトル、水平方向ベクトル、垂直方向ベクトル、カメラの距離、カメラのターゲット位置)
  width, height, viewMat, projMat, cameraUp, camForward, horizon, vertical, _, _, dist, camTarget = p.getDebugVisualizerCamera(
  )

  # getDebugVisualizerCameraで取得した情報からカメラの位置を計算
  camPos = [
      camTarget[0] - dist * camForward[0], camTarget[1] - dist * camForward[1],
      camTarget[2] - dist * camForward[2]
  ]

  # レイの最大距離を計算
  farPlane = 10000

  # レイの前方向ベクトルを計算。具体的には、カメラのターゲット位置からカメラ位置を引いたもの。
  rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]), (camTarget[2] - camPos[2])]

  # レイの前方向ベクトルの長さの逆数
  invLen = farPlane * 1. / (math.sqrt(rayForward[0] * rayForward[0] + rayForward[1] *
                                      rayForward[1] + rayForward[2] * rayForward[2]))

  # レイの前方向のベクトルを正規化
  rayForward = [invLen * rayForward[0], invLen * rayForward[1], invLen * rayForward[2]]

  # カメラ位置をレイの始点とする
  rayFrom = camPos

  # 画面の幅、高さの逆数を計算
  oneOverWidth = float(1) / float(width)
  oneOverHeight = float(1) / float(height)

  # 水平方向と垂直方向のベクトルを画面の幅と高さで正規化
  dHor = [horizon[0] * oneOverWidth, horizon[1] * oneOverWidth, horizon[2] * oneOverWidth]
  dVer = [vertical[0] * oneOverHeight, vertical[1] * oneOverHeight, vertical[2] * oneOverHeight]

  # レイの中心点を計算
  rayToCenter = [
      rayFrom[0] + rayForward[0], rayFrom[1] + rayForward[1], rayFrom[2] + rayForward[2]
  ]

  # マウスの位置に基づいてレイの終点を計算
  rayTo = [
      rayFrom[0] + rayForward[0] - 0.5 * horizon[0] + 0.5 * vertical[0] + float(mouseX) * dHor[0] -
      float(mouseY) * dVer[0], rayFrom[1] + rayForward[1] - 0.5 * horizon[1] + 0.5 * vertical[1] +
      float(mouseX) * dHor[1] - float(mouseY) * dVer[1], rayFrom[2] + rayForward[2] -
      0.5 * horizon[2] + 0.5 * vertical[2] + float(mouseX) * dHor[2] - float(mouseY) * dVer[2]
  ]
  return rayFrom, rayTo

# PyBulletにGUIモードで接続
cid = p.connect(p.GUI)

# 接続が失敗した場合、再度接続を試みる
if (cid < 0):
  p.connect(p.GUI)

# Pybulletのデータパスを設定
p.setAdditionalSearchPath(pybullet_data.getDataPath())

# 物理エンジンのパラメータを設定。この場合、ソルバーの反復回数を10回に設定
p.setPhysicsEngineParameter(numSolverIterations=10)

# シミュレーションが1ステップあたりに進む秒数を1/120秒に設定
p.setTimeStep(1. / 120.)

# 状態のログを開始し、プロファイリングのタイミングを"visualShapeBench.json"というファイルに記録
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json")

# plane_transparent.urdf をロード
p.loadURDF("plane_transparent.urdf", useMaximalCoordinates=True)

# デバッグビジュアライザの設定を行う（レンダリングを無効化、平面反射を有効化、GUIを無効化、ソフトウェアレンダラーを無効化）
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION, 1)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)

# メッシュのシフトとスケールを設定
shift = [0, -0.02, 0]
meshScale = [0.1, 0.1, 0.1]

# アヒルのメッシュオブジェクトのビジュアル形状を作成
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
                                    fileName="duck.obj",
                                    rgbaColor=[1, 1, 1, 1],
                                    specularColor=[0.4, .4, 0],
                                    visualFramePosition=shift,
                                    meshScale=meshScale)

# アヒルのメッシュオブジェクトの衝突形状を作成
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH,
                                          fileName="duck_vhacd.obj",
                                          collisionFramePosition=shift,
                                          meshScale=meshScale)

# オブジェクトを配置する範囲を設定
rangex = 3
rangey = 3

# 複数のアヒルオブジェクトを配置
for i in range(rangex):
  for j in range(rangey):
    p.createMultiBody(baseMass=1,
                      baseInertialFramePosition=[0, 0, 0],
                      baseCollisionShapeIndex=collisionShapeId,
                      baseVisualShapeIndex=visualShapeId,
                      basePosition=[((-rangex / 2) + i) * meshScale[0] * 2,
                                    (-rangey / 2 + j) * meshScale[1] * 2, 1],
                      useMaximalCoordinates=True)

# レンダリングを有効化
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)

# 状態のログ記録を停止
p.stopStateLogging(logId)

# 重力を設定
p.setGravity(0, 0, -10)

# リアルタイムシミュレーションを有効化
p.setRealTimeSimulation(1)

# 色のリストを定義し、色のインデックスを0に設定
colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]]
currentColor = 0

while (1):
  # デバッグビジュアライザのカメラ情報を取得
  p.getDebugVisualizerCamera()

  # マウスイベントを取得
  mouseEvents = p.getMouseEvents()
  
  for e in mouseEvents:

    # マウスの左クリックイベントをチェック
    if ((e[0] == 2) and (e[3] == 0) and (e[4] & p.KEY_WAS_TRIGGERED)):
      
      # マウスのX座標とY座標を取得
      mouseX = e[1]
      mouseY = e[2]

      # getRayFromTo 関数を使用してレイの始点と終点を計算
      rayFrom, rayTo = getRayFromTo(mouseX, mouseY)

      # レイキャストを実行し、レイがどのオブジェクトに当たるかをチェック
      rayInfo = p.rayTest(rayFrom, rayTo)

      # レイが当たったオブジェクトの色を変更します。色は定義されたリストから順に変更され、すべての色を使い切ったら最初の色に戻る
      for l in range(len(rayInfo)):
        hit = rayInfo[l]
        objectUid = hit[0]
        if (objectUid >= 1):
          p.changeVisualShape(objectUid, -1, rgbaColor=colors[currentColor])
          currentColor += 1
          if (currentColor >= len(colors)):
            currentColor = 0