|
@@ -488,24 +488,51 @@ void BitStream::readAffineTransform(MatrixF* matrix)
|
|
|
|
|
|
void BitStream::writeQuat( const QuatF& quat, U32 bitCount )
|
|
|
{
|
|
|
- writeSignedFloat( quat.x, bitCount );
|
|
|
- writeSignedFloat( quat.y, bitCount );
|
|
|
- writeSignedFloat( quat.z, bitCount );
|
|
|
- writeFlag( quat.w < 0.0f );
|
|
|
+ F32 quatVals[4] = { quat.x, quat.y, quat.z, quat.w };
|
|
|
+ bool flipQuat = (quatVals[0] < 0);
|
|
|
+ F32 maxVal = mFabs(quatVals[0]);
|
|
|
+ S32 idxMax = 0;
|
|
|
+
|
|
|
+ for (S32 i = 1; i < 4; ++i)
|
|
|
+ {
|
|
|
+ if (mFabs(quatVals[i]) > maxVal)
|
|
|
+ {
|
|
|
+ idxMax = i;
|
|
|
+ maxVal = mFabs(quatVals[i]);
|
|
|
+ flipQuat = (quatVals[i] < 0);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ writeInt(idxMax, 2);
|
|
|
+
|
|
|
+ for (S32 i = 0; i < 4; ++i)
|
|
|
+ {
|
|
|
+ if (i == idxMax)
|
|
|
+ continue;
|
|
|
+ F32 curValue = (flipQuat ? -quatVals[i] : quatVals[i]) * (F32) M_SQRT2;
|
|
|
+ writeSignedFloat( curValue, bitCount );
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
void BitStream::readQuat( QuatF *outQuat, U32 bitCount )
|
|
|
{
|
|
|
- outQuat->x = readSignedFloat( bitCount );
|
|
|
- outQuat->y = readSignedFloat( bitCount );
|
|
|
- outQuat->z = readSignedFloat( bitCount );
|
|
|
-
|
|
|
- outQuat->w = mSqrt( 1.0 - getMin( mSquared( outQuat->x ) +
|
|
|
- mSquared( outQuat->y ) +
|
|
|
- mSquared( outQuat->z ),
|
|
|
- 1.0f ) );
|
|
|
- if ( readFlag() )
|
|
|
- outQuat->w = -outQuat->w;
|
|
|
+ F32 quatVals[4];
|
|
|
+ F32 sum = 0.0f;
|
|
|
+
|
|
|
+ S32 idxMax = readInt( 2 );
|
|
|
+ for (S32 i = 0; i < 4; ++i)
|
|
|
+ {
|
|
|
+ if (i == idxMax)
|
|
|
+ continue;
|
|
|
+ quatVals[i] = readSignedFloat( bitCount ) * M_SQRTHALF_F;
|
|
|
+ sum += quatVals[i] * quatVals[i];
|
|
|
+ }
|
|
|
+
|
|
|
+ if (sum > 1.0f)
|
|
|
+ quatVals[idxMax] = 1.0f;
|
|
|
+ else
|
|
|
+ quatVals[idxMax] = mSqrt(1.0f - sum);
|
|
|
+
|
|
|
+ outQuat->set(quatVals[0], quatVals[1], quatVals[2], quatVals[3]);
|
|
|
}
|
|
|
|
|
|
void BitStream::writeBits( const BitVector &bitvec )
|