//  vector.c
//
//  by John D. de Boer

#include "common.h"
#include "cstring.h"
#include "cyber.h"
#include "disk.h"
#include "floating.h"
#include "vector.h"


//  Vector functions

  void zerovector(vector *V) {
V->x=V->y=V->z=0.0; }

  void setvector(vector *V, real X, real Y, real Z) {
V->x=X; V->y=Y; V->z=Z; }

  void scalevector(vector *V, real M) {
V->x*=M; V->y*=M; V->z*=M; }

  real lengthsquared(const vector *V) {
return (V->x * V->x) + (V->y * V->y) + (V->z * V->z); }

  real vectorlength(const vector *V) {
return sqrt(lengthsquared(V)); }

  void vectorsum(const vector *A, const vector *B, vector *Sum) {
Sum->x= A->x + B->x; Sum->y= A->y + B->y; Sum->z= A->z + B->z; }

  void vectordifference(const vector *A, const vector *B, vector *Diff) {
Diff->x= A->x - B->x; Diff->y= A->y - B->y; Diff->z= A->z - B->z; }

  real differencesquared(const vector *A, const vector *B) {
vector C; vectordifference(A,B,&C); return lengthsquared(&C); }

  void vectorincr(vector *A, const vector *B) {
A->x+=B->x; A->y+=B->y; A->z+=B->z; }

  void vectordecr(vector *A, const vector *B) {
A->x-=B->x; A->y-=B->y; A->z-=B->z; }

  void vectorincrement(vector *A, real F, const vector *B) {
A->x+= F * B->x; A->y+= F * B->y; A->z+= F * B->z; }

  void vectormean(const vector *A, const vector *B, vector *M) {
M->x= (A->x + B->x) / 2.0; M->y= (A->y + B->y) / 2.0; M->z= (A->z + B->z) / 2.0; }

  void vectorcomb(const vector *A, real F, const vector *B, vector *M) {
vector C; vectordifference(B,A,&C); scalevector(&C,F); vectorsum(A,&C,M); }

  real dotproduct(const vector *A, const vector *B) {
return (A->x * B->x) + (A->y * B->y) + (A->z * B->z); }

  void crossproduct(const vector *A, const vector *B, vector *C) {
real X,Y,Z;
X= (A->y * B->z) - (A->z * B->y);
Y= (A->z * B->x) - (A->x * B->z);
Z= (A->x * B->y) - (A->y * B->x);
C->x=X; C->y=Y; C->z=Z; }

  void normalise(vector *V) {
real L; if (!V) return; L=vectorlength(V); if (isnan(L) || L<=0.0) return;
V->x/=L; V->y/=L; V->z/=L; }


//  Polar vectors

  void vectortopolar(const vector *V, polar *R) {
real Rho2; if (!V || !R) return; R->phi=atan2(V->y,V->x);
Rho2= (V->x * V->x) + (V->y * V->y); R->theta=atan2(sqrt(Rho2),V->z);
R->r=sqrt(Rho2 + (V->z * V->z)); }

  void polartovector(const polar *R, vector *V){
real ST; if (!R || !V) return; ST=sin(R->theta);
V->x= R->r * ST * cos(R->phi); V->y= R->r * ST * sin(R->phi); V->z= R->r * cos(R->theta); }

  void cartesiantopolar(const vector *V, real *Rad, real *Theta, real *Phi) {
polar R; vectortopolar(V,&R); if (Rad) *Rad=R.r; if (Theta) *Theta=R.theta; if (Phi) *Phi=R.phi; }

  void polartocartesian(real Rad, real Theta, real Phi, vector *V) {
polar R; R.r=Rad; R.theta=Theta; R.phi=Phi; polartovector(&R,V); }

  void unitvector(real Theta, real Phi, vector *U) {
polar R; R.r=1.0; R.theta=Theta; R.phi=Phi; polartovector(&R,U); }


//  Simple rotations

