Intersection PointOctreeQuery::TestOctant(const BoundingBox& box, bool inside)
{
if (inside)
return INSIDE;
else
return box.IsInside(point_);
}
float Ray::HitDistance(const BoundingBox& box) const
{
// If undefined, no hit (infinite distance)
if (!box.defined_)
return M_INFINITY;
// Check for ray origin being inside the box
if (box.IsInside(origin_))
return 0.0f;
float dist = M_INFINITY;
// Check for intersecting in the X-direction
if (origin_.x_ < box.min_.x_ && direction_.x_ > 0.0f)
{
float x = (box.min_.x_ - origin_.x_) / direction_.x_;
if (x < dist)
{
Vector3 point = origin_ + x * direction_;
if (point.y_ >= box.min_.y_ && point.y_ <= box.max_.y_ && point.z_ >= box.min_.z_ && point.z_ <= box.max_.z_)
dist = x;
}
}
if (origin_.x_ > box.max_.x_ && direction_.x_ < 0.0f)
{
float x = (box.max_.x_ - origin_.x_) / direction_.x_;
if (x < dist)
{
Vector3 point = origin_ + x * direction_;
if (point.y_ >= box.min_.y_ && point.y_ <= box.max_.y_ && point.z_ >= box.min_.z_ && point.z_ <= box.max_.z_)
dist = x;
}
}
// Check for intersecting in the Y-direction
if (origin_.y_ < box.min_.y_ && direction_.y_ > 0.0f)
{
float x = (box.min_.y_ - origin_.y_) / direction_.y_;
if (x < dist)
{
Vector3 point = origin_ + x * direction_;
if (point.x_ >= box.min_.x_ && point.x_ <= box.max_.x_ && point.z_ >= box.min_.z_ && point.z_ <= box.max_.z_)
dist = x;
}
}
if (origin_.y_ > box.max_.y_ && direction_.y_ < 0.0f)
{
float x = (box.max_.y_ - origin_.y_) / direction_.y_;
if (x < dist)
{
Vector3 point = origin_ + x * direction_;
if (point.x_ >= box.min_.x_ && point.x_ <= box.max_.x_ && point.z_ >= box.min_.z_ && point.z_ <= box.max_.z_)
dist = x;
}
}
// Check for intersecting in the Z-direction
if (origin_.z_ < box.min_.z_ && direction_.z_ > 0.0f)
{
float x = (box.min_.z_ - origin_.z_) / direction_.z_;
if (x < dist)
{
Vector3 point = origin_ + x * direction_;
if (point.x_ >= box.min_.x_ && point.x_ <= box.max_.x_ && point.y_ >= box.min_.y_ && point.y_ <= box.max_.y_)
dist = x;
}
}
if (origin_.z_ > box.max_.z_ && direction_.z_ < 0.0f)
{
float x = (box.max_.z_ - origin_.z_) / direction_.z_;
if (x < dist)
{
Vector3 point = origin_ + x * direction_;
if (point.x_ >= box.min_.x_ && point.x_ <= box.max_.x_ && point.y_ >= box.min_.y_ && point.y_ <= box.max_.y_)
dist = x;
}
}
return dist;
}
void NavigationMesh::FindPath(PODVector<NavigationPathPoint>& dest, const Vector3& start, const Vector3& end,
const Vector3& extents, const dtQueryFilter* filter)
{
ATOMIC_PROFILE(FindPath);
dest.Clear();
if (!InitializeQuery())
return;
// Navigation data is in local space. Transform path points from world to local
const Matrix3x4& transform = node_->GetWorldTransform();
Matrix3x4 inverse = transform.Inverse();
Vector3 localStart = inverse * start;
Vector3 localEnd = inverse * end;
const dtQueryFilter* queryFilter = filter ? filter : queryFilter_.Get();
dtPolyRef startRef;
dtPolyRef endRef;
navMeshQuery_->findNearestPoly(&localStart.x_, &extents.x_, queryFilter, &startRef, 0);
navMeshQuery_->findNearestPoly(&localEnd.x_, &extents.x_, queryFilter, &endRef, 0);
if (!startRef || !endRef)
return;
int numPolys = 0;
int numPathPoints = 0;
navMeshQuery_->findPath(startRef, endRef, &localStart.x_, &localEnd.x_, queryFilter, pathData_->polys_, &numPolys,
MAX_POLYS);
if (!numPolys)
return;
Vector3 actualLocalEnd = localEnd;
// If full path was not found, clamp end point to the end polygon
if (pathData_->polys_[numPolys - 1] != endRef)
navMeshQuery_->closestPointOnPoly(pathData_->polys_[numPolys - 1], &localEnd.x_, &actualLocalEnd.x_, 0);
navMeshQuery_->findStraightPath(&localStart.x_, &actualLocalEnd.x_, pathData_->polys_, numPolys,
&pathData_->pathPoints_[0].x_, pathData_->pathFlags_, pathData_->pathPolys_, &numPathPoints, MAX_POLYS);
// Transform path result back to world space
for (int i = 0; i < numPathPoints; ++i)
{
NavigationPathPoint pt;
pt.position_ = transform * pathData_->pathPoints_[i];
pt.flag_ = (NavigationPathPointFlag)pathData_->pathFlags_[i];
// Walk through all NavAreas and find nearest
unsigned nearestNavAreaID = 0; // 0 is the default nav area ID
float nearestDistance = M_LARGE_VALUE;
for (unsigned j = 0; j < areas_.Size(); j++)
{
NavArea* area = areas_[j].Get();
if (area && area->IsEnabledEffective())
{
BoundingBox bb = area->GetWorldBoundingBox();
if (bb.IsInside(pt.position_) == INSIDE)
{
Vector3 areaWorldCenter = area->GetNode()->GetWorldPosition();
float distance = (areaWorldCenter - pt.position_).LengthSquared();
if (distance < nearestDistance)
{
nearestDistance = distance;
nearestNavAreaID = area->GetAreaID();
}
}
}
}
pt.areaID_ = (unsigned char)nearestNavAreaID;
dest.Push(pt);
}
}
float Ray::HitDistance(const BoundingBox& box) const
{
// Check for ray origin being inside the box
if (box.IsInside(origin))
return 0.0f;
float dist = M_INFINITY;
// Check for intersecting in the X-direction
if (origin.x < box.min.x && direction.x > 0.0f)
{
float x = (box.min.x - origin.x) / direction.x;
if (x < dist)
{
Vector3 point = origin + x * direction;
if (point.y >= box.min.y && point.y <= box.max.y && point.z >= box.min.z && point.z <= box.max.z)
dist = x;
}
}
if (origin.x > box.max.x && direction.x < 0.0f)
{
float x = (box.max.x - origin.x) / direction.x;
if (x < dist)
{
Vector3 point = origin + x * direction;
if (point.y >= box.min.y && point.y <= box.max.y && point.z >= box.min.z && point.z <= box.max.z)
dist = x;
}
}
// Check for intersecting in the Y-direction
if (origin.y < box.min.y && direction.y > 0.0f)
{
float x = (box.min.y - origin.y) / direction.y;
if (x < dist)
{
Vector3 point = origin + x * direction;
if (point.x >= box.min.x && point.x <= box.max.x && point.z >= box.min.z && point.z <= box.max.z)
dist = x;
}
}
if (origin.y > box.max.y && direction.y < 0.0f)
{
float x = (box.max.y - origin.y) / direction.y;
if (x < dist)
{
Vector3 point = origin + x * direction;
if (point.x >= box.min.x && point.x <= box.max.x && point.z >= box.min.z && point.z <= box.max.z)
dist = x;
}
}
// Check for intersecting in the Z-direction
if (origin.z < box.min.z && direction.z > 0.0f)
{
float x = (box.min.z - origin.z) / direction.z;
if (x < dist)
{
Vector3 point = origin + x * direction;
if (point.x >= box.min.x && point.x <= box.max.x && point.y >= box.min.y && point.y <= box.max.y)
dist = x;
}
}
if (origin.z > box.max.z && direction.z < 0.0f)
{
float x = (box.max.z - origin.z) / direction.z;
if (x < dist)
{
Vector3 point = origin + x * direction;
if (point.x >= box.min.x && point.x <= box.max.x && point.y >= box.min.y && point.y <= box.max.y)
dist = x;
}
}
return dist;
}