示例#1
0
Basis Basis::diagonalize() {

	//NOTE: only implemented for symmetric matrices
	//with the Jacobi iterative method method
	
	ERR_FAIL_COND_V(!is_symmetric(), Basis());

	const int ite_max = 1024;

	real_t off_matrix_norm_2 = elements[0][1] * elements[0][1] + elements[0][2] * elements[0][2] + elements[1][2] * elements[1][2]; 

	int ite = 0;
	Basis acc_rot;
	while (off_matrix_norm_2 > CMP_EPSILON2 && ite++ < ite_max ) {
		real_t el01_2 = elements[0][1] * elements[0][1];
		real_t el02_2 = elements[0][2] * elements[0][2];
		real_t el12_2 = elements[1][2] * elements[1][2];
		// Find the pivot element
		int i, j;
		if (el01_2 > el02_2) {
			if (el12_2 > el01_2) {
				i = 1;
				j = 2;
			} else {
				i = 0;
				j = 1;
			}	
		} else {
			if (el12_2 > el02_2) {
				i = 1;
				j = 2;
			} else {
				i = 0;
				j = 2;
			}
		}

		// Compute the rotation angle
	    real_t angle;
		if (Math::abs(elements[j][j] - elements[i][i]) < CMP_EPSILON) {
			angle = Math_PI / 4;
		} else {
			angle = 0.5 * Math::atan(2 * elements[i][j] / (elements[j][j] - elements[i][i]));		
		}

		// Compute the rotation matrix
		Basis rot;
		rot.elements[i][i] = rot.elements[j][j] = Math::cos(angle);
		rot.elements[i][j] = - (rot.elements[j][i] = Math::sin(angle));

		// Update the off matrix norm
		off_matrix_norm_2 -= elements[i][j] * elements[i][j];

		// Apply the rotation
		*this = rot * *this * rot.transposed();
		acc_rot = rot * acc_rot;
	}

	return acc_rot;
}
示例#2
0
void BodySW::_update_transform_dependant() {

	center_of_mass = get_transform().basis.xform(center_of_mass_local);
	principal_inertia_axes = get_transform().basis * principal_inertia_axes_local;

	// update inertia tensor
	Basis tb = principal_inertia_axes;
	Basis tbt = tb.transposed();
	tb.scale(_inv_inertia);
	_inv_inertia_tensor = tb * tbt;
}