Esempio 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));
}
Esempio n. 2
0
void dgBallConstraint::SetLimits(const dgVector& coneDir,
    dgFloat32 minConeAngle, dgFloat32 maxConeAngle, dgFloat32 maxTwistAngle,
    const dgVector& bilateralDir, dgFloat32 negativeBilateralConeAngle__,
    dgFloat32 positiveBilateralConeAngle__)
{
  dgMatrix matrix0;
  dgMatrix matrix1;
  CalculateGlobalMatrixAndAngle(matrix0, matrix1);

  _ASSERTE(m_body0);
  _ASSERTE(m_body1);
  const dgMatrix& body0_Matrix = m_body0->GetMatrix();

  dgVector lateralDir(bilateralDir * coneDir);
  if ((lateralDir % lateralDir) < dgFloat32(1.0e-3f))
  {
    dgMatrix tmp(coneDir);
    lateralDir = tmp.m_up;
  }

  m_localMatrix0.m_front = body0_Matrix.UnrotateVector(coneDir);
  m_localMatrix0.m_up = body0_Matrix.UnrotateVector(lateralDir);
  m_localMatrix0.m_posit = body0_Matrix.UntransformVector(matrix1.m_posit);

  m_localMatrix0.m_front =
      m_localMatrix0.m_front.Scale(
          dgFloat32(
              1.0f) / dgSqrt (m_localMatrix0.m_front % m_localMatrix0.m_front));
  m_localMatrix0.m_up = m_localMatrix0.m_up.Scale(
      dgFloat32(1.0f) / dgSqrt (m_localMatrix0.m_up % m_localMatrix0.m_up));
  m_localMatrix0.m_right = m_localMatrix0.m_front * m_localMatrix0.m_up;

  m_localMatrix0.m_front.m_w = dgFloat32(0.0f);
  m_localMatrix0.m_up.m_w = dgFloat32(0.0f);
  m_localMatrix0.m_right.m_w = dgFloat32(0.0f);
  m_localMatrix0.m_posit.m_w = dgFloat32(1.0f);

//	dgMatrix body1_Matrix (dgGetIdentityMatrix());
//	if (m_body1) {
//		body1_Matrix = m_body1->GetMatrix();
//	}
  const dgMatrix& body1_Matrix = m_body1->GetMatrix();

  m_twistAngle = ClampValue(maxTwistAngle, dgFloat32(5.0f) * dgDEG2RAD,
      dgFloat32(90.0f) * dgDEG2RAD);
  m_coneAngle = ClampValue((maxConeAngle - minConeAngle) * dgFloat32(0.5f),
      dgFloat32(5.0f) * dgDEG2RAD, 175.0f * dgDEG2RAD);
  m_coneAngleCos = dgCos (m_coneAngle);

  dgMatrix coneMatrix(
      dgPitchMatrix((maxConeAngle + minConeAngle) * dgFloat32(0.5f)));

  m_localMatrix0 = coneMatrix * m_localMatrix0;

  m_localMatrix1 = m_localMatrix0 * body0_Matrix * body1_Matrix.Inverse();

}
void dgCollisionChamferCylinder::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const
{
//dgCollisionConvex::DebugCollision (matrix, callback, userData);
//return;

	dgInt32 slices = 12;
	dgInt32 brakes = 24;
	dgFloat32 sliceAngle = dgFloat32 (0.0f);
	dgFloat32 sliceStep = dgPI  / slices; 
	dgFloat32 breakStep = dgPI2 / brakes;

	dgTriplex pool[24 * (12 + 1)];

	dgMatrix rot (dgPitchMatrix (breakStep));	
	dgInt32 index = 0;
	for (dgInt32 j = 0; j <= slices; j ++) {
		dgVector p0 (-m_height * dgCos(sliceAngle), dgFloat32 (0.0f), m_radius + m_height * dgSin(sliceAngle), dgFloat32 (0.0f));
		sliceAngle += sliceStep;
		for (dgInt32 i = 0; i < brakes; i ++) {
			pool[index].m_x = p0.m_x;
			pool[index].m_y = p0.m_y;
			pool[index].m_z = p0.m_z;
			index ++;
			p0 = rot.UnrotateVector (p0);
		}
	}

	//dgMatrix matrix (GetLocalMatrix() * matrixPtr);
	matrix.TransformTriplex (&pool[0].m_x, sizeof (dgTriplex), &pool[0].m_x, sizeof (dgTriplex), 24 * (12 + 1));

	dgTriplex face[32];

	index = 0;
	for (dgInt32 j = 0; j < slices; j ++) {
		dgInt32 index0 = index + brakes - 1;
		for (dgInt32 i = 0; i < brakes; i ++) {
			face[0] = pool[index];
			face[1] = pool[index0];
			face[2] = pool[index0 + brakes];
			face[3] = pool[index + brakes];
			index0 = index;
			index ++;
			callback (userData, 4, &face[0].m_x, 0);
		}
	}

	for (dgInt32 i = 0; i < brakes; i ++) { 
		face[i] = pool[i];
	}
	callback (userData, 24, &face[0].m_x, 0);

	for (dgInt32 i = 0; i < brakes; i ++) { 
		face[i] = pool[brakes * (slices + 1) - i - 1];
	}
	callback (userData, 24, &face[0].m_x, 0);
}
void dgCollisionChamferCylinder::Init (dgFloat32 radius, dgFloat32 height)
{
//	dgInt32 i;
//	dgInt32 j;
//	dgInt32 index;
//	dgInt32 index0;
//	dgFloat32 sliceStep;
//	dgFloat32 sliceAngle;
//	dgFloat32 breakStep;
//	dgFloat32 breakAngle;
//	dgEdge *edge;

	m_rtti |= dgCollisionChamferCylinder_RTTI;
	m_radius = dgAbsf (radius);
	m_height = dgAbsf (height * dgFloat32 (0.5f));
	m_radius = GetMax (dgFloat32(0.001f), m_radius - m_height);

	m_silhuette[0] = dgVector (m_height,  m_radius, dgFloat32 (0.0f), dgFloat32 (0.0f));
	m_silhuette[1] = dgVector (m_height, -m_radius, dgFloat32 (0.0f), dgFloat32 (0.0f));
	m_silhuette[2] = dgVector (-m_height, -m_radius, dgFloat32 (0.0f), dgFloat32 (0.0f));
	m_silhuette[3] = dgVector (-m_height,  m_radius, dgFloat32 (0.0f), dgFloat32 (0.0f));


//	m_tethaStep = GetDiscretedAngleStep (m_radius);
//	m_tethaStepInv = dgFloat32 (1.0f) / m_tethaStep;
//	m_delCosTetha = dgCos (m_tethaStep);
//	m_delSinTetha = dgSin (m_tethaStep);

	dgFloat32 sliceAngle = dgFloat32 (0.0f);
	//dgFloat32 breakAngle = dgFloat32 (0.0f); 
	dgFloat32 sliceStep = dgPI  / DG_CHAMFERCYLINDER_SLICES; 
	dgFloat32 breakStep = dgPI2 / DG_CHAMFERCYLINDER_BRAKES;

	dgMatrix rot (dgPitchMatrix (breakStep));	
	dgInt32 index = 0;
	for (dgInt32 j = 0; j <= DG_CHAMFERCYLINDER_SLICES; j ++) {
		dgVector p0 (-m_height * dgCos(sliceAngle), dgFloat32 (0.0f), m_radius + m_height * dgSin(sliceAngle), dgFloat32 (1.0f));
		sliceAngle += sliceStep;
		for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i ++) {
			m_vertex[index] = p0;
			index ++;
			p0 = rot.UnrotateVector (p0);
		}
	}

	m_edgeCount = (4 * DG_CHAMFERCYLINDER_SLICES + 2)* DG_CHAMFERCYLINDER_BRAKES;
	m_vertexCount = DG_CHAMFERCYLINDER_BRAKES * (DG_CHAMFERCYLINDER_SLICES + 1);
	dgCollisionConvex::m_vertex = m_vertex;

	if (!m_shapeRefCount) {
		dgPolyhedra polyhedra(m_allocator);
		dgInt32 wireframe[DG_CHAMFERCYLINDER_SLICES + 10];

		for (dgInt32 i = 0; i < DG_MAX_CHAMFERCYLINDER_DIR_COUNT; i ++) {
			dgMatrix matrix (dgPitchMatrix (dgFloat32 (dgPI2 * i) / DG_MAX_CHAMFERCYLINDER_DIR_COUNT));
			m_shapesDirs[i] = matrix.RotateVector (dgVector (dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)));
		}


		dgInt32 index = 0;
		for (dgInt32 j = 0; j < DG_CHAMFERCYLINDER_SLICES; j ++) {
			dgInt32 index0 = index + DG_CHAMFERCYLINDER_BRAKES - 1;
			for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i ++) {
				wireframe[0] = index;
				wireframe[1] = index0;
				wireframe[2] = index0 + DG_CHAMFERCYLINDER_BRAKES;
				wireframe[3] = index + DG_CHAMFERCYLINDER_BRAKES;

				index0 = index;
				index ++;
				polyhedra.AddFace (4, wireframe);
			}
		}

		for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i ++) { 
			wireframe[i] = i;
		}
		polyhedra.AddFace (DG_CHAMFERCYLINDER_BRAKES, wireframe);

		for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i ++) { 
			wireframe[i] = DG_CHAMFERCYLINDER_BRAKES * (DG_CHAMFERCYLINDER_SLICES + 1) - i - 1;
		}
		polyhedra.AddFace (DG_CHAMFERCYLINDER_BRAKES, wireframe);
		polyhedra.EndFace ();

		_ASSERTE (SanityCheck (polyhedra));

		dgUnsigned64 i = 0;
		dgPolyhedra::Iterator iter (polyhedra);
		for (iter.Begin(); iter; iter ++) {
			dgEdge* const edge = &(*iter);
			edge->m_userData = i;
			i ++;
		}

		for (iter.Begin(); iter; iter ++) {
			dgEdge* const edge = &(*iter);

			dgConvexSimplexEdge* const ptr = &m_edgeArray[edge->m_userData];
			ptr->m_vertex = edge->m_incidentVertex;
			ptr->m_next = &m_edgeArray[edge->m_next->m_userData];
			ptr->m_prev = &m_edgeArray[edge->m_prev->m_userData];
			ptr->m_twin = &m_edgeArray[edge->m_twin->m_userData];
		}
	}

	m_shapeRefCount ++;
	dgCollisionConvex::m_simplex = m_edgeArray;

	SetVolumeAndCG ();
	//	CalculateDistanceTravel();
}
Esempio n. 5
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
}