Newer
Older
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
#include "DataHelper/LineClass.h"
#include <math.h>
/*
* Constructor
*/
LineClass::LineClass(float x0,
float y0,
float z0,
float ax,
float ay,
float az) {
_x0[0] = x0;
_x0[1] = y0;
_x0[2] = z0;
_ax[0] = ax;
_ax[1] = ay;
_ax[2] = az;
}
LineClass::LineClass(float *x0,
float *ax) {
for (int i=0; i<3; ++i) {
_x0[i] = x0[i];
_ax[i] = ax[i];
}
}
float * LineClass::getReferencePoint() {
return _x0;
}
float * LineClass::getDirectionalVector() {
return _ax;
}
void LineClass::setReferencePoint(float *x0) {
for (int i=0; i<3; ++i)
_x0[i] = x0[i];
}
void LineClass::setDirectionalVector(float *ax) {
for (int i=0; i<3; ++i)
_ax[i] = ax[i];
}
float LineClass::getDistanceToPoint(float * xpoint, float * pos) {
float dif[3];
float prod = 0;
float den = 0;
for (int i=0; i<3; ++i) {
dif[i] = xpoint[i] - _x0[i];
prod += _ax[i]*dif[i];
den += _ax[i]*_ax[i];
}
float time = prod/fmax(1e-10,den);
float dist = 0.0;
for (int i=0; i<3; ++i) {
pos[i] = _x0[i] + _ax[i]*time;
dist += (xpoint[i]-pos[i])*(xpoint[i]-pos[i]);
}
dist = sqrt(dist);
return dist;
}