{ RCSid $Id: mat3.cal,v 1.3 2022/12/02 02:04:49 greg Exp $ }
{
	3x3 Matrix operations

	i's are rows, j's are columns, indexed from 0
}

sq(x) : x*x;

MatDot(A,B,i,j) : A(i,0)*B(0,j) + A(i,1)*B(1,j) + A(i,2)*B(2,j);

MatDotVec(A,v,i) : A(i,0)*v(0) + A(i,1)*v(1) + A(i,2)*v(2);

i3(i) : if(i-2.5, i-3, i);

MinorDet(M,i,j) : M(i3(i+1),i3(j+1))*M(i3(i+2),i3(j+2)) -
			M(i3(i+1),i3(j+2))*M(i3(i+2),i3(j+1));

Det(M) : M(0,0)*MinorDet(M,0,0) +
		M(0,1)*MinorDet(M,0,1) +
		M(0,2)*MinorDet(M,0,2);

Inv(M,i,j) : MinorDet(M,j,i) / Det(M);

NormV(v,i) : v(i) / sqrt(sq(v(0))+sq(v(1))+sq(v(2)));

{ Rotation matrix computed from normalized quaternion [W X Y Z] }

NormQ(q,i) : q(i) / sqrt(sq(q(0))+sq(q(1))+sq(q(2))+sq(q(3)));

RotMatQ(q,i,j) : select(3*i+j+1,
	1 - 2*(sq(q(2))+sq(q(3))),
		2*(q(1)*q(2) - q(0)*q(3)),
		2*(q(0)*q(2) + q(1)*q(3)),
	2*(q(1)*q(2) + q(0)*q(3)),
		1 - 2*(sq(q(1))+sq(q(3))),
		2*(q(2)*q(3) - q(0)*q(1)),
	2*(q(1)*q(3) - q(0)*q(2)),
		2*(q(0)*q(1) + q(2)*q(3)),
		1 - 2*(sq(q(1))+sq(q(2))) );