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
|
//The number of point must be greater than 1. (Otherwise it's not a path.)
if (path.size() > 1) {
unsigned int currentPathIndex = 0;
//We check the direction of the first segment of the path and the direction between out actual position and the first point of the first segment of our path.
Vec3f dir1 = (path[currentPathIndex+1] - path[currentPathIndex]).normalize();
Vec3f dir2 = (actualPos - path[currentPathIndex]).normalize();
float d1 = (path[currentPathIndex+1] - path[currentPathIndex]).magnitude();
float d2 = (actualPos - path[currentPathIndex]).magnitude();
/* std::cout<<"dir 1 "<<dir1<<"dir 2 : "<<dir2<<"angle : "<<Math::toDegrees(dir1.getAngleBetween(dir2, dir1.cross(dir2)))<<std::endl;
std::cout<<"dist 1 : "<<d1<<" dist 2 : "<<d2<<std::endl;*/
//If the number of point is greater than 2, we need to check on which segment of the path our actual position is.
//If the directions are not the same it means that the point is not on this segment of the path.
while (currentPathIndex < path.size() - 1 && !dir2.isNulVector() && !(Math::abs(Math::toDegrees(dir1.getAngleBetween(dir2, dir1.cross(dir2)))) <= 0.1f && d2 < d1)) {
//We need to check if the actual position is on the next segment of the path.
currentPathIndex++;
//If we are arrived on the last point of the path, we don't need to check anymore, it means that our position is not on the path. (Or we are already at the end of the path)
if (currentPathIndex < path.size() - 1) {
dir1 = (path[currentPathIndex+1] - path[currentPathIndex]).normalize();
dir2 = (actualPos - path[currentPathIndex]).normalize();
d1 = (path[currentPathIndex+1] - path[currentPathIndex]).magnitude();
d2 = (actualPos - path[currentPathIndex]).magnitude();
/*std::cout<<"dir 1 : "<<dir1<<"dir 2 : "<<dir2<<"angle : "<<Math::toDegrees(dir1.getAngleBetween(dir2, dir1.cross(dir2)))<<std::endl;
std::cout<<"dist 1 : "<<d1<<" dist 2 : "<<d2<<std::endl;*/
//std::cout<<"current index path : "<<currentPathIndex<<std::endl;
}
}
//std::cout<<"current index path : "<<currentPathIndex<<"size : "<<path.size()<<std::endl;
/*std::string str;
std::cin>>str;*/
//Our actual position is not on the path so we return our actual position.
if (currentPathIndex == path.size() -1 && time > 0 || currentPathIndex == 0 && time < 0)
return actualPos;
//If the time is positive, we need to check a position on the path in the future.
if (time > 0) {
/*We check the direction of the segment, the next position and the distances between the two point of the segment and
* the distance between the next position and the first point of the segment.
*/
//dir1 = (path[currentPathIndex+1] - path[currentPathIndex]).normalize();
Vec3f nextPos = actualPos + dir1 * speed * time;
dir2 = (nextPos - path[currentPathIndex]).normalize();
d2 = (nextPos - path[currentPathIndex]).magnitude();
float dot = dir1.dot(dir2);
// std::cout<<"actual pos : "<<actualPos<<" next pos : "<<nextPos<<std::endl;
//std::cout<<"dir 1 : "<<dir1<<"dir 2 : "<<dir2<<"d1 : "<<d1<<"d2 : "<<d2<<std::endl;
/*If the actual position is not on the same segment of the path than the final position, we need to check on which segment of the path our next position"ll be.
* If the distance between the two points of the segment is shorter than the distance between the next position and the first point of the path, it means that
* the next position is not on this segment of the path.
*/
while (currentPathIndex < path.size() - 1 && !dir2.isNulVector() && d2 > d1) {
//We need to check the next position on the next segment of the path.
currentPathIndex++;
//If we are arrived on the last point of the path, we don't need to check anymore because we are at the end of the path.
if (currentPathIndex < path.size() - 1) {
dir1 = (path[currentPathIndex+1] - path[currentPathIndex]).normalize();
nextPos = path[currentPathIndex] + dir1 * (d2 - d1) * dot;
//std::cout<<"next pos : "<<nextPos<<"dir 1 : "<<dir1<<" dir 2 : "<<dir2<<" d1 : "<<d1<<" d2 : "<<d2<<"d2 - d1 : "<<d2 - d1<<" dot : "<<dot<<std::endl;
dir2 = (nextPos - path[currentPathIndex]).normalize();
dot = dir1.dot(dir2);
d1 = (path[currentPathIndex+1] - path[currentPathIndex]).magnitude();
d2 = (nextPos - path[currentPathIndex]).magnitude();
/*std::cout<<"dir 1 : "<<dir1<<" dir 2 : "<<dir2<<" d1 : "<<d1<<" d2 : "<<d2<<"d2 - d1 : "<<d2 - d1<<" dot : "<<dot<<std::endl;
std::string ok;
std::cin>>ok;*/
}
}
//std::cout<<"next pos : "<<nextPos<<" current path index : "<<currentPathIndex<<"size : "<<path.size()<<std::endl;
//If we reach the last point of the path it means that we are at the end of the path, otherwise we return the next position.
return (currentPathIndex == path.size() - 1) ? path[currentPathIndex] : nextPos;
}
//A negative time give a position on the path in the past, so, we need to do go back on our path.
if (time < 0) {
//The same as above except that we need to check the distance between the last point of the segment of the path.
dir1 = (path[currentPathIndex] - path[currentPathIndex+1]).normalize();
d1 = (path[currentPathIndex] - path[currentPathIndex+1]).magnitude();
Vec3f prevPos = actualPos - dir1 * speed * time;
dir2 = (prevPos - path[currentPathIndex+1]).normalize();
d2 = (prevPos - path[currentPathIndex+1]).magnitude();
float dot = dir1.dot(dir2);
std::cout<<"prev pos : "<<prevPos<<std::endl;
while (currentPathIndex > 0 && !dir2.isNulVector() && d2 > d1) {
currentPathIndex--;
//If the current index is 0, we don't need to check anymore because we are at the beginning of the path.
if (currentPathIndex > 0) {
dir1 = (path[currentPathIndex] - path[currentPathIndex+1]).normalize();
prevPos = path[currentPathIndex+1] - dir1 * (d1 - d2) * dot;
dir2 = (prevPos - path[currentPathIndex+1]).normalize();
dot = dir1.dot(dir2);
d1 = (path[currentPathIndex+1] - path[currentPathIndex]).magnitude();
d2 = (prevPos - path[currentPathIndex+1]).magnitude();
}
}
//If we reach the last point of the path it means that we are at the end of the path, otherwise we return the next position.
return (currentPathIndex == 0) ? path[currentPathIndex] : prevPos;
}
//If the time is equal to zero we have just to return the actual position.
return actualPos;
}
return actualPos; |
Concepts géométriques complexes
Répondre avec citation
Partager