コード例 #1
0
ファイル: Matrix3D.cpp プロジェクト: SimoneNardi/base_math
// Returns a rotation matrix whose third column is equal to the given vector.
Matrix3D Matrix3D::rotationMatrixFromZAxis(const Real3D& dir)
{
	// Normalize the target direction
	// If it is degenerate, returns the identity matrix
	Real3D target = dir;
	REAL len = target.mod();
	if (len == 0) return Matrix3D(1);
	target /= len;
	if (!target.isfinite()) return Matrix3D(1);
	// Find a non-degenerate vector orthogonal to the target
	Real3D v1 = Real3D(1,0,0) ^ target;
	REAL l1 = v1.sqrmod();
	Real3D v2 = Real3D(0,1,0) ^ target;
	REAL l2 = v2.sqrmod();
	Real3D v3 = Real3D(0,0,1) ^ target;
	REAL l3 = v3.sqrmod();
	// Choose the largest among v1,v2,v3
	Real3D v;
	if (l1 >= l2 && l1 >= l3) v = v1.vers();
	else if (l2 >= l1 && l2 >= l3) v = v2.vers();
	else /* if (l3 >= l1 && l3 >= l2) */ v = v3.vers();
	// Build the rotation matrix
	Real3D w = (target^v).vers();
	Matrix3D rot;
	rot.setCols(v, w, target);
	// Debug Check
	assert(fabs(rot.det()-1) < Math::TOLERANCE);
	return rot;
}
コード例 #2
0
ファイル: Matrix3D.cpp プロジェクト: SimoneNardi/base_math
/// Returns a rotation matrix whose third column is equal to the given vector.
Matrix3D Matrix3D::rotationMatrixFromZAxisForDXF(const Real3D& dir)
{
	// Normalize the target direction
	// If it is degenerate, returns the identity matrix
	Real3D target = dir;
	REAL len = target.mod();
	if (len == 0) return Matrix3D(1);
	target /= len;
	if (!target.isfinite()) return Matrix3D(1);

	// Choice of xAxis: see AutoCAD documentation.
	Real3D v;
	if( fabs(target[0])*64 < 1 && fabs(target[1])*64 < 1 )
		v = Real3D(0,1,0) ^ target;
	else
		v = Real3D(0,0,1) ^ target;

	v = v.vers();
	Real3D w = (target^v).vers();
	Matrix3D rot;
	rot.setCols(v, w, target);
	// Debug Check
	assert(fabs(rot.det()-1) < Math::TOLERANCE);
	return rot;
}