IC void q_scale(Fquaternion &q, float v) { float angl;Fvector ax; q.get_axis_angle(ax,angl); q.rotation(ax,angl*v); 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 ; }