void dCustomUpVector::SubmitConstraints (dFloat timestep, int threadIndex)
{
	dMatrix matrix0;
	dMatrix matrix1;

	// calculate the position of the pivot point and the Jacobian direction vectors, in global space. 
	CalculateGlobalMatrix (matrix0, matrix1);
  
	// if the body ha rotated by some amount, the there will be a plane of rotation
	dVector lateralDir (matrix0.m_front.CrossProduct(matrix1.m_front));
	dFloat mag = lateralDir.DotProduct3(lateralDir);
	if (mag > 1.0e-6f) {
		// if the side vector is not zero, it means the body has rotated
		mag = dSqrt (mag);
		lateralDir = lateralDir.Scale (1.0f / mag);
		dFloat angle = dAsin (mag);

		// add an angular constraint to correct the error angle
		NewtonUserJointAddAngularRow (m_joint, angle, &lateralDir[0]);

		// in theory only one correction is needed, but this produces instability as the body may move sideway.
		// a lateral correction prevent this from happening.
		dVector frontDir (lateralDir.CrossProduct(matrix1.m_front));
		NewtonUserJointAddAngularRow (m_joint, 0.0f, &frontDir[0]);
 	} else {
		// if the angle error is very small then two angular correction along the plane axis do the trick
		NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]);
		NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]);
	}
}
Exemplo n.º 2
0
void DemoCamera::SetMatrix (DemoEntityManager& scene, const dQuaternion& rotation, const dVector& position)
{
	dMatrix matrix (rotation, position);
	m_cameraPitch = dAsin (matrix.m_front.m_y);
	m_cameraYaw = dAtan2 (-matrix.m_front.m_z, matrix.m_front.m_x);

	DemoEntity::SetMatrix (scene, rotation, position);
}
void InitCamera (const dVector& eyePoint, const dVector& dir)
{
	gCameraEyepoint = eyePoint;

	gCurrCameraDir = dir.Scale (1.0f / sqrt (dir % dir));
	gRollAngle = dAsin (gCurrCameraDir.m_y);
	gPrevRollAngle = gYawAngle;

	gYawAngle = dAtan2 (-gCurrCameraDir.m_z, gCurrCameraDir.m_x);
	gPrevYawAngle = gYawAngle;
}
Exemplo n.º 4
0
//dVector dMatrix::GetEulerAngles (dEulerAngleOrder order) const
void dMatrix::GetEulerAngles (dVector & euler0, dVector & euler1, dEulerAngleOrder order) const
{
   int a0 = (order >> 8) & 3;
   int a1 = (order >> 4) & 3;
   int a2 = (order >> 0) & 3;
   const dMatrix & matrix = *this;
   // Assuming the angles are in radians.
   if (matrix[a0][a2] > 0.99995f)
   {
      dFloat picth0 = 0.0f;
      dFloat yaw0 = -3.141592f * 0.5f;
      dFloat roll0 = - dAtan2 (matrix[a2][a1], matrix[a1][a1]);
      euler0[a0] = picth0;
      euler0[a1] = yaw0;
      euler0[a2] = roll0;
      euler1[a0] = picth0;
      euler1[a1] = yaw0;
      euler1[a2] = roll0;
   }
   else
      if (matrix[a0][a2] < -0.99995f)
      {
         dFloat picth0 = 0.0f;
         dFloat yaw0 = 3.141592f * 0.5f;
         dFloat roll0 = dAtan2 (matrix[a2][a1], matrix[a1][a1]);
         euler0[a0] = picth0;
         euler0[a1] = yaw0;
         euler0[a2] = roll0;
         euler1[a0] = picth0;
         euler1[a1] = yaw0;
         euler1[a2] = roll0;
      }
      else
      {
         //euler[a0] = -dAtan2(-matrix[a1][a2], matrix[a2][a2]);
         //euler[a1] = -dAsin ( matrix[a0][a2]);
         //euler[a2] = -dAtan2(-matrix[a0][a1], matrix[a0][a0]);
         dFloat yaw0 = -dAsin ( matrix[a0][a2]);
         dFloat yaw1 = 3.141592f - yaw0;
         dFloat sign0 = dSign (dCos (yaw0));
         dFloat sign1 = dSign (dCos (yaw1));
         dFloat picth0 = dAtan2 (matrix[a1][a2] * sign0, matrix[a2][a2] * sign0);
         dFloat picth1 = dAtan2 (matrix[a1][a2] * sign1, matrix[a2][a2] * sign1);
         dFloat roll0 = dAtan2 (matrix[a0][a1] * sign0, matrix[a0][a0] * sign0);
         dFloat roll1 = dAtan2 (matrix[a0][a1] * sign1, matrix[a0][a0] * sign1);
         if (yaw1 > 3.141592f)
            yaw1 -= 2.0f * 3.141592f;
         euler0[a0] = picth0;
         euler0[a1] = yaw0;
         euler0[a2] = roll0;
         euler1[a0] = picth1;
         euler1[a1] = yaw1;
         euler1[a2] = roll1;
      }
   euler0[3] = dFloat (0.0f);
   euler1[3] = dFloat (0.0f);
#ifdef _DEBUG
   if (order == m_pitchYawRoll)
   {
      dMatrix m0 (dPitchMatrix (euler0[0]) * dYawMatrix (euler0[1]) * dRollMatrix (euler0[2]));
      dMatrix m1 (dPitchMatrix (euler1[0]) * dYawMatrix (euler1[1]) * dRollMatrix (euler1[2]));
      for (int i = 0; i < 3; i ++)
      {
         for (int j = 0; j < 3; j ++)
         {
            dFloat error = dAbs (m0[i][j] - matrix[i][j]);
            dAssert (error < 5.0e-2f);
            error = dAbs (m1[i][j] - matrix[i][j]);
            dAssert (error < 5.0e-2f);
         }
      }
   }
#endif
}