IC void q_scalem(Fmatrix &m, float v)
{
Fquaternion q;
q.set(m);
q_scale(q,v);
m.rotation(q);
}
IC void q_add_scaled_basem( Fquaternion &q, const Fquaternion &base, const Fquaternion &q0, const Fquaternion &q1, float v1 )
{
//VERIFY(0.f =< v && 1.f >= v );
Fmatrix m0;m0.rotation(q0);
Fmatrix m,ml1;
q_scale_vs_basem( ml1, q1, base, v1 );
m.mul(m0,ml1);
q.set(m);
q.normalize();
}
bool is_similar( const Fmatrix &m0, const Fmatrix &m1, float param )
{
Fmatrix tmp1; tmp1.invert( m0 );
Fmatrix tmp2; tmp2.mul( tmp1, m1 );
Fvector ax;float ang;
Fquaternion q;
q.set( tmp2 );
q.get_axis_angle( ax, ang );
return _abs( ang )<M_PI/2.f;
}
IC bool cmp( const Fmatrix &f0, const Fmatrix &f1 )
{
Fmatrix if0; if0.invert( f0 );
Fmatrix cm;cm.mul_43( if0, f1 );
Fvector ax; float ang;
Fquaternion q;
q.set( cm );
q.get_axis_angle( ax, ang );
return ang < cmp_angle && cm.c.square_magnitude() < cmp_ldisp* cmp_ldisp ;
}