2010-04-23

bulletのbtTransformが扱うのは剛体変換で内部に3x3の回転行列と３次元ベクトルを持つ。
bulletは縦ベクトルモデルのようなので区分行列で書くと
$\left(\begin{array}{c|c} R & T \\ \hline 0 &1 \\ \end{array}\right)$
となりRが回転行列でTが移動要素となる。
$\left(\begin{array}{c|c} R_{0} & T_{0} \\ \hline 0 &1 \\ \end{array}\right)\left(\begin{array}{c|c} R_{1} & T_{1} \\ \hline 0 & 1 \\ \end{array}\right) = \left(\begin{array}{c|c} R_{0}R_{1} & T_{0}+R_{0}T_{1} \\ \hline 0 &1 \\ \end{array}\right)$
なので
bulletのソースsrc/LinearMath/btTransform.h

  /**@brief Multiply this Transform by another(this = this * another) 
   * @param t The other transform */
	btTransform& operator*=(const btTransform& t) 
	{
		m_origin += m_basis * t.m_origin;
		m_basis *= t.m_basis;
		return *this;
	}

の内容と一致する(basisが回転でoriginが移動)。

一方、IrrlichtでbtTransformに対応するのはcore::matrix4で4x4の変換行列。
btTransformからcore::matrix4を作るには以下のようにできた。

static core::matrix4 getMatrix(const btTransform &t)
{
	core::matrix4 m;

	float m16[16];
	t.getOpenGLMatrix(m16);
	m[0]=m16[0]; m[1]=m16[1]; m[2]=m16[2]; m[3]=m16[3];
	m[4]=m16[4]; m[5]=m16[5]; m[6]=m16[6]; m[7]=m16[7];
	m[8]=m16[8]; m[9]=m16[9]; m[10]=m16[10]; m[11]=m16[11];
	m[12]=m16[12]; m[13]=m16[13]; m[14]=m16[14]; m[15]=m16[15];

	return m;
}

逆に、core::matrix4からbtTransformを作るには

static btTransform getTransform(const core::matrix4 &m)
{
core::vector3 irrRot=m.getRotationDegrees(); // オイラー角
core::vector3 irrPos=m.getTranslation(); // 12, 13, 14

btMatrix3x3 rotation;
// ここでbtQuaternionを使うと結果が変わってしまうようだ？
rotation.setEulerZYX(toRadian(irrRot.X), toRadian(irrRot.Y), toRadian(irrRot.Z));

btVector3 translation(irrPos.X, irrPos.Y, irrPos.Z);

return btTransform(rotation, translation);
}

という感じのようだ。

google testのユニットテストでの辻褄あわせに使ったコード

#include <irrlicht.h>
#include <btBulletDynamicsCommon.h>
#define _USE_MATH_DEFINES
#include <math.h>
#include <gtest/gtest.h>
#include <iostream>

using namespace irr;

inline double toRadian(double degree)
{
	return M_PI * degree /180;
}

inline std::ostream& operator<<(std::ostream &os, const core::matrix4 &m)
{
	return os
		<< "[" << m[0] << ',' << m[1] << ',' << m[2] << "]" << std::endl
		<< "[" << m[4] << ',' << m[5] << ',' << m[6] << "]" << std::endl
		<< "[" << m[8] << ',' << m[9] << ',' << m[10] << "]" << std::endl
		;
}

inline std::ostream& operator<<(std::ostream &os, const btMatrix3x3 &m)
{
	return os
		<< "[" << m[0][0] << ',' << m[0][1] << ',' << m[0][2] << "]" << std::endl
		<< "[" << m[1][0] << ',' << m[1][1] << ',' << m[1][2] << "]" << std::endl
		<< "[" << m[2][0] << ',' << m[2][1] << ',' << m[2][2] << "]" << std::endl
		;
}

TEST(MathTest, Irr){
	static const float D1=42;
	static const float D3=133;

	////////////////////////////////////////////////////////////
	// irr
	////////////////////////////////////////////////////////////
	core::matrix4 irr_t0;
	// translate(1, 0, 0)
	irr_t0[12]=1; 

	core::matrix4 irr_t1;
	// rotation z axis 90 degree
	irr_t1.setRotationDegrees(core::vector3df(0, 0, D1));

	core::matrix4 irr_t2;
	// translate(1, 2, 3)
	irr_t2[12]=1;
	irr_t2[13]=2;
	irr_t2[14]=3;

	core::matrix4 irr_t3;
	// rotation y axis 90 degree
	irr_t3.setRotationDegrees(core::vector3df(0, D3, 0));

	core::matrix4 irr_t=irr_t3 * irr_t2 * irr_t1 * irr_t0;

	////////////////////////////////////////////////////////////
	// bullet
	////////////////////////////////////////////////////////////
	btTransform bt_t0(btMatrix3x3::getIdentity(), btVector3(1, 0, 0));

	btMatrix3x3 bt_r1;
	bt_r1.setEulerZYX(0, 0, static_cast<float>(toRadian(D1)));
	btTransform bt_t1(bt_r1, btVector3(1, 2, 3));

	btMatrix3x3 bt_r3;
	bt_r3.setEulerZYX(0, static_cast<float>(toRadian(D3)), 0);
	btTransform bt_t3(bt_r3);
	btTransform bt_t=bt_t3 * bt_t1 * bt_t0;

	////////////////////////////////////////////////////////////
	// compare
	btMatrix3x3 r=bt_t.getBasis();
	std::cout.setf(std::ios::fixed);
	std::cout << "## irr ##" << std::endl << irr_t;
	std::cout << "## bullet ##" << std::endl << r;

	// compare
	f32 m16[16];
	bt_t.getOpenGLMatrix(m16);

	EXPECT_FLOAT_EQ(irr_t[0], m16[0]);
	EXPECT_FLOAT_EQ(irr_t[1], m16[1]);
	EXPECT_FLOAT_EQ(irr_t[2], m16[2]);
	EXPECT_FLOAT_EQ(irr_t[3], m16[3]);
	EXPECT_FLOAT_EQ(irr_t[4], m16[4]);
	EXPECT_FLOAT_EQ(irr_t[5], m16[5]);
	EXPECT_FLOAT_EQ(irr_t[6], m16[6]);
	EXPECT_FLOAT_EQ(irr_t[7], m16[7]);
	EXPECT_FLOAT_EQ(irr_t[8], m16[8]);
	EXPECT_FLOAT_EQ(irr_t[9], m16[9]);
	EXPECT_FLOAT_EQ(irr_t[10], m16[10]);
	EXPECT_FLOAT_EQ(irr_t[11], m16[11]);
	EXPECT_FLOAT_EQ(irr_t[12], m16[12]);
	EXPECT_FLOAT_EQ(irr_t[13], m16[13]);
	EXPECT_FLOAT_EQ(irr_t[14], m16[14]);
	EXPECT_FLOAT_EQ(irr_t[15], m16[15]);
}

int main(int argc, char **argv) {
	testing::InitGoogleTest(&argc, argv);
	return RUN_ALL_TESTS();
}