#include "geometric.h"

// compute dot product of 3d vectors/points
// p1 * p2
double dotprod(cart_point* p1, cart_point* p2)
{
	return((p1->x)*(p2->x) + (p1->y)*(p2->y) + (p1->z)*(p2->z));
}

// compute modulus
// || p ||
double modulus(cart_point* p)
{
	return(sqrt(p->x*p->x + p->y*p->y + p->z*p->z));
}

// compute modulus of cross product of 3d vectors/points
// || p1 x p2 ||
double mod_crossprod(cart_point* p1, cart_point* p2)
{
	return(sqrt((p1->y*p2->z - p1->z*p2->y)*(p1->y*p2->z - p1->z*p2->y) +
				(p1->z*p2->x - p1->x*p2->z)*(p1->z*p2->x - p1->x*p2->z) +
				(p1->x*p2->y - p1->y*p2->x)*(p1->x*p2->y - p1->y*p2->x)) );
}

// compute square of modulus
// || p ||^2
double sq_modulus(cart_point* p)
{
	return(p->x*p->x + p->y*p->y + p->z*p->z);
}

//Curvature radius ro = ||C'||^3 / ||C' x C''||
double curvature_rad_at_point(cart_point *dC, cart_point *ddC){
    double nom, denom, mod;
    
    mod =  modulus(dC);
    nom = pow(mod, 3);
    denom = mod_crossprod(dC, ddC);
    if(nom < 10e-12) return 10e-12;
    if(denom < 10e-12) return 10e12;
    return nom/denom;
}

double curvature_at_point(cart_point *dC, cart_point *ddC){
    double nom, denom, mod;

    mod =  modulus(dC);
    denom = pow(mod, 3);
    nom = mod_crossprod(dC, ddC);
    if(nom < 10e-7) return 10e-7;
    if(denom < 10e-7) return 10e7;
    return nom/denom;
}

double cart_dist(cart_point* Pt_end, cart_point* Pt_str)
{
	double dist=0.0;
	dist = sqrt(pow(((Pt_end->x) - (Pt_str->x)),2) + pow(((Pt_end->y) - (Pt_str->y)),2) + pow(((Pt_end->z) - (Pt_str->z)),2));
	return(dist);
}
