ベクトル.任意の軸周りの回転 - musaka @ ウィキ

回転軸ベクトル(unit_x,unit_y,unit_z) は単位ベクトルでなければいけない。
回転軸ベクトルの方向を右ねじの進行方向としたときの回転方向を正の方向にとる。

---

void RotateVectorByUnitVector(
double radian,
double target_x,double target_y,double target_z,
double unit_x,double unit_y,double unit_z,
double *rotated_x,double *rotated_y,double *rotated_z)
{
	Point3D<double> vertical,horizontal,normal;
	
	double theta = GetCosFromVector(
	unit_x, unit_y, unit_z,
	target_x, target_y, target_z);
	
	if (theta > 1) { theta = 1; dprintf(TEXT(" theta > 1")); }
	if (theta < -1) { theta = -1; dprintf(TEXT(" theta < -1")); }
	double length = Distance3D(unit_x, unit_y, unit_z);
	
	length = Distance3D(target_x, target_y, target_z);
	
	theta = acos(theta);
	vertical.x = cos(theta) * length * unit_x;
	vertical.y = cos(theta) * length * unit_y;
	vertical.z = cos(theta) * length * unit_z;
	length = Distance3D(vertical.x, vertical.y, vertical.z);
	
	horizontal.x = target_x - vertical.x;
	horizontal.y = target_y - vertical.y;
	horizontal.z = target_z - vertical.z;
	
	length = Distance3D(horizontal.x, horizontal.y, horizontal.z);
	
	//水平成分のベクトルと直交する　それと同じ長さのベクトルを作る
	CrossProduct(unit_x, unit_y, unit_z, horizontal.x, horizontal.y, horizontal.z, &normal.x,&normal.y,&normal.z);
	length = Distance3D(normal.x, normal.y, normal.z);
	
	*rotated_x =cos(radian) * horizontal.x + sin(radian) * normal.x + vertical.x;//水平成分の回転後、垂直成分を加える
	*rotated_y =cos(radian) * horizontal.y + sin(radian) * normal.y + vertical.y;
	*rotated_z =cos(radian) * horizontal.z + sin(radian) * normal.z + vertical.z;
}