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