Получить матрицу преобразования из трех известных точек

Question

У меня есть три 3d (x,y,z) точки. я получаю их, отслеживая углы объекта с помощью kinect.

Теперь я хочу перевести и повернуть 3d-модель соответственно.

я получаю крен и ход, делая это: (я использую openframeworks.cc, поэтому некоторые методы класса могут показаться странными людям)

ofVec3f v10 = pointB - pointA;
ofVec3f v20 = pointC - pointA;  


v10.normalize();
v20.normalize();

//create rotation matrix for roll+pitch relative to up vector 0,0,1
ofVec3f normaleVec = v10.crossed(v20);
ofVec3f fromVec = ofVec3f(0,0,1);       
ofVec3f toVec = normaleVec;

mMR0.makeRotationMatrix(fromVec,toVec);

чтобы получить заголовок / рыскание, я делаю это: ofVec3f myV0_flat = avePointA*mMR0.getInverse(); ofVec3f myV1_flat = avePointB*mMR0.getInverse();

//get points relative to origion
ofVec3f myV10_flat = myV1_flat - myV0_flat;
//create rotation matrix for heading relative to flat 2d plane
float angle = atan2(myV10_flat.x,myV10_flat.y)/M_PI*180;

mMR1.makeRotationMatrix(angle,fromVec);

и, наконец, создать матрицу перевода и объединить все матрицы:

    mMT1.makeTranslationMatrix(avePointD); //translate from origin 


ofMatrix4x4 mMc;
mMc = mMR0 * mMR1 * mMT1;

но когда моя 3d-модель вращается, кажется, что она всегда наклоняется под одним и тем же углом.

мой вопрос Как бы я рассчитал крен и шаг отдельно, чтобы я мог, где он падает и как это исправить.

Спасибо. s.