Va = (xa, ya, za)

Vb = (xb, yb, zb)

A1 = (cos(θ/2); xa * sin(θ/2), ya * sin(θ/2), za * sin(θ/2))
A2 = (cos(θ/2); -xa * sin(θ/2), -ya * sin(θ/2), -za * sin(θ/2))

C2 * P * C1

Vector position; //親座標系でのボーンの位置
float boneLong; //ボーンベクトル（XVector）の長さ

Vector XVector; //親座標系で見たボーン（子座標系でのx軸のこと）
Vector YVector; //親座標系で見たボーンの法線（子座標系でのy軸のこと）

Vector parentXVector; //このボーンの親座標のX軸上のベクトル
Vector parentYVector; //このボーンの親座標のY軸上のベクトル

Quaternion boneCood; //親座標をボーンの座標系に変換するクォータニオン（初期状態を描画するときに使うクォータニオン）
Vector boneVector; //ボーンのベクトル
Vector axisVector; //ボーンを回転するときに、回転軸となるベクトル

Quaternion allQua, callQua; //一番上の座標系から操作している座標系までを合成したクォータニオン（初期状態の姿勢のクォータニオン）
Quaternion qRot, cqRot; //各座標系にさらに回転を加えるためのクォータニオン
int boneDataCnt = 3; //ボーンのデータ数
int i;

for (i = 0; i < boneDataCnt; i++)
{

//一番最初に描画するボーンの時
if (i == 0)
{

//ボーン0の親座標系でのX軸上のベクトル（ボーンのベクトル）
b[i].parentXVector.x = 1.0;
b[i].parentXVector.y = 0.0;
b[i].parentXVector.z = 0.0;

//親座標系のY軸上にあるベクトル
b[i].parentYVector = GetVerticalVector(b[i].parentXVector);

//ボーン0の位置を決めておく
b[i].position.x = 0.3;
b[i].position.y = 0.3;
b[i].position.z = 0.3;

//親座標系からみた子座標系のx軸（ボーンの方向）　XVector長さをボーンの長さとする
b[i].XVector.x = 1.0;
b[i].XVector.y = 1.0;
b[i].XVector.z = 1.0;

}
else
{

//ボーン1つ前のボーンの先の位置を次のボーンの座標系の原点
b[i].position = b[i-1].boneVector;

//親座標系からみた子座標系のx軸（変換前）
b[i].XVector.x = 1.0;
b[i].XVector.y = 0.0;
b[i].XVector.z = 0.0;

//変換後。これが実際の親座標系からみた子座標系のx軸
b[i].XVector = QuaternionToVector( MultiplyQuaternion( MultiplyQuaternion(callQua, VectorToQuaternion(b[i].XVector) ), allQua) );

//1つ前のボーンの座標系が親座標系になる（元々の座標系から一気に変換する）
b[i].parentXVector = QuaternionToVector(MultiplyQuaternion( MultiplyQuaternion( callQua, VectorToQuaternion(b[0].parentXVector) ), allQua ));
b[i].parentYVector = QuaternionToVector(MultiplyQuaternion( MultiplyQuaternion( callQua, VectorToQuaternion(b[0].parentYVector) ), allQua ));

}

//ボーンの長さを取得
b[i].boneLong = VectorLong(b[i].XVector);
//親座標系からみた子座標系のy軸（これをボーンの法線とする）
b[i].YVector = GetVerticalVector(b[i].XVector); //ボーンのベクトルに対するXY平面上の法線を取得
//とりあえず、各座標系のY軸周りの回転だけを行うとする(ボーンの回転制御時に使用）
b[i].axisVector = NormalizeVector(b[i].YVector);

//描きたいボーンの座標系が親座標と同じ座標系になる場合、このボーンの座標系＝親座標系ってことなので単位クォータニオンにする
if (b[i].parentXVector.x == b[i].XVector.x && b[i].parentXVector.y == b[i].XVector.y && b[i].parentXVector.z == b[i].XVector.z)
{

b[i].boneCood.t = 1.0;
b[i].boneCood.x = 0.0;
b[i].boneCood.y = 0.0;
b[i].boneCood.z = 0.0;

}
else {

//親座標系から子座標系へ変換するクォータニオンを作成
b[i].boneCood = QuaternionTransChildCoodinate(b[i].parentXVector, b[i].parentYVector, b[i].position, b[i].XVector, b[i].YVector);

}

if (i == 0)
{

//allQua, callQuaをボーン0の座標系に変換するクォータニオンで初期化
allQua = b[i].boneCood;
callQua = QuaternionConjugate(b[i].boneCood);

}
else
{

//作成したクォータニオンを、ボーン0の親座標からb[i-1]の座標系へ変換するクォータニオンと合成（ボーン0の親座標系→b[i]の座標系への変換）
allQua = MultiplyQuaternion(allQua, b[i].boneCood);
callQua = QuaternionConjugate(allQua);

}

//描画用のボーンのデータを作成
b[i].boneVector.x = b[i].boneLong;
b[i].boneVector.y = 0.0;
b[i].boneVector.z = 0.0;

//ボーンの先頭位置を計算。これが次のボーンの座標系の原点となる
b[i].boneVector = QuaternionToVector( MultiplyQuaternion( MultiplyQuaternion( callQua, VectorToQuaternion(b[i].boneVector) ), allQua ) );

}