void rotx090(vector *V) { real D; D=V->y; V->y=-V->z; V->z=D; }
void rotx180(vector *V) { V->y=-V->y; V->z=-V->z; }
void rotx270(vector *V) { real D; D=V->y; V->y=V->z; V->z=-D; }
void roty090(vector *V) { real D; D=V->z; V->z=-V->x; V->x=D; }
void roty180(vector *V) { V->z=-V->z; V->x=-V->x; }
void roty270(vector *V) { real D; D=V->z; V->z=V->x; V->x=-D; }
void rotz090(vector *V) { real D; D=V->x; V->x=-V->y; V->y=D; }
void rotz180(vector *V) { V->x=-V->x; V->y=-V->y; }
void rotz270(vector *V) { real D; D=V->x; V->x=V->y; V->y=-D; }

  void rotx(vector *V, real Theta) {
real C,S,Y; C=cos(Theta); S=sin(Theta); Y=V->y;
V->y= (C * Y) - (S * V->z);
V->z= (S * Y) + (C * V->z); }

  void roty(vector *V, real Theta) {
real C,S,Z; C=cos(Theta); S=sin(Theta); Z=V->z;
V->z= (C * Z) - (S * V->x);
V->x= (S * Z) + (C * V->x); }

  void rotz(vector *V, real Theta) {
real C,S,X; C=cos(Theta); S=sin(Theta); X=V->x;
V->x= (C * X) - (S * V->y);
V->y= (S * X) + (C * V->y); }


//  Bounds

  void initcuboid(cuboid *C, const vector *V) {
if (!C || !V) return;
C->x1=V->x; C->x2=V->x; C->y1=V->y; C->y2=V->y; C->z1=V->z; C->z2=V->z; }

  void stretchcuboid(cuboid *C, const vector *V) {
if (!C || !V) return;
if (V->x<C->x1) C->x1=V->x; if (V->x>C->x2) C->x2=V->x;
if (V->y<C->y1) C->y1=V->y; if (V->y>C->y2) C->y2=V->y;
if (V->z<C->z1) C->z1=V->z; if (V->z>C->z2) C->z2=V->z; }

  void unioncuboid(cuboid *C, const cuboid *D) {
if (!C || !D) return;
if (D->x1<C->x1) C->x1=D->x1; if (D->x2>C->x2) C->x2=D->x2;
if (D->y1<C->y1) C->y1=D->y1; if (D->y2>C->y2) C->y2=D->y2;
if (D->z1<C->z1) C->z1=D->z1; if (D->z2>C->z2) C->z2=D->z2; }


//  Matrix functions

  void identitymatrix(matrix *T) {
if (!T) return;
T->xx=1.0; T->xy=0.0; T->xz=0.0;
T->yx=0.0; T->yy=1.0; T->yz=0.0;
T->zx=0.0; T->zy=0.0; T->zz=1.0; }

  void transposematrix(matrix *T) {
real Temp; if (!T) return;
Temp=T->xy; T->xy=T->yx; T->yx=Temp;
Temp=T->xz; T->xz=T->zx; T->zx=Temp;
Temp=T->yz; T->yz=T->zy; T->zy=Temp; }

  void product(const matrix *M, const vector *A, vector *B) {
real X,Y,Z; if (!A || !M || !B) return; X=A->x; Y=A->y; Z=A->z;
B->x= (M->xx * X) + (M->xy * Y) + (M->xz * Z);
B->y= (M->yx * X) + (M->yy * Y) + (M->yz * Z);
B->z= (M->zx * X) + (M->zy * Y) + (M->zz * Z); }

  void matrixproduct(const matrix *R2, const matrix *R1, matrix *R) {
matrix M; if (!R2 || !R1 || !R) return;
M.xx= (R2->xx * R1->xx) + (R2->xy * R1->yx) + (R2->xz * R1->zx);
M.xy= (R2->xx * R1->xy) + (R2->xy * R1->yy) + (R2->xz * R1->zy);
M.xz= (R2->xx * R1->xz) + (R2->xy * R1->yz) + (R2->xz * R1->zz);
M.yx= (R2->yx * R1->xx) + (R2->yy * R1->yx) + (R2->yz * R1->zx);
M.yy= (R2->yx * R1->xy) + (R2->yy * R1->yy) + (R2->yz * R1->zy);
M.yz= (R2->yx * R1->xz) + (R2->yy * R1->yz) + (R2->yz * R1->zz);
M.zx= (R2->zx * R1->xx) + (R2->zy * R1->yx) + (R2->zz * R1->zx);
M.zy= (R2->zx * R1->xy) + (R2->zy * R1->yy) + (R2->zz * R1->zy);
M.zz= (R2->zx * R1->xz) + (R2->zy * R1->yz) + (R2->zz * R1->zz);
*R=M; }


