/*
 * geometry.cpp
 *
 *  Created on: 14/06/2013
 *      Author: timothyw
 */

#include "hole_detector/geometry.h"

#include <cmath>

using crosbot::Point3D;

namespace casualty {

PolarPoint3D::PolarPoint3D(const PolarPoint3D& p) :
    Point3D(p)
{
    dist = p.dist;
    theta = p.theta;
    phi = p.phi;
}

PolarPoint3D::PolarPoint3D(Point3D& p) :
    Point3D(p)
{
    dist = sqrt(p.x * p.x + p.y * p.y + p.z * p.z);
    theta = RAD2DEG(acos(p.z / dist));
    phi = RAD2DEG(atan2(p.y, p.x));
}

PolarPoint3D::~PolarPoint3D() {
}

PolarColouredPoint::PolarColouredPoint(ColouredPoint& p) :
    ColouredPoint(p)
{
    dist = sqrt(p.point.x * p.point.x + p.point.y * p.point.y + p.point.z * p.point.z);
    theta = RAD2DEG(acos(p.point.z / dist));
    phi = RAD2DEG(atan2(p.point.y, p.point.x));
}

PolarColouredPoint::PolarColouredPoint(Point3D& p) :
    ColouredPoint(p.x, p.y, p.z, 0, 0, 0)
{
    dist = sqrt(p.x * p.x + p.y * p.y + p.z * p.z);
    theta = RAD2DEG(acos(p.z / dist));
    phi = RAD2DEG(atan2(p.y, p.x));
}

PolarColouredPoint::~PolarColouredPoint() {
}

} // namespace casualty
