Ejemplo n.º 1
0
dgVector dgMatrix::CalcPitchYawRoll () const
{
	const hacd::HaF32 minSin = hacd::HaF32(0.99995f);

	const dgMatrix& matrix = *this;

	hacd::HaF32 roll = hacd::HaF32(0.0f);
	hacd::HaF32 pitch  = hacd::HaF32(0.0f);
	hacd::HaF32 yaw = dgAsin (-ClampValue (matrix[0][2], hacd::HaF32(-0.999999f), hacd::HaF32(0.999999f)));

	HACD_ASSERT (dgCheckFloat (yaw));
	if (matrix[0][2] < minSin) {
		if (matrix[0][2] > (-minSin)) {
			roll = dgAtan2 (matrix[0][1], matrix[0][0]);
			pitch = dgAtan2 (matrix[1][2], matrix[2][2]);
		} else {
			pitch = dgAtan2 (matrix[1][0], matrix[1][1]);
		}
	} else {
		pitch = -dgAtan2 (matrix[1][0], matrix[1][1]);
	}

#ifdef _DEBUG
	dgMatrix m (dgPitchMatrix (pitch) * dgYawMatrix(yaw) * dgRollMatrix(roll));
	for (hacd::HaI32 i = 0; i < 3; i ++) {
		for (hacd::HaI32 j = 0; j < 3; j ++) {
			hacd::HaF32 error = dgAbsf (m[i][j] - matrix[i][j]);
			HACD_ASSERT (error < 5.0e-2f);
		}
	}
#endif

	return dgVector (pitch, yaw, roll, hacd::HaF32(0.0f));
}
Ejemplo n.º 2
0
void ShowMousePicking (const dVector& p0, const dVector& p1)
{

	dFloat radius = 0.25f;
	// set the color of the cube's surface
	GLfloat cubeColor[] = { 1.0f, 1.0f, 1.0f, 1.0 };
	glMaterialfv(GL_FRONT, GL_SPECULAR, cubeColor);
	glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, cubeColor);
	glMaterialf(GL_FRONT, GL_SHININESS, 50.0);


	// set up the cube's texture
	glDisable(GL_TEXTURE_2D);


	GLUquadricObj *pObj;
	glPushMatrix();
	dMatrix matrix (GetIdentityMatrix());
	matrix.m_posit = p0;
	glMultMatrix(&matrix[0][0]);
	
	// Get a new Quadric off the stack
	pObj = gluNewQuadric();				
	gluQuadricTexture(pObj, true);						
	gluSphere(pObj, radius, 10, 10);					
	glPopMatrix();

	glPushMatrix();
	matrix.m_posit = p1;
	glMultMatrix(&matrix[0][0]);
	gluSphere(pObj, radius, 10, 10);					
	gluDeleteQuadric(pObj);	
	glPopMatrix();

	dVector dir (p1 - p0);
	dFloat lenght (dir % dir);
	if (lenght > 1.0e-2f) {
	
		glPushMatrix();
		
		lenght = sqrt (lenght);
		dMatrix align (dgYawMatrix(0.5f * 3.1426f));
		align.m_posit.m_x = -lenght * 0.5f;
		matrix = align * dgGrammSchmidt(dir);
		matrix.m_posit += (p1 + p0).Scale (0.5f);
		glMultMatrix(&matrix[0][0]);
		
		// Get a new Quadric off the stack
		pObj = gluNewQuadric();				
		gluCylinder(pObj, radius * 0.5f, radius * 0.5f, lenght, 10, 2);
		gluDeleteQuadric(pObj);	
		glPopMatrix();
	}

}
Ejemplo n.º 3
0
void dgMatrix::CalcPitchYawRoll (dgVector& euler0, dgVector& euler1) const
{
	const dgMatrix& matrix = *this;
	dgAssert (dgAbsf (((matrix[0] * matrix[1]) % matrix[2]) - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));

	// Assuming the angles are in radians.
	if (matrix[0][2] > dgFloat32 (0.99995f)) {
		dgFloat32 picth0 = dgFloat32 (0.0f);
		dgFloat32 yaw0 = dgFloat32 (-3.141592f * 0.5f);
		dgFloat32 roll0 = - dgAtan2(matrix[2][1], matrix[1][1]);
		euler0[0] = picth0;
		euler0[1] = yaw0;
		euler0[2] = roll0;

		euler1[0] = picth0;
		euler1[1] = yaw0;
		euler1[2] = roll0;

	} else if (matrix[0][2] < dgFloat32 (-0.99995f)) {
		dgFloat32 picth0 = dgFloat32 (0.0f);
		dgFloat32 yaw0 = dgFloat32 (3.141592f * 0.5f);
		dgFloat32 roll0 = dgAtan2(matrix[2][1], matrix[1][1]);
		euler0[0] = picth0;
		euler0[1] = yaw0;
		euler0[2] = roll0;

		euler1[0] = picth0;
		euler1[1] = yaw0;
		euler1[2] = roll0;
	} else {
		dgFloat32 yaw0 = -dgAsin ( matrix[0][2]);
		dgFloat32 yaw1 = dgFloat32 (3.141592f) - yaw0;
		dgFloat32 sign0 = dgSign(dgCos (yaw0));
		dgFloat32 sign1 = dgSign(dgCos (yaw1));

		dgFloat32 picth0 = dgAtan2(matrix[1][2] * sign0, matrix[2][2] * sign0);
		dgFloat32 picth1 = dgAtan2(matrix[1][2] * sign1, matrix[2][2] * sign1);

		dgFloat32 roll0 = dgAtan2(matrix[0][1] * sign0, matrix[0][0] * sign0);
		dgFloat32 roll1 = dgAtan2(matrix[0][1] * sign1, matrix[0][0] * sign1);

		if (yaw1 > dgFloat32 (3.141592f)) {
			yaw1 -= dgFloat32 (2.0f * 3.141592f);
		}

		euler0[0] = picth0;
		euler0[1] = yaw0;
		euler0[2] = roll0;

		euler1[0] = picth1;
		euler1[1] = yaw1;
		euler1[2] = roll1;
	}
	euler0[3] = dgFloat32(0.0f);
	euler1[3] = dgFloat32(0.0f);

#ifdef _DEBUG
	dgMatrix m0 (dgPitchMatrix (euler0[0]) * dgYawMatrix(euler0[1]) * dgRollMatrix(euler0[2]));
	dgMatrix m1 (dgPitchMatrix (euler1[0]) * dgYawMatrix(euler1[1]) * dgRollMatrix(euler1[2]));
	for (int i = 0; i < 3; i ++) {
		for (int j = 0; j < 3; j ++) {
			dgFloat32 error = dgAbsf (m0[i][j] - matrix[i][j]);
			dgAssert (error < 5.0e-2f);
			error = dgAbsf (m1[i][j] - matrix[i][j]);
			dgAssert (error < 5.0e-2f);
		}
	}
#endif
}
Ejemplo n.º 4
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;

}
Ejemplo n.º 5
0
//	Keyboard handler. 
void Keyboard()
{
	// check for termination
	if (dGetKeyState (VK_ESCAPE) & 0x8000) {
		exit(0);
	}

	// read the mouse position and set the camera direction
	static MOUSE_POINT mouse0;
	static dFloat yawAngle = 90.0f * 3.1416 / 180.0f;
	static dFloat rollAngle = 0.0f;

	MOUSE_POINT mouse1;
	GetCursorPos(mouse1);

	if (dGetKeyState (VK_LBUTTON) & 0x8000) {
		if (mouse1.x > (mouse0.x + 1)) {
			yawAngle += 1.0f * 3.1416 / 180.0f;
			if (yawAngle > (360.0f * 3.1416 / 180.0f)) {
				yawAngle -= (360.0f * 3.1416 / 180.0f);
			}
		} else if (mouse1.x < (mouse0.x - 1)) {
			yawAngle -= 1.0f * 3.1416 / 180.0f;
			if (yawAngle < 0.0f) {
				yawAngle += (360.0f * 3.1416 / 180.0f);
			}
		}

		if (mouse1.y > (mouse0.y + 1)) {
			rollAngle += 1.0f * 3.1416 / 180.0f;
			if (rollAngle > (80.0f * 3.1416 / 180.0f)) {
				rollAngle = 80.0f * 3.1416 / 180.0f;
			}
		} else if (mouse1.y < (mouse0.y - 1)) {
			rollAngle -= 1.0f * 3.1416 / 180.0f;
			if (rollAngle < -(80.0f * 3.1416 / 180.0f)) {
				rollAngle = -80.0f * 3.1416 / 180.0f;
			}
		}
		dMatrix cameraDirMat (dgRollMatrix(rollAngle) * dgYawMatrix(yawAngle));
		cameraDir = cameraDirMat.m_front;
	}

	mouse0 = mouse1;


	// camera control
	if (dGetKeyState ('W') & 0x8000) {
		cameraEyepoint += cameraDir.Scale (CAMERA_SPEED / 60.0f);
	} else if (dGetKeyState ('S') & 0x8000) {
		cameraEyepoint -= cameraDir.Scale (CAMERA_SPEED / 60.0f);
	}

	if (dGetKeyState ('D') & 0x8000) {
		dVector up (0.0f, 1.0f, 0.0f);
		dVector right (cameraDir * up);
		cameraEyepoint += right.Scale (CAMERA_SPEED / 60.0f);
	} else if (dGetKeyState ('A') & 0x8000) {
		dVector up (0.0f, 1.0f, 0.0f);
		dVector right (cameraDir * up);
		cameraEyepoint -= right.Scale (CAMERA_SPEED / 60.0f);
	}
}