//  Rotation matrices

  void xrotation(real Theta, matrix *R) {
real C,S; if (!R) return; C=cos(Theta); S=sin(Theta);
identitymatrix(R); R->yy=C; R->yz=-S; R->zy=S; R->zz=C; }

  void yrotation(real Theta, matrix *R) {
real C,S; if (!R) return; C=cos(Theta); S=sin(Theta);
identitymatrix(R); R->zz=C; R->zx=-S; R->xz=S; R->xx=C; }

  void zrotation(real Theta, matrix *R) {
real C,S; if (!R) return; C=cos(Theta); S=sin(Theta);
identitymatrix(R); R->xx=C; R->xy=-S; R->yx=S; R->yy=C; }

  void eulermatrix(real Precession, real Nutation, real Body, matrix *R) {
real sin1,cos1,sin2,cos2,sin3,cos3; if (!R) return;
sin1=sin(Precession); cos1=cos(Precession);
sin2=sin(Nutation);   cos2=cos(Nutation);
sin3=sin(Body);       cos3=cos(Body);
R->xx=  (cos1 * cos3) - (sin1 * cos2 * sin3);
R->xy=  (sin1 * cos3) + (cos1 * cos2 * sin3); R->xz= sin2 * sin3;
R->yx= -(cos1 * sin3) - (sin1 * cos2 * cos3);
R->yy= -(sin1 * sin3) + (cos1 * cos2 * cos3); R->yz= sin2 * cos3;
R->zx= sin1 * sin2; R->zy= -cos1 * sin2; R->zz= cos2; }

/*  void invertrotation(matrix *R) {
real Temp; if (!R) return;
Temp=R->xy; R->xy=R->yx; R->yx=Temp;
Temp=R->xz; R->xz=R->zx; R->zx=Temp;
Temp=R->yz; R->yz=R->zy; R->zy=Temp; }*/

  void invertrotation(matrix *R) {
transposematrix(R); }

  void rotationinverse(const matrix *R, matrix *R1) {
matrix M; if (!R || !R1) return; M=*R; invertrotation(&M); *R1=M;
/*R1->xx=R->xx; R1->xy=R->yx; R1->xz=R->zx;
R1->yx=R->xy; R1->yy=R->yy; R1->yz=R->zy;
R1->zx=R->xz; R1->zy=R->yz; R1->zz=R->zz;*/ }

  void rotatevector(const matrix *R, vector *V) {
product(R,V,V); }

  void unrotatevector(const matrix *R, vector *V) {
real X,Y,Z; if (!R || !V) return; X=V->x; Y=V->y; Z=V->z;
V->x= (R->xx * X) + (R->yx * Y) + (R->zx * Z);
V->y= (R->xy * X) + (R->yy * Y) + (R->zy * Z);
V->z= (R->xz * X) + (R->yz * Y) + (R->zz * Z); }

  void rotatematrix(const matrix *R, matrix *T) {
matrix M,N; if (!R || !T) return;
M.xx= (R->xx * T->xx) + (R->xy * T->yx) + (R->xz * T->zx);
M.xy= (R->xx * T->xy) + (R->xy * T->yy) + (R->xz * T->zy);
M.xz= (R->xx * T->xz) + (R->xy * T->yz) + (R->xz * T->zz);
M.yx= (R->yx * T->xx) + (R->yy * T->yx) + (R->yz * T->zx);
M.yy= (R->yx * T->xy) + (R->yy * T->yy) + (R->yz * T->zy);
M.yz= (R->yx * T->xz) + (R->yy * T->yz) + (R->yz * T->zz);
M.zx= (R->zx * T->xx) + (R->zy * T->yx) + (R->zz * T->zx);
M.zy= (R->zx * T->xy) + (R->zy * T->yy) + (R->zz * T->zy);
M.zz= (R->zx * T->xz) + (R->zy * T->yz) + (R->zz * T->zz);
N.xx= (M.xx  * R->xx) + (M.xy  * R->xy) + (M.xz  * R->xz);
N.xy= (M.xx  * R->yx) + (M.xy  * R->yy) + (M.xz  * R->yz);
N.xz= (M.xx  * R->zx) + (M.xy  * R->zy) + (M.xz  * R->zz);
N.yx= (M.yx  * R->xx) + (M.yy  * R->xy) + (M.yz  * R->xz);
N.yy= (M.yx  * R->yx) + (M.yy  * R->yy) + (M.yz  * R->yz);
N.yz= (M.yx  * R->zx) + (M.yy  * R->zy) + (M.yz  * R->zz);
N.zx= (M.zx  * R->xx) + (M.zy  * R->xy) + (M.zz  * R->xz);
N.zy= (M.zx  * R->yx) + (M.zy  * R->yy) + (M.zz  * R->yz);
N.zz= (M.zx  * R->zx) + (M.zy  * R->zy) + (M.zz  * R->zz);
*T=N; }

  void unrotatematrix(const matrix *R, matrix *T) {
matrix Temp; if (!R || !T) return;
Temp=*R; invertrotation(&Temp); rotatematrix(&Temp,T); }