Example #1
0
QuatF& QuatF::set( const EulerF & e )
{
   F32 cx, sx;
   F32 cy, sy;
   F32 cz, sz;
   mSinCos( -e.x * 0.5f, sx, cx );
   mSinCos( -e.y * 0.5f, sy, cy );
   mSinCos( -e.z * 0.5f, sz, cz );

   // Qyaw(z)   = [ (0, 0, sin z/2), cos z/2 ]
   // Qpitch(x) = [ (sin x/2, 0, 0), cos x/2 ]
   // Qroll(y)  = [ (0, sin y/2, 0), cos y/2 ]
   // this = Qresult = Qyaw*Qpitch*Qroll  ZXY
   //
   // The code that folows is a simplification of:
   //    roll*=pitch;
   //    roll*=yaw;
   //    *this = roll;
   F32 cycz, sysz, sycz, cysz;
   cycz = cy*cz;
   sysz = sy*sz;
   sycz = sy*cz;
   cysz = cy*sz;
   w = cycz*cx + sysz*sx;
   x = cycz*sx + sysz*cx;
   y = sycz*cx - cysz*sx;
   z = cysz*cx - sycz*sx;

   return *this;
}
Example #2
0
//-----------------------------------------------------------------------------
void Rigid::integrate(F32 delta)
{
   // Update Angular position
   F32 angle = angVelocity.len();
   if (angle != 0.0f) {
      QuatF dq;
      F32 sinHalfAngle;
      mSinCos(angle * delta * -0.5f, sinHalfAngle, dq.w);
      sinHalfAngle *= 1.0f / angle;
      dq.x = angVelocity.x * sinHalfAngle;
      dq.y = angVelocity.y * sinHalfAngle;
      dq.z = angVelocity.z * sinHalfAngle;
      QuatF tmp = angPosition;
      angPosition.mul(tmp, dq);
      angPosition.normalize();

      // Rotate the position around the center of mass
      Point3F lp = linPosition - worldCenterOfMass;
      dq.mulP(lp,&linPosition);
      linPosition += worldCenterOfMass;
   }

   // Update angular momentum
   angMomentum = angMomentum + torque * delta;

   // Update linear position, momentum
   linPosition = linPosition + linVelocity * delta;
   linMomentum = linMomentum + force * delta;
   linVelocity = linMomentum * oneOverMass;

   // Update dependent state variables
   updateInertialTensor();
   updateVelocity();
   updateCenterOfMass();
}
Example #3
0
QuatF & QuatF::set( const Point3F &axis, F32 angle )
{
   F32 sinHalfAngle, cosHalfAngle;
   mSinCos( angle * 0.5f, sinHalfAngle, cosHalfAngle );
   x = axis.x * sinHalfAngle;
   y = axis.y * sinHalfAngle;
   z = axis.z * sinHalfAngle;
   w = cosHalfAngle;
   return *this;
}
Example #4
0
QuatF & QuatF::set( const Point3F &axis, F32 angle )
{
   PROFILE_SCOPE( QuatF_set_AngAxisF );

   F32 sinHalfAngle, cosHalfAngle;
   mSinCos( angle * 0.5f, sinHalfAngle, cosHalfAngle );
   x = axis.x * sinHalfAngle;
   y = axis.y * sinHalfAngle;
   z = axis.z * sinHalfAngle;
   w = cosHalfAngle;
   return *this;
}
Example #5
0
//--------------------------------------
static void m_matF_set_euler_C(const F32 *e, F32 *result)
{
   enum {
      AXIS_X   = (1<<0),
      AXIS_Y   = (1<<1),
      AXIS_Z   = (1<<2)
   };

   U32 axis = 0;
   if (e[0] != 0.0f) axis |= AXIS_X;
   if (e[1] != 0.0f) axis |= AXIS_Y;
   if (e[2] != 0.0f) axis |= AXIS_Z;

   switch (axis)
   {
      case 0:
         m_matF_identity(result);
         break;

      case AXIS_X:
      {
         F32 cx,sx;
         mSinCos( e[0], sx, cx );

         result[0] = 1.0f;
         result[1] = 0.0f;
         result[2] = 0.0f;
         result[3] = 0.0f;

         result[4] = 0.0f;
         result[5] = cx;
         result[6] = sx;
         result[7] = 0.0f;

         result[8] = 0.0f;
         result[9] = -sx;
         result[10]= cx;
         result[11]= 0.0f;

         result[12]= 0.0f;
         result[13]= 0.0f;
         result[14]= 0.0f;
         result[15]= 1.0f;
         break;
      }

      case AXIS_Y:
      {
         F32 cy,sy;
         mSinCos( e[1], sy, cy );

         result[0] = cy;
         result[1] = 0.0f;
         result[2] = -sy;
         result[3] = 0.0f;

         result[4] = 0.0f;
         result[5] = 1.0f;
         result[6] = 0.0f;
         result[7] = 0.0f;

         result[8] = sy;
         result[9] = 0.0f;
         result[10]= cy;
         result[11]= 0.0f;

         result[12]= 0.0f;
         result[13]= 0.0f;
         result[14]= 0.0f;
         result[15]= 1.0f;
         break;
      }

      case AXIS_Z:
      {
         // the matrix looks like this:
         //  r1 - (r4 * sin(x))     r2 + (r3 * sin(x))   -cos(x) * sin(y)
         //  -cos(x) * sin(z)       cos(x) * cos(z)      sin(x)
         //  r3 + (r2 * sin(x))     r4 - (r1 * sin(x))   cos(x) * cos(y)
         //
         // where:
         //  r1 = cos(y) * cos(z)
         //  r2 = cos(y) * sin(z)
         //  r3 = sin(y) * cos(z)
         //  r4 = sin(y) * sin(z)
         F32 cz,sz;
         mSinCos( e[2], sz, cz );

         result[0] = cz;
         result[1] = sz;
         result[2] = 0.0f;
         result[3] = 0.0f;

         result[4] = -sz;
         result[5] = cz;
         result[6] = 0.0f;
         result[7] = 0.0f;

         result[8] = 0.0f;
         result[9] = 0.0f;
         result[10]= 1.0f;
         result[11]= 0.0f;

         result[12]= 0.0f;
         result[13]= 0.0f;
         result[14]= 0.0f;
         result[15]= 1.0f;
         break;
      }

      default:
         // the matrix looks like this:
         //  r1 - (r4 * sin(x))     r2 + (r3 * sin(x))   -cos(x) * sin(y)
         //  -cos(x) * sin(z)       cos(x) * cos(z)      sin(x)
         //  r3 + (r2 * sin(x))     r4 - (r1 * sin(x))   cos(x) * cos(y)
         //
         // where:
         //  r1 = cos(y) * cos(z)
         //  r2 = cos(y) * sin(z)
         //  r3 = sin(y) * cos(z)
         //  r4 = sin(y) * sin(z)
         F32 cx,sx;
         mSinCos( e[0], sx, cx );
         F32 cy,sy;
         mSinCos( e[1], sy, cy );
         F32 cz,sz;
         mSinCos( e[2], sz, cz );
         F32 r1 = cy * cz;
         F32 r2 = cy * sz;
         F32 r3 = sy * cz;
         F32 r4 = sy * sz;

         result[0] = r1 - (r4 * sx);
         result[1] = r2 + (r3 * sx);
         result[2] = -cx * sy;
         result[3] = 0.0f;

         result[4] = -cx * sz;
         result[5] = cx * cz;
         result[6] = sx;
         result[7] = 0.0f;

         result[8] = r3 + (r2 * sx);
         result[9] = r4 - (r1 * sx);
         result[10]= cx * cy;
         result[11]= 0.0f;

         result[12]= 0.0f;
         result[13]= 0.0f;
         result[14]= 0.0f;
         result[15]= 1.0f;
         break;
   }
}