Point3D.cpp

Go to the documentation of this file.

//
// Point3D.cpp
// A point in the 3-D cartesian plane. All data is stored in 64-bit-"double"-precision.
//

#define _USE_MATH_DEFINES // for MSVC
#include <cmath>
#include "Point2D.hpp"
#include "Point3D.hpp"

namespace datatypes
{
        
//
// Some of the non-trivial member functions
//

//
// Constructs a point from the given ScanPoint
//
Point3D::Point3D(const Point2D& p)
        : m_x(double(p.getX()))
        , m_y(double(p.getY()))
        , m_z(0.0)
{
        m_datatype = Datatype_Point3D;
}


bool Point3D::isZero() const
{
        return (fuzzyCompare(m_x, 0.0) &&
                        fuzzyCompare(m_y, 0.0) &&
                        fuzzyCompare(m_z, 0.0));
}


double Point3D::dist( const Point3D& point ) const
{
        return hypot(m_x - point.getX(), m_y - point.getY(), m_z - point.getZ());
}

Point2D Point3D::toPoint2D() const
{
        Point2D result(m_x, m_y);
        return result;
}

Point3D Point3D::vectorProduct(const Point3D& v1, const Point3D& v2)
{
        Point3D kreuz;

        kreuz.m_x = + ((v1.getY() * v2.getZ()) - (v1.getZ() * v2.getY()));
        kreuz.m_y = -((v1.getX() * v2.getZ()) - (v1.getZ() * v2.getX()));
        kreuz.m_z = + ((v1.getX() * v2.getY()) - (v1.getY() * v2.getX()));

        return kreuz;
}

void Point3D::normalize()
{
        if (isZero())
                // Vector has zero length. Such a vector will be left unchanged.
                return;

        double len = length();
        // To be really sure about avoiding division-by-zero, we check again here
        if (fuzzyCompare(len, 0.0))
        {
                // Vector has zero length. Such a vector will be left unchanged.
                return;
        }

        *this /= len;
}


void Point3D::rotateAroundZ(double yawAngle)
{
        double dCos = cos(yawAngle);
        double dSin = sin(yawAngle);

        double x = m_x * dCos - m_y * dSin;
        double y = m_x * dSin + m_y * dCos;

        m_x = x;
        m_y = y;
}


void Point3D::rotateAroundX(double rollAngle)
{
        double dCos = cos(rollAngle);
        double dSin = sin(rollAngle);

        double y = -m_z * dSin + m_y * dCos;
        double z = m_z * dCos + m_y * dSin;

        m_z = z;
        m_y = y;
}

void Point3D::rotateAroundY(double pitchAngle)
{
        double dCos = cos(pitchAngle);
        double dSin = sin(pitchAngle);

        double x = m_z * dSin + m_x * dCos;
        double z = m_z * dCos - m_x * dSin;

        m_z = z;
        m_x = x;
}


double Point3D::distFromOrigin() const
{
        return hypot(m_x, m_y, m_z);
}

double Point3D::length() const
{
        return distFromOrigin();
}


double Point3D::getAngleAroundZ() const
{
        double angle;

        // atan2(y,x) returns the angle against the x-axis
        angle = std::atan2(m_y, m_x);

        return angle;
}

double Point3D::getAngleAroundY() const
{
        double angle;

        // atan2(y,x) returns the angle against the x-axis
        angle = std::atan2(m_x, m_z);

        return angle;
}


double Point3D::getAngleAroundX() const
{
        double angle;

        // atan2(z,y) returns the angle against the y-axis
        angle = std::atan2(m_z, m_y);

        return angle;
}



double Point3D::getDistanceBetweenPoints(const Point3D& pt1, const Point3D& pt2)
{
        return pt1.dist(pt2);
}





std::string Point3D::toString() const
{
//      std::string text;
        std::ostringstream ostr;
        ostr << *this;
        return ostr.str();
}

std::ostream& operator<<(std::ostream& os, const Point3D& point)
{
        os << "(" << point.getX() << ", " << point.getY() << ", " << point.getZ() << ")";
        return os;
}


Point3D Point3D::calcIntersectionPointOfVectorWithPlane(const Point3D& PlaneStartpoint,
                const Point3D& PlaneNormal,
                const Point3D& VectorStartpoint,
                const Point3D& VectorDirection)
{
        Point3D intersectionPoint;
        double nenner;

        nenner = PlaneNormal * VectorDirection;
        if (fuzzyCompare(nenner, 0.0) == false)
        {
                // Calc intersection point
                intersectionPoint = VectorStartpoint + ((PlaneNormal * (PlaneStartpoint - VectorStartpoint)) / nenner) * VectorDirection;
        }
        else
        {
                // Vector points "along" the plane, so there is no intersection point.
                intersectionPoint.setXYZ(NaN_double, NaN_double, NaN_double);
        }

        return intersectionPoint;
}

}       // namespace datatypes