| 1 |
{ RCSid $Id: mat3.cal,v 1.2 2009/03/24 01:41:40 greg Exp $ } |
| 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); |
| 13 |
|
| 14 |
i3(i) : if(i-2.5, i-3, i); |
| 15 |
|
| 16 |
MinorDet(M,i,j) : M(i3(i+1),i3(j+1))*M(i3(i+2),i3(j+2)) - |
| 17 |
M(i3(i+1),i3(j+2))*M(i3(i+2),i3(j+1)); |
| 18 |
|
| 19 |
Det(M) : M(0,0)*MinorDet(M,0,0) + |
| 20 |
M(0,1)*MinorDet(M,0,1) + |
| 21 |
M(0,2)*MinorDet(M,0,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))) ); |
