#include "vec.h"
void vadd(Vector *a, Vector *b, Vector *c) {
    c->x = a->x + b->x;
    c->y = a->y + b->y;
    c->z = a->z + b->z;
}
void vsub(Vector *a, Vector *b, Vector *c) {
    c->x = a->x - b->x;
    c->y = a->y - b->y;
    c->z = a->z - b->z;
}
double dotprod(Vector *a, Vector *b) {
    return(a->x * b->x + a->y * b->y + a->z * b->z);
}
void crossprod(Vector *a, Vector *b, Vector *c) {
    Vector d;
    
    d.x = a->y * b->z - a->z * b->y;
    d.y = a->z * b->x - a->x * b->z;
    d.z = a->x * b->y - a->y * b->x;
    *c = d;
}
void smul(double s, Vector *a, Vector *b) {
    b->x = a->x * s;
    b->y = a->y * s;
    b->z = a->z * s;
}
void vnorm(Vector *src, Vector *dst) {
    double len = sqrt(dotprod(src,src));
    smul(1.0/len,src,dst);
}
void vmul(double m[NMAT][NMAT], Vector *a, Vector *b) {
    Vector c;
    c.x = m[0][0] * a->x + m[0][1] * a->y + m[0][2] * a->z + m[0][3];
    c.y = m[1][0] * a->x + m[1][1] * a->y + m[1][2] * a->z + m[1][3];
    c.z = m[2][0] * a->x + m[2][1] * a->y + m[2][2] * a->z + m[2][3];
    *b = c;
}
void mmult(double a[NMAT][NMAT], double b[NMAT][NMAT], double c[NMAT][NMAT]) {
    double d[NMAT][NMAT];
    int i, j, k;
    for (i=0; i<NMAT; ++i) {
	for (j=0; j<NMAT; ++j) {
	    d[i][j] = 0;
	    for (k=0; k<NMAT; ++k) {
		d[i][j] += a[i][k] * b[k][j];
	    }
	}
    }
    bcopy(d,c,sizeof(double)*NMAT*NMAT);
}
void mset(double m[NMAT][NMAT],
	  double a00,double a01,double a02,double a03,
	  double a10,double a11,double a12,double a13,
	  double a20,double a21,double a22,double a23,
	  double a30,double a31,double a32,double a33) {
    m[0][0]=a00; m[0][1]=a01; m[0][2]=a02; m[0][3]=a03;
    m[1][0]=a10; m[1][1]=a11; m[1][2]=a12; m[1][3]=a13;
    m[2][0]=a20; m[2][1]=a21; m[2][2]=a22; m[2][3]=a23;
    m[3][0]=a30; m[3][1]=a31; m[3][2]=a32; m[3][3]=a33;
}
void getmatrix(double m[NMAT][NMAT],Vector *p,Vector *v) {
    double len_xy, len2_xy, len_xyz;
    double cos_theta, sin_theta, cos_phi, sin_phi;
    double a[NMAT][NMAT];
    len2_xy = v->x * v->x + v->y * v->y;
    len_xy = sqrt(len2_xy);
    len_xyz =sqrt(len2_xy + v->z * v->z);
    if (len_xy > 1e-10) {
	cos_theta = v->y / len_xy;
	sin_theta = v->x / len_xy;
    } else {
	cos_theta = 1.0;
	sin_theta = 0.0;
    }
    mset(m,
	 1,0,0,-p->x,
	 0,1,0,-p->y,
	 0,0,1,-p->z,
	 0,0,0,1);
    cos_phi = - v->z /len_xyz;
    sin_phi = len_xy / len_xyz;
    mset(a,
	 cos_theta,-sin_theta,0,0,
	 sin_theta,cos_theta,0,0,
	 0,0,1,0,
	 0,0,0,1);
    mmult(a,m,m);
    mset(a,
	 1,0,0,0,
	 0,cos_phi,sin_phi,0,
	 0,-sin_phi,cos_phi,0,
	 0,0,0,1);
    mmult(a,m,m);
    mset(a,
	 1,0,0,0,
	 0,1,0,0,
	 0,0,-1,0,
	 0,0,0,1);
    mmult(a,m,m);
}
void project(Vector *a, double d, Vector *ans) {
    ans->x = d * a->x / a->z;
    ans->y = d * a->y / a->z;
    ans->z = a->z;
}
void printvec(Vector *v) {
    printf("(%f,%f,%f)",v->x,v->y,v->z);
    fflush(stdout);
}
void printmatrix(double m[NMAT][NMAT]) {
    int i, j;
    for (i=0; i<NMAT; ++i) {
	for (j=0; j<NMAT; ++j) {
	    printf("%f  ",m[i][j]);
	}
	printf("\n");
    }
}