void operator()(const MeshTriangle &tri, G3D::AABox &out) const { G3D::Vector3 lo = vertices[tri.idx0]; G3D::Vector3 hi = lo; lo = (lo.min(vertices[tri.idx1])).min(vertices[tri.idx2]); hi = (hi.max(vertices[tri.idx1])).max(vertices[tri.idx2]); out = G3D::AABox(lo, hi); }
void PathGenerator::ReducePathLenghtByDist(float dist) { if (GetPathType() == PATHFIND_BLANK) { TC_LOG_ERROR("maps", "PathGenerator::ReducePathLenghtByDist called before path was built"); return; } if (_pathPoints.size() < 2) // path building failure return; uint32 i = _pathPoints.size(); G3D::Vector3 nextVec = _pathPoints[--i]; while (i > 0) { G3D::Vector3 currVec = _pathPoints[--i]; G3D::Vector3 diffVec = (nextVec - currVec); float len = diffVec.length(); if (len > dist) { float step = dist / len; // same as nextVec _pathPoints[i + 1] -= diffVec * step; _sourceUnit->UpdateAllowedPositionZ(_pathPoints[i + 1].x, _pathPoints[i + 1].y, _pathPoints[i + 1].z); _pathPoints.resize(i + 2); break; } else if (i == 0) // at second point { _pathPoints[1] = _pathPoints[0]; _pathPoints.resize(2); break; } dist -= len; nextVec = currVec; // we're going backwards } }
bool IntersectTriangle(const MeshTriangle & tri, std::vector<G3D::Vector3>::const_iterator points, const G3D::Ray & ray, float & distance) { static const float EPS = 1e-5f; // See RTR2 ch. 13.7 for the algorithm. const G3D::Vector3 e1 = points[tri.idx1] - points[tri.idx0]; const G3D::Vector3 e2 = points[tri.idx2] - points[tri.idx0]; const G3D::Vector3 p(ray.direction().cross(e2)); const float a = e1.dot(p); if(abs(a) < EPS) { // Determinant is ill-conditioned; abort early return false; } const float f = 1.0f / a; const G3D::Vector3 s(ray.origin() - points[tri.idx0]); const float u = f * s.dot(p); if((u < 0.0f) || (u > 1.0f)) { // We hit the plane of the m_geometry, but outside the m_geometry return false; } const G3D::Vector3 q(s.cross(e1)); const float v = f * ray.direction().dot(q); if((v < 0.0f) || ((u + v) > 1.0f)) { // We hit the plane of the triangle, but outside the triangle return false; } const float t = f * e2.dot(q); if((t > 0.0f) && (t < distance)) { // This is a new hit, closer than the previous one distance = t; /* baryCoord[0] = 1.0 - u - v; baryCoord[1] = u; baryCoord[2] = v; */ return true; } // This hit is after the previous hit, so ignore it return false; }
void PathGenerator::BuildPolyPath(G3D::Vector3 const& startPos, G3D::Vector3 const& endPos) { // *** getting start/end poly logic *** float distToStartPoly, distToEndPoly; float startPoint[VERTEX_SIZE] = {startPos.y, startPos.z, startPos.x}; float endPoint[VERTEX_SIZE] = {endPos.y, endPos.z, endPos.x}; dtPolyRef startPoly = GetPolyByLocation(startPoint, &distToStartPoly); dtPolyRef endPoly = GetPolyByLocation(endPoint, &distToEndPoly); // we have a hole in our mesh // make shortcut path and mark it as NOPATH ( with flying and swimming exception ) // its up to caller how he will use this info if (startPoly == INVALID_POLYREF || endPoly == INVALID_POLYREF) { TC_LOG_DEBUG("maps", "++ BuildPolyPath :: (startPoly == 0 || endPoly == 0)\n"); BuildShortcut(); bool path = _sourceUnit->GetTypeId() == TYPEID_UNIT && _sourceUnit->ToCreature()->CanFly(); bool waterPath = _sourceUnit->GetTypeId() == TYPEID_UNIT && _sourceUnit->ToCreature()->CanSwim(); if (waterPath) { // Check both start and end points, if they're both in water, then we can *safely* let the creature move for (uint32 i = 0; i < _pathPoints.size(); ++i) { ZLiquidStatus status = _sourceUnit->GetBaseMap()->getLiquidStatus(_pathPoints[i].x, _pathPoints[i].y, _pathPoints[i].z, MAP_ALL_LIQUIDS, NULL); // One of the points is not in the water, cancel movement. if (status == LIQUID_MAP_NO_WATER) { waterPath = false; break; } } } _type = (path || waterPath) ? PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH) : PATHFIND_NOPATH; return; } // we may need a better number here bool farFromPoly = (distToStartPoly > 7.0f || distToEndPoly > 7.0f); if (farFromPoly) { TC_LOG_DEBUG("maps", "++ BuildPolyPath :: farFromPoly distToStartPoly=%.3f distToEndPoly=%.3f\n", distToStartPoly, distToEndPoly); bool buildShotrcut = false; if (_sourceUnit->GetTypeId() == TYPEID_UNIT) { Creature* owner = (Creature*)_sourceUnit; G3D::Vector3 const& p = (distToStartPoly > 7.0f) ? startPos : endPos; if (_sourceUnit->GetBaseMap()->IsUnderWater(p.x, p.y, p.z)) { TC_LOG_DEBUG("maps", "++ BuildPolyPath :: underWater case\n"); if (owner->CanSwim()) buildShotrcut = true; } else { TC_LOG_DEBUG("maps", "++ BuildPolyPath :: flying case\n"); if (owner->CanFly()) buildShotrcut = true; } } if (buildShotrcut) { BuildShortcut(); _type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH); return; } else { float closestPoint[VERTEX_SIZE]; // we may want to use closestPointOnPolyBoundary instead if (dtStatusSucceed(_navMeshQuery->closestPointOnPoly(endPoly, endPoint, closestPoint, NULL))) { dtVcopy(endPoint, closestPoint); SetActualEndPosition(G3D::Vector3(endPoint[2], endPoint[0], endPoint[1])); } _type = PATHFIND_INCOMPLETE; } } // *** poly path generating logic *** // start and end are on same polygon // just need to move in straight line if (startPoly == endPoly) { TC_LOG_DEBUG("maps", "++ BuildPolyPath :: (startPoly == endPoly)\n"); BuildShortcut(); _pathPolyRefs[0] = startPoly; _polyLength = 1; _type = farFromPoly ? PATHFIND_INCOMPLETE : PATHFIND_NORMAL; TC_LOG_DEBUG("maps", "++ BuildPolyPath :: path type %d\n", _type); return; } // look for startPoly/endPoly in current path /// @todo we can merge it with getPathPolyByPosition() loop bool startPolyFound = false; bool endPolyFound = false; uint32 pathStartIndex = 0; uint32 pathEndIndex = 0; if (_polyLength) { for (; pathStartIndex < _polyLength; ++pathStartIndex) { // here to catch few bugs if (_pathPolyRefs[pathStartIndex] == INVALID_POLYREF) { TC_LOG_ERROR("maps", "Invalid poly ref in BuildPolyPath. _polyLength: %u, pathStartIndex: %u," " startPos: %s, endPos: %s, mapid: %u", _polyLength, pathStartIndex, startPos.toString().c_str(), endPos.toString().c_str(), _sourceUnit->GetMapId()); break; } if (_pathPolyRefs[pathStartIndex] == startPoly) { startPolyFound = true; break; } } for (pathEndIndex = _polyLength-1; pathEndIndex > pathStartIndex; --pathEndIndex) if (_pathPolyRefs[pathEndIndex] == endPoly) { endPolyFound = true; break; } } if (startPolyFound && endPolyFound) { TC_LOG_DEBUG("maps", "++ BuildPolyPath :: (startPolyFound && endPolyFound)\n"); // we moved along the path and the target did not move out of our old poly-path // our path is a simple subpath case, we have all the data we need // just "cut" it out _polyLength = pathEndIndex - pathStartIndex + 1; memmove(_pathPolyRefs, _pathPolyRefs + pathStartIndex, _polyLength * sizeof(dtPolyRef)); } else if (startPolyFound && !endPolyFound) { TC_LOG_DEBUG("maps", "++ BuildPolyPath :: (startPolyFound && !endPolyFound)\n"); // we are moving on the old path but target moved out // so we have atleast part of poly-path ready _polyLength -= pathStartIndex; // try to adjust the suffix of the path instead of recalculating entire length // at given interval the target cannot get too far from its last location // thus we have less poly to cover // sub-path of optimal path is optimal // take ~80% of the original length /// @todo play with the values here uint32 prefixPolyLength = uint32(_polyLength * 0.8f + 0.5f); memmove(_pathPolyRefs, _pathPolyRefs+pathStartIndex, prefixPolyLength * sizeof(dtPolyRef)); dtPolyRef suffixStartPoly = _pathPolyRefs[prefixPolyLength-1]; // we need any point on our suffix start poly to generate poly-path, so we need last poly in prefix data float suffixEndPoint[VERTEX_SIZE]; if (dtStatusFailed(_navMeshQuery->closestPointOnPoly(suffixStartPoly, endPoint, suffixEndPoint, NULL))) { // we can hit offmesh connection as last poly - closestPointOnPoly() don't like that // try to recover by using prev polyref --prefixPolyLength; suffixStartPoly = _pathPolyRefs[prefixPolyLength-1]; if (dtStatusFailed(_navMeshQuery->closestPointOnPoly(suffixStartPoly, endPoint, suffixEndPoint, NULL))) { // suffixStartPoly is still invalid, error state BuildShortcut(); _type = PATHFIND_NOPATH; return; } } // generate suffix uint32 suffixPolyLength = 0; dtStatus dtResult; if (_straightLine) { float hit = 0; float hitNormal[3]; memset(hitNormal, 0, sizeof(hitNormal)); dtResult = _navMeshQuery->raycast( suffixStartPoly, suffixEndPoint, endPoint, &_filter, &hit, hitNormal, _pathPolyRefs + prefixPolyLength - 1, (int*)&suffixPolyLength, MAX_PATH_LENGTH - prefixPolyLength); // raycast() sets hit to FLT_MAX if there is a ray between start and end if (hit != FLT_MAX) { // the ray hit something, return no path instead of the incomplete one _type = PATHFIND_NOPATH; return; } } else { dtResult = _navMeshQuery->findPath( suffixStartPoly, // start polygon endPoly, // end polygon suffixEndPoint, // start position endPoint, // end position &_filter, // polygon search filter _pathPolyRefs + prefixPolyLength - 1, // [out] path (int*)&suffixPolyLength, MAX_PATH_LENGTH - prefixPolyLength); // max number of polygons in output path } if (!suffixPolyLength || dtStatusFailed(dtResult)) { // this is probably an error state, but we'll leave it // and hopefully recover on the next Update // we still need to copy our preffix TC_LOG_ERROR("maps", "%u's Path Build failed: 0 length path", _sourceUnit->GetGUIDLow()); } TC_LOG_DEBUG("maps", "++ m_polyLength=%u prefixPolyLength=%u suffixPolyLength=%u \n", _polyLength, prefixPolyLength, suffixPolyLength); // new path = prefix + suffix - overlap _polyLength = prefixPolyLength + suffixPolyLength - 1; } else { TC_LOG_DEBUG("maps", "++ BuildPolyPath :: (!startPolyFound && !endPolyFound)\n"); // either we have no path at all -> first run // or something went really wrong -> we aren't moving along the path to the target // just generate new path // free and invalidate old path data Clear(); dtStatus dtResult; if (_straightLine) { float hit = 0; float hitNormal[3]; memset(hitNormal, 0, sizeof(hitNormal)); dtResult = _navMeshQuery->raycast( startPoly, startPoint, endPoint, &_filter, &hit, hitNormal, _pathPolyRefs, (int*)&_polyLength, MAX_PATH_LENGTH); // raycast() sets hit to FLT_MAX if there is a ray between start and end if (hit != FLT_MAX) { // the ray hit something, return no path instead of the incomplete one _type = PATHFIND_NOPATH; return; } } else { dtResult = _navMeshQuery->findPath( startPoly, // start polygon endPoly, // end polygon startPoint, // start position endPoint, // end position &_filter, // polygon search filter _pathPolyRefs, // [out] path (int*)&_polyLength, MAX_PATH_LENGTH); // max number of polygons in output path } if (!_polyLength || dtStatusFailed(dtResult)) { // only happens if we passed bad data to findPath(), or navmesh is messed up TC_LOG_ERROR("maps", "%u's Path Build failed: 0 length path", _sourceUnit->GetGUIDLow()); BuildShortcut(); _type = PATHFIND_NOPATH; return; } } // by now we know what type of path we can get if (_pathPolyRefs[_polyLength - 1] == endPoly && !(_type & PATHFIND_INCOMPLETE)) _type = PATHFIND_NORMAL; else _type = PATHFIND_INCOMPLETE; // generate the point-path out of our up-to-date poly-path BuildPointPath(startPoint, endPoint); }
void PathGenerator::BuildPointPath(const float *startPoint, const float *endPoint) { float pathPoints[MAX_POINT_PATH_LENGTH*VERTEX_SIZE]; uint32 pointCount = 0; dtStatus dtResult = DT_FAILURE; if (_straightLine) { dtResult = DT_SUCCESS; pointCount = 1; memcpy(&pathPoints[VERTEX_SIZE * 0], startPoint, sizeof(float)* 3); // first point // path has to be split into polygons with dist SMOOTH_PATH_STEP_SIZE between them G3D::Vector3 startVec = G3D::Vector3(startPoint[0], startPoint[1], startPoint[2]); G3D::Vector3 endVec = G3D::Vector3(endPoint[0], endPoint[1], endPoint[2]); G3D::Vector3 diffVec = (endVec - startVec); G3D::Vector3 prevVec = startVec; float len = diffVec.length(); diffVec *= SMOOTH_PATH_STEP_SIZE / len; while (len > SMOOTH_PATH_STEP_SIZE) { len -= SMOOTH_PATH_STEP_SIZE; prevVec += diffVec; pathPoints[VERTEX_SIZE * pointCount + 0] = prevVec.x; pathPoints[VERTEX_SIZE * pointCount + 1] = prevVec.y; pathPoints[VERTEX_SIZE * pointCount + 2] = prevVec.z; ++pointCount; } memcpy(&pathPoints[VERTEX_SIZE * pointCount], endPoint, sizeof(float)* 3); // last point ++pointCount; } else if (_useStraightPath) { dtResult = _navMeshQuery->findStraightPath( startPoint, // start position endPoint, // end position _pathPolyRefs, // current path _polyLength, // lenth of current path pathPoints, // [out] path corner points NULL, // [out] flags NULL, // [out] shortened path (int*)&pointCount, _pointPathLimit); // maximum number of points/polygons to use } else { dtResult = FindSmoothPath( startPoint, // start position endPoint, // end position _pathPolyRefs, // current path _polyLength, // length of current path pathPoints, // [out] path corner points (int*)&pointCount, _pointPathLimit); // maximum number of points } if (pointCount < 2 || dtStatusFailed(dtResult)) { // only happens if pass bad data to findStraightPath or navmesh is broken // single point paths can be generated here /// @todo check the exact cases TC_LOG_DEBUG("maps", "++ PathGenerator::BuildPointPath FAILED! path sized %d returned\n", pointCount); BuildShortcut(); _type = PATHFIND_NOPATH; return; } else if (pointCount == _pointPathLimit) { TC_LOG_DEBUG("maps", "++ PathGenerator::BuildPointPath FAILED! path sized %d returned, lower than limit set to %d\n", pointCount, _pointPathLimit); BuildShortcut(); _type = PATHFIND_SHORT; return; } _pathPoints.resize(pointCount); for (uint32 i = 0; i < pointCount; ++i) _pathPoints[i] = G3D::Vector3(pathPoints[i*VERTEX_SIZE+2], pathPoints[i*VERTEX_SIZE], pathPoints[i*VERTEX_SIZE+1]); NormalizePath(); // first point is always our current location - we need the next one SetActualEndPosition(_pathPoints[pointCount-1]); // force the given destination, if needed if (_forceDestination && (!(_type & PATHFIND_NORMAL) || !InRange(GetEndPosition(), GetActualEndPosition(), 1.0f, 1.0f))) { // we may want to keep partial subpath if (Dist3DSqr(GetActualEndPosition(), GetEndPosition()) < 0.3f * Dist3DSqr(GetStartPosition(), GetEndPosition())) { SetActualEndPosition(GetEndPosition()); _pathPoints[_pathPoints.size()-1] = GetEndPosition(); } else { SetActualEndPosition(GetEndPosition()); BuildShortcut(); } _type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH); } TC_LOG_DEBUG("maps", "++ PathGenerator::BuildPointPath path type %d size %d poly-size %d\n", _type, pointCount, _polyLength); }