VRlabBuaa

void extractRotation(const Matrix3d &A, Quaterniond &q,
	const unsigned int maxIter)
{
	for (unsigned int iter = 0; iter < maxIter; iter++)
	{
		Matrix3d R = q.matrix();
		Vector3d omega = (R.col(0).cross(A.col(0)) + R.col
			(1).cross(A.col(1)) + R.col(2).cross(A.col(2))
			) * (1.0 / fabs(R.col(0).dot(A.col(0)) + R.col
				(1).dot(A.col(1)) + R.col(2).dot(A.col(2))) +

VRlabBuaa

void jacobiRotate(Matrix3d &A, Matrix3d &R, int p, int q)
{
	if (A(p, q) == 0.0)
		return;
	double d = (A(p, p) - A(q, q)) / (2.0*A(p, q));
	double t = 1.0 / (fabs(d) + sqrt(d*d + 1.0));
	if (d < 0.0)
		t = -t;
	double c = 1.0 / sqrt(t*t + 1);
	double s = t*c;