/////////////////////////////////////////////////////////////////////
// File: Vector.h                                                  //
// Date: 11/8/09                                                   //
// Desc: 2D vector definition. Modified from http://softsurfer.com //
//                                                                 //
//  Copyright 2003, softSurfer (www.softsurfer.com)                //
//  This code may be freely used and modified for any purpose      //
//  providing that this copyright notice is included with it.      //
//  SoftSurfer makes no warranty for this code, and cannot be held //
//  liable for any real or imagined damage resulting from its use. //
//  Users of this code must verify correctness for their           //
//  application.                                                   //
//                                                                 //
/////////////////////////////////////////////////////////////////////

#ifndef _POINT_H_
#define _POINT_H_

#include <math.h>

namespace physics {

template <class T>
class Vector;

template <class T>
class Point {
    friend Vector<T>;
public:
    T x, y;

    Point() : x(0), y(0) {}
    Point(T x, T y) : x(x), y(y) {}
	~Point() {};

	//----------------------------------------------------------
	// Comparison
	bool operator==(Point);
	bool operator!=(Point);

	//----------------------------------------------------------
	// Point and Vector Operations (always valid) 
	Vector operator-(Point);       // Vector difference
	Point  operator+(Vector);      // +translate
	Point  operator-(Vector);      // -translate
	Point& operator+=(Vector);     // inc translate
	Point& operator-=(Vector);     // dec translate

	//----------------------------------------------------------
	// Point Scalar Operations (convenient but often illegal)
	// using any type of scalar (int, float, or double)
	//    are not valid for points in general,
	//    unless they are 'affine' as coeffs of 
	//    a sum in which all the coeffs add to 1,
	//    such as: the sum (a*P + b*Q) with (a+b == 1).
	//    The programmer must enforce this (if they want to).

	// Scalar Multiplication
	friend Point operator*(T, Point);
	friend Point operator*(Point, T);
	// Scalar Division
	friend Point operator/(Point, T);

	//----------------------------------------------------------
	// Point Addition (also convenient but often illegal)
	//    is not valid unless part of an affine sum.
	//    The programmer must enforce this (if they want to).
	friend Point operator+(Point, Point);     // add points

	//----------------------------------------------------------
	// Point Relations
	friend T dist(Point, Point);         // Distance
	friend T distSquared(Point, Point);  // Distance^2
};

}  // namespace physics

#endif  // _POINT_H_

#include "ballistic/include.h"
#include "Point.h"
#include "Vector.h"
using namespace physics;


//------------------------------------------------------------------
// Comparison
//------------------------------------------------------------------

template <class T>
bool Point<T>::operator==(Point<T> q) {
	return (x == q.x && y == q.y);
}

template <class T>
bool Point<T>::operator!=(Point<T> q) {
	return (x != q.x || y != q.y);
}

//------------------------------------------------------------------
// Point Vector Operations
//------------------------------------------------------------------

template <class T>
Vector<T> Point<T>::operator-(Point<T> q) {   // Vector diff of Points
	Vector<T> v;
	v.x = x - q.x;
	v.y = y - q.y;
	return v;
}

template <class T>
Point<T> Point<T>::operator+(Vector<T> v) {   // +ve translation
	Point<T> p;
	p.x = x + v.x;
	p.y = y + v.y;
	return p;
}

template <class T>
Point<T> Point<T>::operator-(Vector<T> v) {   // -ve translation
	Point<T> p;
	p.x = x - v.x;
	p.y = y - v.y;
	return p;
}

template <class T>
Point<T>& Point<T>::operator+=(Vector<T> v) { // +ve translation
	x += v.x;
	y += v.y;
	return *this;
}

template <class T>
Point<T>& Point<T>::operator-=(Vector<T> v) { // -ve translation
	x -= v.x;
	y -= v.y;
	return *this;
}

//------------------------------------------------------------------
// Point Scalar Operations (convenient but often illegal)
//        are not valid for points in general,
//        unless they are 'affine' as coeffs of 
//        a sum in which all the coeffs add to 1,
//        such as: the sum (a*P + b*q) with (a+b == 1).
//        The programmer must enforce this (if they want to).
//------------------------------------------------------------------

template <class T>
Point<T> operator*(T c, Point<T> q) {
	Point<T> p;
	p.x = c * q.x;
	p.y = c * q.y;
	return p;
}

template <class T>
Point<T> operator*(Point<T> q, T c) {
	Point<T> p;
	p.x = c * q.x;
	p.y = c * q.y;
	return p;
}

template <class T>
Point<T> operator/(Point<T> q, T c) {
	Point<T> p;
	p.x = q.x / c;
	p.y = q.y / c;
	return p;
}

//------------------------------------------------------------------
// Point Addition (also convenient but often illegal)
//    is not valid unless part of an affine sum.
//    The programmer must enforce this (if they want to).
//------------------------------------------------------------------
template <class T>
Point<T> operator+(Point<T> q, Point<T> r) {
	Point<T> p;
	p.x = q.x + r.x;
	p.y = q.y + r.y;
	return p;
}

//------------------------------------------------------------------
// Distance between Points
//------------------------------------------------------------------

