geometry/src/vector3d.cpp at main · Navigine/geometry · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
#include <cmath>
#include <limits>

#include <navigine/geometry/vector3d.h>
#include <navigine/geometry/utils.h>

namespace math = std;

namespace navigine::geometry {

Vector3d::Vector3d()
    : x(0.0)
    , y(0.0)
    , z(0.0)
{}

Vector3d::Vector3d(double x, double y, double z)
    : x(x)
    , y(y)
    , z(z)
{}

double Vector3d::magnitude() const
{
    return math::sqrt(x * x + y * y + z * z);
}

double Vector3d::squaredNorm() const
{
    return x * x + y * y + z * z;
}

Vector3d Vector3d::normalized() const
{
    const auto length = magnitude();
    return length > std::numeric_limits<double>::epsilon() ?
        Vector3d(x / length, y / length, z / length) :
        *this;
}

Vector3d& Vector3d::normalize()
{
    double length = magnitude();
    if (std::fabs(length) > std::numeric_limits<double>::epsilon()) {
        *this /= length;
    }
    return *this;
}

bool Vector3d::isNull() const
{
    return x == 0 && y == 0 && z == 0;
}

bool Vector3d::operator==(const Vector3d& v) const
{
    return checkCloseValues(x, v.x) &&
           checkCloseValues(y, v.y) &&
           checkCloseValues(z, v.z);
}

bool Vector3d::operator!=(const Vector3d& v) const
{
    return !(*this == v);
}

Vector3d& Vector3d::operator+=(const Vector3d& v)
{
    x += v.x;
    y += v.y;
    z += v.z;
    return *this;
}

Vector3d& Vector3d::operator-=(const Vector3d& v)
{
    x -= v.x;
    y -= v.y;
    z -= v.z;
    return *this;
}

Vector3d& Vector3d::operator*=(double multiplier)
{
    x *= multiplier;
    y *= multiplier;
    z *= multiplier;
    return *this;
}

Vector3d& Vector3d::operator/=(double divisor)
{
    x /= divisor;
    y /= divisor;
    z /= divisor;
    return *this;
}

double Vector3d::dotProduct(const Vector3d& v1, const Vector3d& v2)
{
    return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
}

Vector3d Vector3d::crossProduct(const Vector3d& v1, const Vector3d& v2)
{
    return Vector3d(
        v1.y * v2.z - v1.z * v2.y,
        v1.z * v2.x - v1.x * v2.z,
        v1.x * v2.y - v1.y * v2.x);
}

std::ostream& operator<<(std::ostream& os, const Vector3d& v)
{
    os << "[" << v.x << ", " << v.y << ", " << v.z << "]";
    return os;
}

} // namespace navigine::geometry