static inline void setRotateFromVectors( float * A, const float * u, const float * v) {
float lu2 = u[ 0 ] * u[ 0 ] + u[ 1 ] * u[ 1 ] + u[ 2 ] * u[ 2 ] ;
float lv2 = v[ 0 ] * v[ 0 ] + v[ 1 ] * v[ 1 ] + v[ 2 ] * v[ 2 ] ;
if ( lu2* lv2< 1.e -10 ) {
setIdentity( A) ;
return ;
}
float scaler = 1.0f / sqrtf( lu2* lv2) ;
float c = ( u[ 0 ] * v[ 0 ] + u[ 1 ] * v[ 1 ] + u[ 2 ] * v[ 2 ] ) * scaler;
float x = ( v[ 1 ] * u[ 2 ] - v[ 2 ] * u[ 1 ] ) * scaler;
float y = ( v[ 2 ] * u[ 0 ] - v[ 0 ] * u[ 2 ] ) * scaler;
float z = ( v[ 0 ] * u[ 1 ] - v[ 1 ] * u[ 0 ] ) * scaler;
float s2 = x* x + y* y + z* z;
if ( s2< 1.e -10 ) {
setIdentity( A) ;
return ;
}
float cs2 = ( 1.0f - c) / s2;
A[ 0 ] = c+ x* x* cs2; A[ 4 ] = x* y* cs2+ z; A[ 8 ] = x* z* cs2- y; A[ 12 ] = 0 ;
A[ 1 ] = x* y* cs2- z; A[ 5 ] = c+ y* y* cs2; A[ 9 ] = y* z* cs2+ x; A[ 13 ] = 0 ;
A[ 2 ] = x* z* cs2+ y; A[ 6 ] = y* z* cs2- x; A[ 10 ] = c+ z* z* cs2; A[ 14 ] = 0 ;
A[ 3 ] = 0 ; A[ 7 ] = 0 ; A[ 11 ] = 0 ; A[ 15 ] = 1 ;
}
c3RhdGljIGlubGluZSB2b2lkIHNldFJvdGF0ZUZyb21WZWN0b3JzKGZsb2F0ICpBLCBjb25zdCBmbG9hdCAqdSwgY29uc3QgZmxvYXQgKnYpIHsKICAgIGZsb2F0IGx1MiA9IHVbMF0qdVswXSArIHVbMV0qdVsxXSArIHVbMl0qdVsyXTsKICAgIGZsb2F0IGx2MiA9IHZbMF0qdlswXSArIHZbMV0qdlsxXSArIHZbMl0qdlsyXTsKICAgIGlmKGx1MipsdjI8MS5lLTEwKSB7CiAgICAgICAgc2V0SWRlbnRpdHkoQSk7CiAgICAgICAgcmV0dXJuOwogICAgfQogICAgZmxvYXQgc2NhbGVyID0gMS4wZi9zcXJ0ZihsdTIqbHYyKTsKCiAgICBmbG9hdCBjID0gKHVbMF0qdlswXSArIHVbMV0qdlsxXSArIHVbMl0qdlsyXSkqc2NhbGVyOwoKICAgIGZsb2F0IHggPSAodlsxXSp1WzJdIC0gdlsyXSp1WzFdKSpzY2FsZXI7CiAgICBmbG9hdCB5ID0gKHZbMl0qdVswXSAtIHZbMF0qdVsyXSkqc2NhbGVyOwogICAgZmxvYXQgeiA9ICh2WzBdKnVbMV0gLSB2WzFdKnVbMF0pKnNjYWxlcjsKCiAgICBmbG9hdCBzMiA9IHgqeCArIHkqeSArIHoqejsKICAgIGlmKHMyPDEuZS0xMCkgewogICAgICAgIHNldElkZW50aXR5KEEpOwogICAgICAgIHJldHVybjsKICAgIH0KICAgIGZsb2F0IGNzMiA9ICgxLjBmLWMpL3MyOwogICAgQVsgMF0gPSBjK3gqeCpjczI7ICAgQVsgNF0gPSB4KnkqY3MyK3o7ICAgQVsgOF0gPSB4KnoqY3MyLXk7ICAgQVsxMl0gPSAwOwogICAgQVsgMV0gPSB4KnkqY3MyLXo7ICAgQVsgNV0gPSBjK3kqeSpjczI7ICAgQVsgOV0gPSB5KnoqY3MyK3g7ICAgQVsxM10gPSAwOwogICAgQVsgMl0gPSB4KnoqY3MyK3k7ICAgQVsgNl0gPSB5KnoqY3MyLXg7ICAgQVsxMF0gPSBjK3oqeipjczI7ICAgQVsxNF0gPSAwOwogICAgQVsgM10gPSAwOyAgICAgICAgICAgQVsgN10gPSAwOyAgICAgICAgICAgQVsxMV0gPSAwOyAgICAgICAgICAgQVsxNV0gPSAxOwp9