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;
}
//-----------------------------------------------------------------------------
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();
}
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;
}
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;
}
//--------------------------------------
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;
}
}