Headtracking-VTK-Kinect/SkeletonMath.cpp at master · bmwesting/Headtracking-VTK-Kinect · 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
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
#include <stdio.h>

#include "SkeletonMath.h"

Vector::Vector()
{
}

Vector::Vector(const Point* p)
{

    point_.x_ = p->x_;
    point_.y_ = p->y_;
    point_.z_ = p->z_;
    point_.confidence_ = p->confidence_;

}

Vector::Vector(const Point& p)
{

    point_.x_ = p.x_;
    point_.y_ = p.y_;
    point_.z_ = p.z_;
    point_.confidence_ = p.confidence_;

}

Vector::Vector(const float x, const float y, const float z)
{
    point_.x_ = x;
    point_.y_ = y;
    point_.z_ = z;
}

Vector::Vector(const int x, const int y, const int z)
{
    point_.x_ = x;
    point_.y_ = y;
    point_.z_ = z;
}

Vector::~Vector()
{
}

Vector& Vector::operator=(const Vector& rhs)
{

    // they are pointing to the same location!
    if (this == &rhs)
        return *this;

    point_.x_ = rhs.getPoint().x_;
    point_.y_ = rhs.getPoint().y_;
    point_.z_ = rhs.getPoint().z_;
    point_.confidence_ = rhs.getPoint().confidence_;

}

Vector& Vector::operator+=(const Vector& rhs)
{
    point_.x_ = point_.x_ + rhs.getPoint().x_;
    point_.y_ = point_.y_ + rhs.getPoint().y_;
    point_.z_ = point_.z_ + rhs.getPoint().z_;

    return *this;

}

Vector& Vector::operator-=(const Vector& rhs)
{
    point_.x_ = point_.x_ - rhs.getPoint().x_;
    point_.y_ = point_.y_ - rhs.getPoint().y_;
    point_.z_ = point_.z_ - rhs.getPoint().z_;

    return *this;
}

Vector& Vector::operator*=(const Vector& rhs)
{
    point_.x_ = point_.x_ * rhs.getPoint().x_;
    point_.y_ = point_.y_ * rhs.getPoint().y_;
    point_.z_ = point_.z_ * rhs.getPoint().z_;

    return *this;
}

const Vector Vector::operator+(const Vector& rhs) const
{
    Vector result = *this;
    result += rhs;
    return result;
}

const Vector Vector::operator-(const Vector& rhs) const
{
    Vector result = *this;
    result -= rhs;
    return result;
}

const Vector Vector::operator*(const Vector& rhs) const
{
    Vector result = *this;
    result *= rhs;
    return result;
}

const Vector Vector::operator*(const float f) const
{
    Vector result = *this;
    result.setPoint(result.getPoint().x_ * f, result.getPoint().y_ * f, result.getPoint().z_ * f);
    return result;
}

void Vector::print() const
{
    printf("point_.x_:       %f\n", point_.x_);
    printf("point_.y_:       %f\n", point_.y_);
    printf("point_.z_:       %f\n", point_.z_);
}

Vector Vector::crossProduct(const Vector& rhs)
{
    Vector c = Vector(point_.y_*rhs.getPoint().z_ - point_.z_*rhs.getPoint().y_,
                point_.z_*rhs.getPoint().x_ - point_.x_*rhs.getPoint().z_,
                point_.x_*rhs.getPoint().y_ - point_.y_*rhs.getPoint().x_);

    return c;
}

float Vector::dotProduct(const Vector& rhs)
{
    return point_.x_*rhs.getPoint().x_ +
           point_.y_*rhs.getPoint().y_ +
           point_.z_*rhs.getPoint().z_;
}