void dgCollisionScene::CalcAABB (const dgMatrix& matrix, dgVector& p0, dgVector& p1) const
{
	dgVector origin (matrix.TransformVector(m_boxOrigin));
	dgVector size (m_boxSize.m_x * dgAbsf(matrix[0][0]) + m_boxSize.m_y * dgAbsf(matrix[1][0]) + m_boxSize.m_z * dgAbsf(matrix[2][0]) + DG_MAX_COLLISION_PADDING,  
				   m_boxSize.m_x * dgAbsf(matrix[0][1]) + m_boxSize.m_y * dgAbsf(matrix[1][1]) + m_boxSize.m_z * dgAbsf(matrix[2][1]) + DG_MAX_COLLISION_PADDING,  
				   m_boxSize.m_x * dgAbsf(matrix[0][2]) + m_boxSize.m_y * dgAbsf(matrix[1][2]) + m_boxSize.m_z * dgAbsf(matrix[2][2]) + DG_MAX_COLLISION_PADDING,
				   dgFloat32 (0.0f));

	p0 = origin - size;
	p1 = origin + size;

#ifdef DG_DEBUG_AABB
	dgInt32 i;
	dgVector q0;
	dgVector q1;
	dgMatrix trans (matrix.Transpose());
	for (i = 0; i < 3; i ++) {
		q0[i] = matrix.m_posit[i] + matrix.RotateVector (BoxSupportMapping(trans[i].Scale (-1.0f)))[i];
		q1[i] = matrix.m_posit[i] + matrix.RotateVector (BoxSupportMapping(trans[i]))[i];
	}

	dgVector err0 (p0 - q0);
	dgVector err1 (p1 - q1);
	dgFloat32 err; 
	err = GetMax (size.m_x, size.m_y, size.m_z) * 0.5f; 
	_ASSERTE ((err0 % err0) < err);
	_ASSERTE ((err1 % err1) < err);
#endif
}
예제 #2
0
dgMatrix::dgMatrix (const dgMatrix& transformMatrix, const dgVector& scale, const dgMatrix& stretchAxis)
{
	dgMatrix scaledAxis;
	scaledAxis[0] = stretchAxis[0].Scale4 (scale[0]);
	scaledAxis[1] = stretchAxis[1].Scale4 (scale[1]);
	scaledAxis[2] = stretchAxis[2].Scale4 (scale[2]);
	scaledAxis[3] = stretchAxis[3];

	*this = stretchAxis.Transpose() * scaledAxis * transformMatrix;
}
예제 #3
0
void dgMatrix::PolarDecomposition (dgMatrix& transformMatrix, dgVector& scale, dgMatrix& stretchAxis, const dgMatrix* const initialStretchAxis) const
{
	// a polar decomposition decompose matrix A = O * S
	// where S = sqrt (transpose (L) * L)

/*
	// calculate transpose (L) * L 
	dgMatrix LL ((*this) * Transpose());

	// check is this is a pure uniformScale * rotation * translation
	dgFloat32 det2 = (LL[0][0] + LL[1][1] + LL[2][2]) * dgFloat32 (1.0f / 3.0f);

	dgFloat32 invdet2 = 1.0f / det2;

	dgMatrix pureRotation (LL);
	pureRotation[0] = pureRotation[0].Scale3 (invdet2);
	pureRotation[1] = pureRotation[1].Scale3 (invdet2);
	pureRotation[2] = pureRotation[2].Scale3 (invdet2);

	dgFloat32 sign = ((((*this)[0] * (*this)[1]) % (*this)[2]) > 0.0f) ? 1.0f : -1.0f;
	dgFloat32 det = (pureRotation[0] * pureRotation[1]) % pureRotation[2];
	if (dgAbsf (det - dgFloat32 (1.0f)) < dgFloat32 (1.0e-5f)) {
		// this is a pure scale * rotation * translation
		det = sign * dgSqrt (det2);
		scale[0] = det;
		scale[1] = det;
		scale[2] = det;
		det = dgFloat32 (1.0f)/ det;
		transformMatrix.m_front = m_front.Scale3 (det);
		transformMatrix.m_up = m_up.Scale3 (det);
		transformMatrix.m_right = m_right.Scale3 (det);
		transformMatrix[0][3] = dgFloat32 (0.0f);
		transformMatrix[1][3] = dgFloat32 (0.0f);
		transformMatrix[2][3] = dgFloat32 (0.0f);
		transformMatrix.m_posit = m_posit;
		stretchAxis = dgGetIdentityMatrix();

	} else {
		stretchAxis = LL;
		stretchAxis.EigenVectors (scale);

		// I need to deal with by seeing of some of the Scale are duplicated
		// do this later (maybe by a given rotation around the non uniform axis but I do not know if it will work)
		// for now just us the matrix

		scale[0] = sign * dgSqrt (scale[0]);
		scale[1] = sign * dgSqrt (scale[1]);
		scale[2] = sign * dgSqrt (scale[2]);
		scale[3] = dgFloat32 (0.0f);

		dgMatrix scaledAxis;
		scaledAxis[0] = stretchAxis[0].Scale3 (dgFloat32 (1.0f) / scale[0]);
		scaledAxis[1] = stretchAxis[1].Scale3 (dgFloat32 (1.0f) / scale[1]);
		scaledAxis[2] = stretchAxis[2].Scale3 (dgFloat32 (1.0f) / scale[2]);
		scaledAxis[3] = stretchAxis[3];
		dgMatrix symetricInv (stretchAxis.Transpose() * scaledAxis);

		transformMatrix = symetricInv * (*this);
		transformMatrix.m_posit = m_posit;
	}
*/

// test the f*****g factorization 
dgMatrix xxxxx(dgRollMatrix(30.0f * 3.1416f / 180.0f));
xxxxx = dgYawMatrix(30.0f * 3.1416f / 180.0f) * xxxxx;
dgMatrix xxxxx1(dgGetIdentityMatrix());
xxxxx1[0][0] = 2.0f;
dgMatrix xxxxx2(xxxxx.Inverse() * xxxxx1 * xxxxx);
dgMatrix xxxxx3 (xxxxx2);
xxxxx2.EigenVectors(scale);
dgMatrix xxxxx4(xxxxx2.Inverse() * xxxxx1 * xxxxx2);


	//dgFloat32 sign = ((((*this)[0] * (*this)[1]) % (*this)[2]) > 0.0f) ? 1.0f : -1.0f;
	dgFloat32 sign = dgSign(((*this)[0] * (*this)[1]) % (*this)[2]);
	stretchAxis = (*this) * Transpose();
	stretchAxis.EigenVectors (scale);

	// I need to deal with by seeing of some of the Scale are duplicated
	// do this later (maybe by a given rotation around the non uniform axis but I do not know if it will work)
	// for now just us the matrix

	scale[0] = sign * dgSqrt (scale[0]);
	scale[1] = sign * dgSqrt (scale[1]);
	scale[2] = sign * dgSqrt (scale[2]);
	scale[3] = dgFloat32 (0.0f);

	dgMatrix scaledAxis;
	scaledAxis[0] = stretchAxis[0].Scale3 (dgFloat32 (1.0f) / scale[0]);
	scaledAxis[1] = stretchAxis[1].Scale3 (dgFloat32 (1.0f) / scale[1]);
	scaledAxis[2] = stretchAxis[2].Scale3 (dgFloat32 (1.0f) / scale[2]);
	scaledAxis[3] = stretchAxis[3];
	dgMatrix symetricInv (stretchAxis.Transpose() * scaledAxis);

	transformMatrix = symetricInv * (*this);
	transformMatrix.m_posit = m_posit;

}