void Body::stepPhysics ()
{
WorldObject::stepPhysics();
dBodyID bodyID = getMainOdeObject()->getBodyID();
if (userDriver)
{
double moveZ = System::get()->axisMap[getIDKeyboardKey(SDLK_BACKSPACE)]->getValue() * 50000;
moveZ += System::get()->axisMap[getIDKeyboardKey(SDLK_RETURN)]->getValue() * 12200;
moveZ -= System::get()->axisMap[getIDKeyboardKey(SDLK_RSHIFT)]->getValue() * 10000;
dBodyAddForce (bodyID, 0, 0, moveZ);
}
// apply simple air drag forces:
const double airDensity = 1.225;
// f = Cx * 0.5 * airDensity * v^2 * area;
// f = dragCoefficient * 0.5 * 1.225 * vel*vel * frontalArea;
Vector3d velocity (dBodyGetLinearVel (bodyID));
double velModule = velocity.distance();
Vector3d normalizedVel (velocity);
normalizedVel.scalarDivide(velModule);
normalizedVel.scalarMultiply(-1);
Vector3d force (normalizedVel);
force.scalarMultiply (0.5 * dragCoefficient * airDensity * frontalArea * velModule * velModule);
dBodyAddForce (bodyID, force.x, force.y, force.z);
//log->__format(LOG_DEVELOPER, "Body air drag force = (%f, %f, %f)", force.x, force.y, force.z);
}
/**
Tries to find a (high-level) path from sourcePos in sourcePoly to destPos in destPoly.
@param sourcePos The source position
@param sourcePoly The source nav polygon
@param destPos The destination position
@param destPoly The destination nav polygon
@param path Used to return the path (if found) to the caller
@return true, if a path was found, or false otherwise
*/
bool GlobalPathfinder::find_path(const Vector3d& sourcePos, int sourcePoly,
const Vector3d& destPos, int destPoly,
std::list<int>& path) const
{
// Step 1: If the source position and dest position are both in the same nav polygon,
// the high-level path is empty (the entity just needs to go in a straight line).
if(sourcePoly == destPoly)
{
return true;
}
// Step 2: Find the shortest unblocked path-table path from a source navlink to a dest navlink.
// If such a path is found, and it's no more than (say) 25% longer than the optimal path
// we'd have if we ignored blocks, then use it.
const std::vector<NavLink_Ptr>& links = m_navMesh->links();
const std::vector<NavPolygon_Ptr>& polygons = m_navMesh->polygons();
const std::vector<int>& sourceLinkIndices = polygons[sourcePoly]->out_links();
const std::vector<int>& destLinkIndices = polygons[destPoly]->in_links();
// Work out the costs of all the possible paths and get them ready to be processed in ascending order of cost.
std::priority_queue<PathDescriptor, std::vector<PathDescriptor>, std::greater<PathDescriptor> > pq;
int sourceLinkCount = static_cast<int>(sourceLinkIndices.size());
int destLinkCount = static_cast<int>(destLinkIndices.size());
for(int i=0; i<sourceLinkCount; ++i)
{
int sourceLinkIndex = sourceLinkIndices[i];
const NavLink_Ptr& sourceLink = links[sourceLinkIndex];
float sourceCost = static_cast<float>(sourcePos.distance(sourceLink->source_position()));
for(int j=0; j<destLinkCount; ++j)
{
// The cost of going from sourcePos to destPos via the shortest path-table path
// between the two navlinks is the sum of the cost of going to the source navlink,
// the cost of going from the source navlink to the dest navlink, and the cost of
// going from the dest navlink to destPos.
int destLinkIndex = destLinkIndices[j];
const NavLink_Ptr& destLink = links[destLinkIndex];
float destCost = static_cast<float>(destPos.distance(destLink->dest_position()));
float interlinkCost = m_pathTable->cost(sourceLinkIndex, destLinkIndex);
pq.push(PathDescriptor(sourceCost + interlinkCost + destCost, sourceLinkIndex, destLinkIndex));
}
}
// Starting from the least costly path (if any), construct it and see whether it's blocked or not. If not, use it.
if(!pq.empty())
{
// We accept path-table paths which are no more than 25% longer than the shortest
// path-table path we found without considering blocks: they are "good enough",
// and are MUCH quicker to calculate than paths we might find using A*.
const float WORST_ACCEPTABLE_COST = 1.25f * pq.top().cost;
while(!pq.empty())
{
PathDescriptor desc = pq.top();
if(desc.cost > WORST_ACCEPTABLE_COST) break;
pq.pop();
path = m_pathTable->construct_path(desc.sourceLink, desc.destLink);
if(!is_blocked(sourcePos, path, destPos))
return true; // note that the path to be returned has already been stored in the out parameter
}
}
// Step 3: If a reasonable unblocked path-table path does not exist, do an A* search on
// the adjacency list representation of the navigation graph. Use a temporary node
// in both the source and destination polygons to represent the actual position of
// the player.
// TODO
// Step 4: If an A* path was found, use it. Otherwise, no valid path exists.
// TODO
// NYI
return false;
}