Real SO3CSpace::Distance(const Config& a,const Config& b) { Matrix3 Ra,Rb; GetRotation(a,Ra); GetRotation(b,Rb); Matrix3 Rrel; Rrel.mulTransposeB(Ra,Rb); AngleAxisRotation aa; aa.setMatrix(Rrel); return Abs(aa.angle); }
virtual Real Eval(Real x) { AngleAxisRotation aa; aa.setMatrix(temp); return aa.angle; }