/*  Written by W. Taylor, Math Dept, Univ. of Colorado, Boulder,Colo
    80309-0426, ...![ucbvax,allegra]!nbires!boulder!wtaylor  or
    wtaylor@boulder (cs-net).  Last revised 8/22/87                   */

#include <math.h>
#include <stdio.h>
#include "coords.h"

#define PI 3.14159

Projective_point_3 homogenize_3(p)
point_3 p;
{
Projective_point_3 P;
P.coord_1 = p.coord_1;
P.coord_2 = p.coord_2;
P.coord_3 = p.coord_3;
P.coord_4 = 1.0;
return(P);
}

Projective_point_2 homogenize_2(p)
point_3 p;
{
Projective_point_2 P;
P.coord_1 = p.coord_1;
P.coord_2 = p.coord_2;
P.coord_3 = 1.0;
return(P);
}

point_3 affine_3(P)
Projective_point_3 P;
{
point_3 p;
p.coord_1 = P.coord_1/P.coord_4;
p.coord_2 = P.coord_2/P.coord_4;
p.coord_3 = P.coord_3/P.coord_4;
return(p);
}

point affine_2(P)
Projective_point_2 P;
{
point p;
p.x_coord = P.coord_1/P.coord_3;
p.y_coord = P.coord_2/P.coord_3;
return(p);
}

matrix_4_4 multiply_4_4_4(m,n)
matrix_4_4 m,n;
{
matrix_4_4 p;
p.a_11 = m.a_11*n.a_11 + m.a_12*n.a_21 + m.a_13*n.a_31 + m.a_14*n.a_41;
p.a_12 = m.a_11*n.a_12 + m.a_12*n.a_22 + m.a_13*n.a_32 + m.a_14*n.a_42;
p.a_13 = m.a_11*n.a_13 + m.a_12*n.a_23 + m.a_13*n.a_33 + m.a_14*n.a_43;
p.a_14 = m.a_11*n.a_14 + m.a_12*n.a_24 + m.a_13*n.a_34 + m.a_14*n.a_44;
p.a_21 = m.a_21*n.a_11 + m.a_22*n.a_21 + m.a_23*n.a_31 + m.a_24*n.a_41;
p.a_22 = m.a_21*n.a_12 + m.a_22*n.a_22 + m.a_23*n.a_32 + m.a_24*n.a_42;
p.a_23 = m.a_21*n.a_13 + m.a_22*n.a_23 + m.a_23*n.a_33 + m.a_24*n.a_43;
p.a_24 = m.a_21*n.a_14 + m.a_22*n.a_24 + m.a_23*n.a_34 + m.a_24*n.a_44;
p.a_31 = m.a_31*n.a_11 + m.a_32*n.a_21 + m.a_33*n.a_31 + m.a_34*n.a_41;
p.a_32 = m.a_31*n.a_12 + m.a_32*n.a_22 + m.a_33*n.a_32 + m.a_34*n.a_42;
p.a_33 = m.a_31*n.a_13 + m.a_32*n.a_23 + m.a_33*n.a_33 + m.a_34*n.a_43;
p.a_34 = m.a_31*n.a_14 + m.a_32*n.a_24 + m.a_33*n.a_34 + m.a_34*n.a_44;
p.a_41 = m.a_41*n.a_11 + m.a_42*n.a_21 + m.a_43*n.a_31 + m.a_44*n.a_41;
p.a_42 = m.a_41*n.a_12 + m.a_42*n.a_22 + m.a_43*n.a_32 + m.a_44*n.a_42;
p.a_43 = m.a_41*n.a_13 + m.a_42*n.a_23 + m.a_43*n.a_33 + m.a_44*n.a_43;
p.a_44 = m.a_41*n.a_14 + m.a_42*n.a_24 + m.a_43*n.a_34 + m.a_44*n.a_44;
return(p);
}

matrix_3_3 multiply_3_3_3(m,n)
matrix_3_3 m,n;
{
matrix_3_3 p;
p.a_11 = m.a_11*n.a_11 + m.a_12*n.a_21 + m.a_13*n.a_31;
p.a_12 = m.a_11*n.a_12 + m.a_12*n.a_22 + m.a_13*n.a_32;
p.a_13 = m.a_11*n.a_13 + m.a_12*n.a_23 + m.a_13*n.a_33;
p.a_21 = m.a_21*n.a_11 + m.a_22*n.a_21 + m.a_23*n.a_31;
p.a_22 = m.a_21*n.a_12 + m.a_22*n.a_22 + m.a_23*n.a_32;
p.a_23 = m.a_21*n.a_13 + m.a_22*n.a_23 + m.a_23*n.a_33;
p.a_31 = m.a_31*n.a_11 + m.a_32*n.a_21 + m.a_33*n.a_31;
p.a_32 = m.a_31*n.a_12 + m.a_32*n.a_22 + m.a_33*n.a_32;
p.a_33 = m.a_31*n.a_13 + m.a_32*n.a_23 + m.a_33*n.a_33;
return(p);
}

