PythonとOpenCVでアッカーマンステアリング機構のシミュレーション

Python OpenCV 力学

色々あってアッカーマンステアリング機構を学ばなければいけなくなった。
ググったら色々出てきた。
為になったサイト
http://www.geocities.jp/bonzo_hp/Ackerman.htm　アッカーマンステアリング機構について参考になった。
wikipedia:ヴィルヘルム・アッカーマン 　　　　「アッカーマン」の綴りが分かった。
復習も兼ねてPythonとOpenCVを使用してアッカーマンステアリング機構のシミュレータを書いてみた。
以下がそのコード
これに関することは自己責任でおねがいします。

#!/usr/bin/env python
#coding: utf-8

from math import *
from opencv.cv import *
from opencv.highgui import *


wWidth		= 450		# ウィンドウの幅
wHeight		= 300		# 　　　　　　高さ
wName		= 'Ackermann'	# 　　　　　　名前
wTrackbarName 	= 'mTheta1'	# トラックバーの名前
mLength		= 150		# 機体の長さ
mWidth		= 100		# 　　　幅
#mTheta1	= 0.0		# 内輪(右上の車輪)の角度
#mTheta2	= 0.0		# 外輪(左上の車輪)の角度
mTireR	= 20			# 車輪の半径
mPoint	= cvPoint(100, 100)	# 機体の左上の絶対座標

map = cvCreateImage(cvSize(wWidth, wHeight), IPL_DEPTH_8U, 3)

# 外輪の角度を返却
def get_mTheta2(mTheta1):
	tan_theta1 = tan(mTheta1)
	return atan((mLength * tan_theta1) / (mWidth * tan_theta1 + mLength))


# 機体を描画
def DrawMachine(mTheta1 = 0.0, mTheta2 = 0.0):
	cvSetZero(map)
	# フレームを描画
	cvRectangle(map, mPoint, cvPoint(mPoint.x + mWidth, mPoint.y + mLength), CV_RGB(255, 255, 255))
	# タイヤの描画
	cvLine(map, 
		cvPoint(int(mPoint.x + mTireR * sin(mTheta2)), int(mPoint.y - mTireR * cos(mTheta2))), 
		cvPoint(int(mPoint.x - mTireR * sin(mTheta2)), int(mPoint.y + mTireR * cos(mTheta2))), 
		CV_RGB(255, 0, 0), 3)	# 左上
	cvLine(map, 
		cvPoint(int(mPoint.x + mWidth + mTireR * sin(mTheta1)), int(mPoint.y - mTireR * cos(mTheta1))), 
		cvPoint(int(mPoint.x + mWidth - mTireR * sin(mTheta1)), int(mPoint.y + mTireR * cos(mTheta1))), 
		CV_RGB(255, 0, 0), 3)	# 右上
	cvLine(map, 
		cvPoint(mPoint.x, mPoint.y + mLength - mTireR), 
		cvPoint(mPoint.x, mPoint.y + mLength + mTireR), 
		CV_RGB(255, 0, 0), 3)	# 左下
	cvLine(map, 
		cvPoint(mPoint.x + mWidth, mPoint.y + mLength - mTireR), 
		cvPoint(mPoint.x + mWidth, mPoint.y + mLength + mTireR), 
		CV_RGB(255, 0, 0), 3)	# 右下


# 補助線を描画
def DrawAdditionalLines(mTheta1 = 0.0, mTheta2 = 0.0):
	# 後輪の補助線
	cvLine(map, 
		cvPoint(mPoint.x, mPoint.y + mLength), 
		cvPoint(wWidth, mPoint.y + mLength), 
		CV_RGB(0, 255, 0))
	# 前輪の補助線
	r = wWidth * 2	# 補助線の長さ
	mTheta1 += pi / 2
	mTheta2 += pi / 2
	cvLine(map, 
		mPoint,
		cvPoint(int(mPoint.x + r * sin(mTheta2)), int(mPoint.y - r * cos(mTheta2))),
		CV_RGB(0, 255, 0))	# 左上車輪
	cvLine(map,
		cvPoint(mPoint.x + mWidth, mPoint.y),
		cvPoint(int(mPoint.x + mWidth + r * sin(mTheta1)), int(mPoint.y - r * cos(mTheta1))),
		CV_RGB(0, 255, 0))	# 右上車輪

			
# トラックバーのコールバック関数
def wOnChange(pos):
	mTheta1 = pos * (pi / 180)
	mTheta2 = get_mTheta2(mTheta1)
	cvSetZero(map)
	DrawMachine(mTheta1, mTheta2)
	DrawAdditionalLines(mTheta1, mTheta2)
	cvShowImage(wName, map)


def main():
	cvSetZero(map)
	cvNamedWindow(wName)
	cvCreateTrackbar(wTrackbarName, wName, 0, 90, wOnChange)
	DrawMachine()
	DrawAdditionalLines()
	cvShowImage(wName, map)
	cvWaitKey()

if __name__=='__main__':
	main()

機体が向いている方向から見た内輪の角度mTheta1をトラックバーで0度から90度まで選べられるようにした。
mTheata1から、外輪の角度mTheta2を計算させた。
これらの値から車輪と直交する補助線を描画させた。
下が実行画面

外輪と内輪の補助線が後輪の補助線上で交わった。