template <class T>
T dist(Point<T> p, Point<T> q) {      // Euclidean distance
	T dx = p.x - q.x;
	T dy = p.y - q.y;
	return sqrt(dx*dx + dy*dy);
}

template <class T>
T distSquared(Point<T> p, Point<T> q) {// squared distance (more efficient)
	T dx = p.x - q.x;
	T dy = p.y - q.y;
	return (dx*dx + dy*dy);
}

#ifndef _VECTOR_H_
#define _VECTOR_H_

#include "Point.h"

namespace physics {

template <class T>
class Vector : public Point<T> {
public:
    Vector() : x(0), y(0) {}
    Vector(T x, T y) : x(x), y(y) {}
    static Vector fromPolar(T dist, double angle);

    T length() { return (T)sqrt(x*x + y*y); }

    //----------------------------------------------------------
	// Vector Unary Operations
	Vector operator-();                // unary minus
	Vector operator~();                // unary 2D perp operator

	//----------------------------------------------------------
	// Scalar Multiplication
	friend Vector operator*(T, Vector);
	friend Vector operator*(Vector, T);
	// Scalar Division
	friend Vector operator/(Vector, T);

	//----------------------------------------------------------
	// Vector Arithmetic Operations
	Vector operator+(Vector);      // vector add
	Vector operator-(Vector);      // vector subtract
	T operator*(Vector);           // inner dot product
	T operator|(Vector);           // 2D exterior perp product

	Vector& operator*=(T);         // vector scalar mult
	Vector& operator/=(T);         // vector scalar div
	Vector& operator+=(Vector);    // vector increment
	Vector& operator-=(Vector);    // vector decrement

    // Returns a normalized vector from this one
    Vector normalized();

    // Normalizes the given vector
    static void normalize(Vector& v);
};

}  // namespace physics

#endif  // _VECTOR_H_

#include "ballistic/include.h"
#include "Vector.h"
using namespace physics;


template <class T>
Vector<T> Vector<T>::fromPolar(T dist, double angle) {
    return Vector((T)(dist * cos(degToRad(angle))),
                  (T)(dist * sin(degToRad(angle))));
}

//------------------------------------------------------------------
//  Unary Ops
//------------------------------------------------------------------

// unary minus
template <class T>
Vector<T> Vector<T>::operator-() {
	Vector<T> v;
	v.x = -x; v.y = -y;
	return v;
}

// unary 2D perp operator
template <class T>
Vector<T> Vector<T>::operator~() {
	Vector<T> v;
	v.x = -y; v.y = x;
	return v;
}

//------------------------------------------------------------------
//  Scalar Ops
//------------------------------------------------------------------

// Scalar Multiplies
template <class T>
Vector<T> operator*(T c, Vector<T> w) {
	Vector<T> v;
	v.x = c * w.x;
	v.y = c * w.y;
	return v;
}

template <class T>
Vector<T> operator*(Vector<T> w, T c) {
	Vector<T> v;
	v.x = c * w.x;
	v.y = c * w.y;
	return v;
}

// Scalar Divides
template <class T>
Vector<T> operator/(Vector<T> w, T c) {
	Vector<T> v;
	v.x = w.x / c;
	v.y = w.y / c;
	return v;
}

//------------------------------------------------------------------
//  Arithmetic Ops
//------------------------------------------------------------------

template <class T>
Vector<T> Vector<T>::operator+(Vector<T> w) {
	Vector<T> v;
	v.x = x + w.x;
	v.y = y + w.y;
	return v;
}

template <class T>
Vector<T> Vector<T>::operator-(Vector<T> w) {
	Vector<T> v;
	v.x = x - w.x;
	v.y = y - w.y;
	return v;
}

//------------------------------------------------------------------
//  Products
//------------------------------------------------------------------

// Inner Dot Product
template <class T>
T Vector<T>::operator*(Vector<T> w) {
	return (x * w.x + y * w.y);
}

// 2D Exterior Perp Product
template <class T>
T Vector<T>::operator|(Vector<T> w) {
	return (x * w.y - y * w.x);
}

//------------------------------------------------------------------
//  Shorthand Ops
//------------------------------------------------------------------

template <class T>
Vector<T>& Vector<T>::operator*=(T c) {         // vector scalar mult
	x *= c;  y *= c;
	return *this;
}

template <class T>
Vector<T>& Vector<T>::operator/=(T c) {         // vector scalar div
	x /= c;  y /= c;
	return *this;
}

template <class T>
Vector<T>& Vector<T>::operator+=(Vector<T> w) { // vector increment
	x += w.x;  y += w.y;
	return *this;
}

template <class T>
Vector<T>& Vector<T>::operator-=(Vector<T> w) { // vector decrement
	x -= w.x;  y -= w.y;
	return *this;
}

template <class T>
Vector<T> Vector<T>::normalized() {
    if (x == 0 && y == 0) return Vector();
    T len = length();
    Vector<T> v;
    v.x = x / len;
    v.y = y / len;
    return v;
}

template <class T>
void Vector<T>::normalize(Vector& v) {
    if (v.x == 0 && v.y == 0) return;
	T len = v.length();
    v.x /= len;
    v.y /= len;
}