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
|
/* This file is part of the Spring engine (GPL v2 or later), see LICENSE.html */
#include <cstdlib>
#include "PathFinderDef.h"
#include "Sim/MoveTypes/MoveDefHandler.h"
CPathFinderDef::CPathFinderDef(const float3& goalCenter, float goalRadius, float sqGoalDistance)
: goal(goalCenter)
, sqGoalRadius(goalRadius * goalRadius)
, constraintDisabled(false)
, testMobile(true)
, needPath(true)
, exactPath(true)
, dirIndependent(false)
, synced(true)
{
goalSquareX = goalCenter.x / SQUARE_SIZE;
goalSquareZ = goalCenter.z / SQUARE_SIZE;
// make sure that the goal can be reached with 2-square resolution
sqGoalRadius = std::max(sqGoalRadius, SQUARE_SIZE * SQUARE_SIZE * 2.0f);
startInGoalRadius = (sqGoalRadius >= sqGoalDistance);
}
// returns true when the goal is within our defined range
bool CPathFinderDef::IsGoal(unsigned int xSquare, unsigned int zSquare) const {
return (SquareToFloat3(xSquare, zSquare).SqDistance2D(goal) <= sqGoalRadius);
}
// returns distance to goal center in heightmap-squares
float CPathFinderDef::Heuristic(unsigned int xSquare, unsigned int zSquare) const
{
const float dx = std::abs(int(xSquare) - int(goalSquareX));
const float dz = std::abs(int(zSquare) - int(goalSquareZ));
// grid is 8-connected, so use octile distance
constexpr const float C1 = 1.0f;
constexpr const float C2 = 1.4142f - (2.0f * C1);
return ((dx + dz) * C1 + std::min(dx, dz) * C2);
}
// returns if the goal is inaccessable: this is
// true if the goal area is "small" and blocked
bool CPathFinderDef::IsGoalBlocked(const MoveDef& moveDef, const CMoveMath::BlockType& blockMask, const CSolidObject* owner) const {
const float r0 = SQUARE_SIZE * SQUARE_SIZE * 4.0f; // same as (SQUARE_SIZE*2)^2
const float r1 = ((moveDef.xsize * SQUARE_SIZE) >> 1) * ((moveDef.zsize * SQUARE_SIZE) >> 1) * 1.5f;
if (sqGoalRadius >= r0 && sqGoalRadius > r1)
return false;
return ((CMoveMath::IsBlocked(moveDef, goal, owner) & blockMask) != 0);
}
int2 CPathFinderDef::GoalSquareOffset(unsigned int blockSize) const {
const unsigned int blockPixelSize = blockSize * SQUARE_SIZE;
int2 offset;
offset.x = (unsigned(goal.x) % blockPixelSize) / SQUARE_SIZE;
offset.y = (unsigned(goal.z) % blockPixelSize) / SQUARE_SIZE;
return offset;
}
CCircularSearchConstraint::CCircularSearchConstraint(
const float3& start,
const float3& goal,
float goalRadius,
float searchSize,
unsigned int extraSize
): CPathFinderDef(goal, goalRadius, start.SqDistance2D(goal))
{
// calculate the center and radius of the constrained area
const unsigned int startX = start.x / SQUARE_SIZE;
const unsigned int startZ = start.z / SQUARE_SIZE;
const float3 halfWay = (start + goal) * 0.5f;
halfWayX = halfWay.x / SQUARE_SIZE;
halfWayZ = halfWay.z / SQUARE_SIZE;
const int dx = startX - halfWayX;
const int dz = startZ - halfWayZ;
searchRadiusSq = dx * dx + dz * dz;
searchRadiusSq *= (searchSize * searchSize);
searchRadiusSq += extraSize;
}
CRectangularSearchConstraint::CRectangularSearchConstraint(
const float3 startPos,
const float3 goalPos,
float sqRadius,
unsigned int blockSize
): CPathFinderDef(goalPos, 0.0f, startPos.SqDistance2D(goalPos))
{
sqGoalRadius = std::max(sqRadius, sqGoalRadius);
// construct the rectangular areas containing {start,goal}Pos
// (nodes are constrained to these when a PE uses the max-res
// PF to cache costs)
unsigned int startBlockX = startPos.x / SQUARE_SIZE;
unsigned int startBlockZ = startPos.z / SQUARE_SIZE;
unsigned int goalBlockX = goalPos.x / SQUARE_SIZE;
unsigned int goalBlockZ = goalPos.z / SQUARE_SIZE;
startBlockX -= startBlockX % blockSize;
startBlockZ -= startBlockZ % blockSize;
goalBlockX -= goalBlockX % blockSize;
goalBlockZ -= goalBlockZ % blockSize;
startBlockRect.x1 = startBlockX;
startBlockRect.z1 = startBlockZ;
startBlockRect.x2 = startBlockX + blockSize;
startBlockRect.z2 = startBlockZ + blockSize;
goalBlockRect.x1 = goalBlockX;
goalBlockRect.z1 = goalBlockZ;
goalBlockRect.x2 = goalBlockX + blockSize;
goalBlockRect.z2 = goalBlockZ + blockSize;
}
|
