void Quaternion::FromRotationMatrix( const Matrix4& m )
{
GLASSERT( m.IsRotation() );
float trace = m.x[0] + m.x[5] + m.x[10];
const int next[3] = { 1, 2, 0 };
if ( trace > 0.0f )
{
float s = (float)sqrt(trace + 1.0f);
w = s / 2.0f;
s = 0.5f / s;
x = ( m.x[9] - m.x[6] ) * s;
y = ( m.x[2] - m.x[8] ) * s;
z = ( m.x[4] - m.x[1] ) * s;
}
else
{
int i=0;
if (m.x[5] > m.x[0]) i = 1;
if (m.x[10] > m.x[i*4+i]) i = 2;
int j = next[i];
int k = next[j];
float s = (float) sqrt ((m.x[i*4+i] - (m.x[j*4+j] + m.x[k*4+k])) + 1.0f);
float q[4];
q[i] = s * 0.5f;
if (s != 0.0f) s = 0.5f / s;
q[3] = (m.x[k*4+j] - m.x[j*4+k]) * s;
q[j] = (m.x[j*4+i] + m.x[i*4+j]) * s;
q[k] = (m.x[k*4+i] + m.x[i*4+k]) * s;
x = q[0];
y = q[1];
z = q[2];
w = q[3];
}
GLASSERT( Equal( 1.0f, x*x + y*y + z*z + w*w, 0.0001f ) );
}
void StaticMesh::SetPosV( const Vector3F& _newLoc, const Matrix4& newRot )
{
Vector3F newPos = _newLoc;
GLASSERT( newRot.IsRotation() );
if ( AttachedToTerrain() ) {
newPos.z = Lilith3D::Instance()->GetTerrainMesh()->CalcHeight( newPos.x, newPos.y, 0, 0 );
}
if ( Pos() != newPos || newRot != rotation || !InList() )
{
// If in the container, remove it from the container.
if ( InList() ) {
ListRemove();
}
pos = newPos;
rotation = newRot;
ComputeTransform();
CalcAABB( staticResource->AABB() );
Lilith3D::Instance()->GetQuadTree()->AddMesh( this );
}
}