| 1 |
|
{ RCSid $Id$ } |
| 2 |
|
{ |
| 3 |
|
3x3 Matrix operations |
| 4 |
+ |
|
| 5 |
+ |
i's are rows, j's are columns, indexed from 0 |
| 6 |
|
} |
| 7 |
|
|
| 8 |
+ |
sq(x) : x*x; |
| 9 |
+ |
|
| 10 |
|
MatDot(A,B,i,j) : A(i,0)*B(0,j) + A(i,1)*B(1,j) + A(i,2)*B(2,j); |
| 11 |
|
|
| 12 |
|
MatDotVec(A,v,i) : A(i,0)*v(0) + A(i,1)*v(1) + A(i,2)*v(2); |
| 22 |
|
|
| 23 |
|
Inv(M,i,j) : MinorDet(M,j,i) / Det(M); |
| 24 |
|
|
| 25 |
+ |
NormV(v,i) : v(i) / sqrt(sq(v(0))+sq(v(1))+sq(v(2))); |
| 26 |
+ |
|
| 27 |
+ |
{ Rotation matrix computed from normalized quaternion [W X Y Z] } |
| 28 |
+ |
|
| 29 |
+ |
NormQ(q,i) : q(i) / sqrt(sq(q(0))+sq(q(1))+sq(q(2))+sq(q(3))); |
| 30 |
+ |
|
| 31 |
+ |
RotMatQ(q,i,j) : select(3*i+j+1, |
| 32 |
+ |
1 - 2*(sq(q(2))+sq(q(3))), |
| 33 |
+ |
2*(q(1)*q(2) - q(0)*q(3)), |
| 34 |
+ |
2*(q(0)*q(2) + q(1)*q(3)), |
| 35 |
+ |
2*(q(1)*q(2) + q(0)*q(3)), |
| 36 |
+ |
1 - 2*(sq(q(1))+sq(q(3))), |
| 37 |
+ |
2*(q(2)*q(3) - q(0)*q(1)), |
| 38 |
+ |
2*(q(1)*q(3) - q(0)*q(2)), |
| 39 |
+ |
2*(q(0)*q(1) + q(2)*q(3)), |
| 40 |
+ |
1 - 2*(sq(q(1))+sq(q(2))) ); |
