/***************************************************************************
 *            vector.c
 *
 *  Copyright  2010 - Steffen Pohle
 *  steffen@gulpe.de
 ****************************************************************************/

/*
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 */

#include <math.h>
#include "osmroute.h"
#include "vector.h"

inline fPoint vec_add (fPoint veca, fPoint vecb) {
	fPoint r;

	r.x = veca.x + vecb.x;
	r.y = veca.y + vecb.y;

	return r;	
};


inline fPoint vec_sub (fPoint veca, fPoint vecb) {
	fPoint r;

	r.x = veca.x - vecb.x;
	r.y = veca.y - vecb.y;

	return r;	
};


fPoint vec_normale (fPoint veca, fPoint vecb) {
	fPoint ret;
	fPoint veca0;
	float a, sklprd;

	/* get vector a */
	a = sqrt (veca.x * veca.x + veca.y * veca.y);
	veca0.x = veca.x / a;
	veca0.y = veca.y / a;

	/* scalarprodukt */
	sklprd = vecb.x * veca0.x + vecb.y * veca0.y;
	ret.x = sklprd * veca0.x;
	ret.y = sklprd * veca0.y;
	
	return ret;
};


float vec_len (fPoint veca) {
	return sqrtf (veca.x * veca.x + veca.y * veca.y);
};


fPoint vec_one (fPoint veca) {
	fPoint a;
	float l = vec_len (veca);

	a.x = veca.x/l;
	a.y = veca.y/l;
	
	return a;
};


fPoint vec_turn90 (fPoint veca) {
	float len = vec_len (veca);
	fPoint a;
	float angle = atan2 (veca.y, veca.x);
	
	angle += M_PI_2;
	a.x = cos (angle) * len;
	a.y = sin (angle) * len;

	return a;
};


fPoint vec_mirror (fPoint veca) {
	fPoint a;

	a.x = -veca.x;
	a.y = -veca.y;
	return a;
};


fPoint vec_mul (fPoint v, float f) {
	fPoint a;

	a.x = f * v.x;
	a.y = f * v.y;
	return a;
};


/* calc angle of three points assuming v2 is the center.. */
float point_angle (fPoint v1, fPoint v2, fPoint v3) {
	float a1;
	float a3;
	float delta;

	v1 = vec_sub (v1, v2);
	v3 = vec_sub (v3, v2);
	a1 = atan2f (v1.y, v1.x);
	a3 = atan2f (v3.y, v3.x);

	if (a1 < 0.0) a1 += 2.0*M_PI;
	if (a3 < 0.0) a3 += 2.0*M_PI;
	if ((delta = a1-a3) < 0.0) delta += 2.0*M_PI;

	return delta;
};