return 0;

Vector parentXVector, //親座標系のX軸となっているベクトル
Vector parentYVector, //親座標系のY軸となっているベクトル
Vector childPosition, //親座標系から見た時の子座標系の原点となる点
Vector childXVector, //親座標系からみた、子座標系のX軸
Vector childYVector //親座標系からみた、子座標系のY軸

//引数を正規化したもの
Vector nparentXVector, nparentYVector, nchildXVector, nchildYVector;

Vector parentX, parentY; //親座標系のX軸・Y軸のベクトル
Vector axis; //クォータニオンの回転の軸になるベクトル
Vector v_transY; //回転後Y軸
float angle; //回転する角度
Quaternion q_parentX, q_parentY; //parentX, parentYをクォータニオンに置き換えたもの（実部の0を加えただけ）
Quaternion qx; //親座標系のX軸を子座標系のX軸に合わせるクォータニオン
Quaternion qy; //親座標系のY軸を子座標系のY軸に合わせるクォータニオン
Quaternion cq; //共役のクォータニオンを格納する用
Quaternion transX, transY; //qxを使って回転した後の親座標系のX軸・Y軸

//引数のベクトルを正規化
nparentXVector = NormalizeVector(parentXVector);
nparentYVector = NormalizeVector(parentYVector);
nchildXVector = NormalizeVector(childXVector);
nchildYVector = NormalizeVector(childYVector);

//子座標系の原点に移動
glTranslatef(childPosition.x, childPosition.y, childPosition.z);

//親座標系X軸のベクトル
parentX.x = nparentXVector.x;
parentX.y = nparentXVector.y;
parentX.z = nparentXVector.z;

//親座標系Y軸のベクトル
parentY.x = nparentYVector.x;
parentY.y = nparentYVector.y;
parentY.z = nparentYVector.z;

axis = outerProduct(parentX, nchildXVector); //外積で回転軸を求める
angle = getAngleFromInnerProduct(parentX, nchildXVector); //内積から角度を求める

axis = NormalizeVector(axis);

qx = QuaternionRotationAxis(axis, angle); //親座標系X軸を、子座標系のX軸に合わせるクォータニオンを作成する
cq = QuaternionConjugate(qx); //共役のクォータニオンの作成

//親座標系X軸をクォータニオンに置き換え
q_parentX.t = 0.0;
q_parentX.x = parentX.x;
q_parentX.y = parentX.y;
q_parentX.z = parentX.z;

//qxで親座標系のX軸を回転
transX = MultiplyQuaternion(cq, q_parentX);
transX = MultiplyQuaternion(transX, qx);

//親座標系Y軸をクォータニオンに置き換え
q_parentY.t = 0.0;
q_parentY.x = parentY.x;
q_parentY.y = parentY.y;
q_parentY.z = parentY.z;

//qxで親座標系のY軸を回転
transY = MultiplyQuaternion(cq, q_parentY);
transY = MultiplyQuaternion(transY, qx);

//クォータニオンをベクトルにする
v_transY.x = transY.x;
v_transY.y = transY.y;
v_transY.z = transY.z;

//変換されたY軸と、親座標系からみた子座標系のY軸の角度差を計算
angle = getAngleFromInnerProduct(v_transY, nchildYVector);

//回転軸を子座標系のX軸にする
axis.x = transX.x;
axis.y = transX.y;
axis.z = transX.z;

axis = NormalizeVector(axis);

//qxでの回転後Y軸と、子座標系のY軸を合わせるクォータニオンを作成
qy = QuaternionRotationAxis(axis, angle);

//x軸とy軸を子座標系に合わせるクォータニオンを作成し、それを返す
return MultiplyQuaternion(qx, qy);