matrix_3_4 multiply_3_4_4(m,n)
matrix_3_4 m; matrix_4_4 n;
{
matrix_3_4 p;
p.a_11 = m.a_11*n.a_11 + m.a_12*n.a_21 + m.a_13*n.a_31 + m.a_14*n.a_41;
p.a_12 = m.a_11*n.a_12 + m.a_12*n.a_22 + m.a_13*n.a_32 + m.a_14*n.a_42;
p.a_13 = m.a_11*n.a_13 + m.a_12*n.a_23 + m.a_13*n.a_33 + m.a_14*n.a_43;
p.a_14 = m.a_11*n.a_14 + m.a_12*n.a_24 + m.a_13*n.a_34 + m.a_14*n.a_44;
p.a_21 = m.a_21*n.a_11 + m.a_22*n.a_21 + m.a_23*n.a_31 + m.a_24*n.a_41;
p.a_22 = m.a_21*n.a_12 + m.a_22*n.a_22 + m.a_23*n.a_32 + m.a_24*n.a_42;
p.a_23 = m.a_21*n.a_13 + m.a_22*n.a_23 + m.a_23*n.a_33 + m.a_24*n.a_43;
p.a_24 = m.a_21*n.a_14 + m.a_22*n.a_24 + m.a_23*n.a_34 + m.a_24*n.a_44;
p.a_31 = m.a_31*n.a_11 + m.a_32*n.a_21 + m.a_33*n.a_31 + m.a_34*n.a_41;
p.a_32 = m.a_31*n.a_12 + m.a_32*n.a_22 + m.a_33*n.a_32 + m.a_34*n.a_42;
p.a_33 = m.a_31*n.a_13 + m.a_32*n.a_23 + m.a_33*n.a_33 + m.a_34*n.a_43;
p.a_34 = m.a_31*n.a_14 + m.a_32*n.a_24 + m.a_33*n.a_34 + m.a_34*n.a_44;
return(p);
}

matrix_3_4 multiply_3_3_4(m,n)
matrix_3_3 m; matrix_3_4 n;
{
matrix_3_4 p;
p.a_11 = m.a_11*n.a_11 + m.a_12*n.a_21 + m.a_13*n.a_31;
p.a_12 = m.a_11*n.a_12 + m.a_12*n.a_22 + m.a_13*n.a_32;
p.a_13 = m.a_11*n.a_13 + m.a_12*n.a_23 + m.a_13*n.a_33;
p.a_14 = m.a_11*n.a_14 + m.a_12*n.a_24 + m.a_13*n.a_34;
p.a_21 = m.a_21*n.a_11 + m.a_22*n.a_21 + m.a_23*n.a_31;
p.a_22 = m.a_21*n.a_12 + m.a_22*n.a_22 + m.a_23*n.a_32;
p.a_23 = m.a_21*n.a_13 + m.a_22*n.a_23 + m.a_23*n.a_33;
p.a_24 = m.a_21*n.a_14 + m.a_22*n.a_24 + m.a_23*n.a_34;
p.a_31 = m.a_31*n.a_11 + m.a_32*n.a_21 + m.a_33*n.a_31;
p.a_32 = m.a_31*n.a_12 + m.a_32*n.a_22 + m.a_33*n.a_32;
p.a_33 = m.a_31*n.a_13 + m.a_32*n.a_23 + m.a_33*n.a_33;
p.a_34 = m.a_31*n.a_14 + m.a_32*n.a_24 + m.a_33*n.a_34;
return(p);
}

point_4 apply_4_4_1(m,n)
matrix_4_4 m; point_4 n;
{
point_4 p;
p.coord_1 = m.a_11*n.coord_1 + m.a_12*n.coord_2 + m.a_13*n.coord_3 +
							m.a_14*n.coord_4;
p.coord_2 = m.a_21*n.coord_1 + m.a_22*n.coord_2 + m.a_23*n.coord_3 +
							m.a_24*n.coord_4;
p.coord_3 = m.a_31*n.coord_1 + m.a_32*n.coord_2 + m.a_33*n.coord_3 +
							m.a_34*n.coord_4;
p.coord_4 = m.a_41*n.coord_1 + m.a_42*n.coord_2 + m.a_43*n.coord_3 +
							m.a_44*n.coord_4;
return(p);
}

point_3 apply_3_4_1(m,n)
matrix_3_4 m; point_4 n;
{
point_3 p;
p.coord_1 = m.a_11*n.coord_1 + m.a_12*n.coord_2 + m.a_13*n.coord_3 +
							m.a_14*n.coord_4;
p.coord_2 = m.a_21*n.coord_1 + m.a_22*n.coord_2 + m.a_23*n.coord_3 +
							m.a_24*n.coord_4;
p.coord_3 = m.a_31*n.coord_1 + m.a_32*n.coord_2 + m.a_33*n.coord_3 +
							m.a_34*n.coord_4;
return(p);
}

display_3_4(m)
matrix_3_4 m;
{
printf("%7.4f  ",m.a_11);printf("%7.4f  ",m.a_12);printf("%7.4f  ",m.a_13);printf("%7.4f  \n",m.a_14);
printf("%7.4f  ",m.a_21);printf("%7.4f  ",m.a_22);printf("%7.4f  ",m.a_23);printf("%7.4f  \n",m.a_24);
printf("%7.4f  ",m.a_31);printf("%7.4f  ",m.a_32);printf("%7.4f  ",m.a_33);printf("%7.4f  \n",m.a_34);
}

display_4_4(m)
matrix_4_4 m;
{
printf("%7.4f  ",m.a_11);printf("%7.4f  ",m.a_12);printf("%7.4f  ",m.a_13);printf("%7.4f  \n",m.a_14);
printf("%7.4f  ",m.a_21);printf("%7.4f  ",m.a_22);printf("%7.4f  ",m.a_23);printf("%7.4f  \n",m.a_24);
printf("%7.4f  ",m.a_31);printf("%7.4f  ",m.a_32);printf("%7.4f  ",m.a_33);printf("%7.4f  \n",m.a_34);
printf("%7.4f  ",m.a_41);printf("%7.4f  ",m.a_42);printf("%7.4f  ",m.a_43);printf("%7.4f  \n\n",m.a_44);
}