// ok, need thing to step down through bodies and find closest approach
// modify targpos directly to aim short of dangerous bodies
static bool ParentSafetyAdjust(Ship *ship, Frame *targframe, vector3d &targpos, vector3d &targvel)
{
Body *body = 0;
Frame *frame = targframe->GetNonRotFrame();
while (frame)
{
if (ship->GetFrame()->GetNonRotFrame() == frame) break; // ship in frame, stop
if (frame->GetBody()) body = frame->GetBody(); // ignore grav points?
double sdist = ship->GetPositionRelTo(frame).Length();
if (sdist < frame->GetRadius()) break; // ship inside frame, stop
frame = frame->GetParent()->GetNonRotFrame(); // check next frame down
}
if (!body) return false;
// aim for zero velocity at surface of that body
// still along path to target
vector3d targpos2 = targpos - ship->GetPosition();
double targdist = targpos2.Length();
double bodydist = body->GetPositionRelTo(ship).Length() - MaxEffectRad(body, ship)*1.5;
if (targdist < bodydist) return false;
targpos -= (targdist - bodydist) * targpos2 / targdist;
targvel = body->GetVelocityRelTo(ship->GetFrame());
return true;
}
// ok, need thing to step down through bodies and find closest approach
// modify targpos directly to aim short of dangerous bodies
static bool ParentSafetyAdjust(Ship *ship, Frame *targframe, const vector3d &posoff, vector3d &targpos)
{
Body *body = 0;
Frame *frame = targframe->IsRotatingFrame() ? targframe->m_parent : targframe;
while (frame)
{
double sdist = ship->GetPositionRelTo(frame).Length(); // ship position in that frame
if (sdist < frame->GetRadius()) break;
while (frame && !(body = frame->GetBodyFor()))
frame = frame->m_parent;
if (!frame) return false;
frame = body->GetFrame()->m_parent;
if (body->HasDoubleFrame()) frame = frame->m_parent;
}
if (!body) return false;
// ok, so if body != 0, aim for zero velocity at distance to surface of that body
// still along path to target
vector3d targpos2 = targpos - ship->GetPosition();
double targdist = targpos2.Length();
double bodydist = body->GetPositionRelTo(ship).Length() - MaxEffectRad(body, ship)*1.5;
if (targdist < bodydist) return false;
targpos -= (targdist - bodydist) * targpos2 / targdist;
// printf("Adjusted targpos for safety from %s: old = %.1f, new = %.1f\n",
// body->GetLabel().c_str(), targdist, (targpos-ship->GetPosition()).Length());
return true;
}
AICmdFlyAround::AICmdFlyAround(Ship *ship, Body *obstructor, double relalt, float hungriness)
: AICommand (ship, CMD_FLYAROUND)
{
m_fuelEconomy = Clamp(hungriness, 0.0f, 1.0f);
double alt = relalt*MaxEffectRad(obstructor, ship);
Setup(obstructor, alt, 0.0, 3, 0, 0, vector3d(0.0));
}
// Fly to vicinity of body
AICmdFlyTo::AICmdFlyTo(Ship *ship, Body *target) : AICommand(ship, CMD_FLYTO)
{
m_frame = 0; m_state = -6; m_lockhead = true; m_endvel = 0; m_tangent = false;
if (!target->IsType(Object::TERRAINBODY)) m_dist = VICINITY_MIN;
else m_dist = VICINITY_MUL*MaxEffectRad(target, ship);
if (target->IsType(Object::SPACESTATION) && static_cast<SpaceStation*>(target)->IsGroundStation()) {
m_posoff = target->GetPosition() + 15000.0 * target->GetOrient().VectorY();
// m_posoff += 500.0 * target->GetOrient().VectorX();
m_targframe = target->GetFrame(); m_target = 0;
}
else { m_target = target; m_targframe = 0; }
if (ship->GetPositionRelTo(target).Length() <= 15000.0) m_targframe = 0;
}
// Pursue ship, not body
AICmdFlyTo::AICmdFlyTo(Ship *ship, Ship *target, float hungriness) : AICommand (ship, CMD_FLYTO)
{
double dist = std::max(VICINITY_MIN, VICINITY_MUL*MaxEffectRad(target, ship));
m_targframe = GetNonRotFrame(target);
m_posoff = dist * m_ship->GetPositionRelTo(m_targframe).Normalized();
m_posoff += target->GetPosition();
m_endvel = 0; m_tangent = 0;
m_state = -1; m_frame = 0;
// check if we're already close enough
if (dist > m_ship->GetPositionRelTo(target).Length()) m_state = 5;
m_fuelEconomy = Clamp(hungriness, 0.0f, 1.0f);;
m_targetShip = target;
}
// Fly to "vicinity" of body
AICmdFlyTo::AICmdFlyTo(Ship *ship, Body *target) : AICommand (ship, CMD_FLYTO)
{
double dist = std::max(VICINITY_MIN, VICINITY_MUL*MaxEffectRad(target, ship));
if (target->IsType(Object::SPACESTATION) && static_cast<SpaceStation *>(target)->IsGroundStation()) {
matrix4x4d rot; target->GetRotMatrix(rot);
m_posoff = target->GetPosition() + dist * vector3d(rot[4], rot[5], rot[6]); // up vector for starport
m_targframe = target->GetFrame();
}
else {
m_targframe = GetNonRotFrame(target);
m_posoff = dist * m_ship->GetPositionRelTo(m_targframe).Normalized();
m_posoff += target->GetPosition();
}
m_endvel = 0; m_tangent = 0;
m_state = -1; m_frame = 0;
// check if we're already close enough
if (dist > m_ship->GetPositionRelTo(target).Length()) m_state = 5;
}
AICmdFlyAround::AICmdFlyAround(Ship *ship, Body *obstructor, double relalt)
: AICommand (ship, CMD_FLYAROUND)
{
double alt = relalt*MaxEffectRad(obstructor, ship);
Setup(obstructor, alt, 0.0, 3, 0, 0, vector3d(0.0));
}
bool AICmdFlyTo::TimeStepUpdate()
{
double timestep = Pi::game->GetTimeStep();
vector3d targvel = GetVelInFrame(m_ship->GetFrame(), m_targframe, m_posoff);
vector3d relvel = m_ship->GetVelocity() - targvel;
vector3d targpos = GetPosInFrame(m_ship->GetFrame(), m_targframe, m_posoff);
bool safe = ParentSafetyAdjust(m_ship, m_targframe, m_posoff, targpos);
double endvel = safe ? 0.0 : m_endvel; // don't use endvel if safety-adjusted
vector3d relpos = targpos - m_ship->GetPosition();
vector3d reldir = relpos.NormalizedSafe();
double targdist = relpos.Length();
// sort out gear, launching
if (m_ship->GetFlightState() == Ship::FLYING) m_ship->SetWheelState(false);
else { LaunchShip(m_ship); return false; }
// frame switch stuff - clear children/collision state
if (m_frame != m_ship->GetFrame()) {
if (m_child) { delete m_child; m_child = 0; }
if (m_frame && m_tangent) return true; // regen tangent on frame switch
m_frame = m_ship->GetFrame();
m_reldir = reldir; // for +vel termination condition
}
#ifdef DEBUG_AUTOPILOT
if (m_ship->IsType(Object::PLAYER))
printf("Autopilot dist = %.1f, speed = %.1f, zthrust = %.2f, term = %.3f, state = %i\n",
targdist, relvel.Length(), m_ship->GetThrusterState().z, reldir.Dot(m_reldir), m_state);
#endif
Body *body = m_frame->GetBodyFor();
double erad = MaxEffectRad(body, m_ship);
if (!m_tangent || !(body == m_targframe->GetBodyFor()))
{
// process path collisions with frame body
int coll = CheckCollision(m_ship, reldir, targdist, targpos, endvel, erad);
if (coll == 0) { // no collision
if (m_child) { delete m_child; m_child = 0; m_state = -1; }
}
else if (coll == 1) { // below feature height, target not below
double ang = m_ship->AIFaceDirection(m_ship->GetPosition());
m_ship->AIMatchVel(ang < 0.05 ? 1000.0 * m_ship->GetPosition().Normalized() : 0.0);
m_state = -3; return false;
}
else { // same thing for 2/3/4
if (!m_child) m_child =
new AICmdFlyAround(m_ship, body, erad, 0.0, m_targframe, m_posoff);
ProcessChild(); m_state = -5; return false;
}
}
// if dangerously close to local body, pretend target isn't moving
if (body) {
double localdist = m_ship->GetPosition().Length();
if (targdist > localdist && localdist < 1.5*MaxFeatureRad(body))
relvel += targvel;
}
// regenerate state to flipmode if we're off course
bool overshoot = CheckOvershoot(m_ship, reldir, targdist, relvel, endvel);
if (m_tangent && m_state == -4 && !overshoot) return true; // bail out
if (m_state < 0) m_state = GetFlipMode(m_ship, relpos, relvel);
// linear thrust
double ang, maxdecel = GetMaxDecel(m_ship, reldir, m_state, &ang);
maxdecel -= GetGravityAtPos(m_targframe, m_posoff);
if(maxdecel <= 0) { m_ship->AIMessage(Ship::AIERROR_GRAV_TOO_HIGH); return true; }
bool cap = m_ship->AIMatchPosVel2(reldir, targdist, relvel, endvel, maxdecel);
// path overshoot check, response
if (m_state < 3 && overshoot) {
double ispeed = calc_ivel(targdist, endvel, maxdecel);
m_ship->AIFaceDirection(ispeed*reldir - relvel);
m_state = -4; return false;
}
// flip check - if facing forward and not accelerating at maximum
if (m_state == 1 && ang > 0.99 && !cap) m_state = 2;
// termination conditions
if (m_state == 3) m_state++; // finished last adjustment, hopefully
else if (endvel > 0.0) { if (reldir.Dot(m_reldir) < 0.9) m_state = 4; }
else if (targdist < 0.5*m_ship->GetAccelMin()*timestep*timestep) m_state = 3;
// set heading according to current state
if (m_state < 2) { // this shit still needed? yeah, sort of
vector3d newrelpos = targpos - m_ship->AIGetNextFramePos();
if ((newrelpos + reldir*100.0).Dot(relpos) <= 0.0)
m_ship->AIFaceDirection(reldir); // last frames turning workaround
else m_ship->AIFaceDirection(newrelpos);
}
else if (m_state == 2) m_ship->AIFaceDirection(-reldir); // hmm. -relvel instead?
else m_ship->AIMatchAngVelObjSpace(vector3d(0.0));
if (m_state == 4) return true;
return false;
}
bool AICmdFlyTo::TimeStepUpdate()
{
if (!m_target && !m_targframe) return true; // deleted object
// sort out gear, launching
if (m_ship->GetFlightState() == Ship::FLYING) m_ship->SetWheelState(false);
else { LaunchShip(m_ship); return false; }
// generate base target pos (with vicinity adjustment) & vel
double timestep = Pi::game->GetTimeStep();
vector3d targpos, targvel;
if (m_target) {
targpos = m_target->GetPositionRelTo(m_ship->GetFrame());
targpos -= (targpos - m_ship->GetPosition()).NormalizedSafe() * m_dist;
targvel = m_target->GetVelocityRelTo(m_ship->GetFrame());
} else {
targpos = GetPosInFrame(m_ship->GetFrame(), m_targframe, m_posoff);
targvel = GetVelInFrame(m_ship->GetFrame(), m_targframe, m_posoff);
}
Frame *targframe = m_target ? m_target->GetFrame() : m_targframe;
ParentSafetyAdjust(m_ship, targframe, targpos, targvel);
vector3d relpos = targpos - m_ship->GetPosition();
vector3d reldir = relpos.NormalizedSafe();
vector3d relvel = targvel - m_ship->GetVelocity();
double targdist = relpos.Length();
#ifdef DEBUG_AUTOPILOT
if (m_ship->IsType(Object::PLAYER))
printf("Autopilot dist = %.1f, speed = %.1f, zthrust = %.2f, state = %i\n",
targdist, relvel.Length(), m_ship->GetThrusterState().z, m_state);
#endif
// frame switch stuff - clear children/collision state
if (m_frame != m_ship->GetFrame()) {
if (m_child) { delete m_child; m_child = 0; }
if (m_tangent && m_frame) return true; // regen tangent on frame switch
m_reldir = reldir; // for +vel termination condition
m_frame = m_ship->GetFrame();
}
// TODO: collision needs to be processed according to vdiff, not reldir?
Body *body = m_frame->GetBody();
double erad = MaxEffectRad(body, m_ship);
if ((m_target && body != m_target)
|| (m_targframe && (!m_tangent || body != m_targframe->GetBody())))
{
int coll = CheckCollision(m_ship, reldir, targdist, targpos, m_endvel, erad);
if (coll == 0) { // no collision
if (m_child) { delete m_child; m_child = 0; }
}
else if (coll == 1) { // below feature height, target not below
double ang = m_ship->AIFaceDirection(m_ship->GetPosition());
m_ship->AIMatchVel(ang < 0.05 ? 1000.0 * m_ship->GetPosition().Normalized() : vector3d(0.0));
}
else { // same thing for 2/3/4
if (!m_child) m_child = new AICmdFlyAround(m_ship, m_frame->GetBody(), erad*1.05, 0.0);
static_cast<AICmdFlyAround*>(m_child)->SetTargPos(targpos);
ProcessChild();
}
if (coll) { m_state = -coll; return false; }
}
if (m_state < 0 && m_state > -6 && m_tangent) return true; // bail out
if (m_state < 0) m_state = targdist > 10000000.0 ? 1 : 0; // still lame
double maxdecel = m_state ? m_ship->GetAccelFwd() : m_ship->GetAccelRev();
double gravdir = -reldir.Dot(m_ship->GetPosition().Normalized());
maxdecel -= gravdir * GetGravityAtPos(m_ship->GetFrame(), m_ship->GetPosition());
if (maxdecel < 0) maxdecel = 0.0;
// target ship acceleration adjustment
if (m_target && m_target->IsType(Object::SHIP)) {
Ship *targship = static_cast<Ship*>(m_target);
matrix3x3d orient = m_target->GetFrame()->GetOrientRelTo(m_frame);
vector3d targaccel = orient * targship->GetLastForce() / m_target->GetMass();
// fudge: targets accelerating towards you are usually going to flip
if (targaccel.Dot(reldir) < 0.0 && !targship->IsDecelerating()) targaccel *= 0.5;
relvel += targaccel * timestep;
maxdecel += targaccel.Dot(reldir);
// if we have margin lower than 10%, fly as if 10% anyway
maxdecel = std::max(maxdecel, 0.1*m_ship->GetAccelFwd());
}
double curspeed = -relvel.Dot(reldir);
double tt = sqrt(2.0*targdist / maxdecel);
if (tt < timestep) tt = timestep;
vector3d perpvel = relvel + reldir * curspeed;
double perpspeed = perpvel.Length();
vector3d perpdir = (perpspeed > 1e-30) ? perpvel / perpspeed : vector3d(0,0,1);
double sidefactor = perpspeed / (tt*0.5);
if (curspeed > (tt+timestep)*maxdecel || maxdecel < sidefactor) {
m_ship->AIFaceDirection(relvel);
m_ship->AIMatchVel(targvel);
m_state = -5; return false;
}
else maxdecel = sqrt(maxdecel*maxdecel - sidefactor*sidefactor);
// ignore targvel if we could clear with side thrusters in a fraction of minimum time
// if (perpspeed < tt*0.01*m_ship->GetAccelMin()) perpspeed = 0;
// calculate target speed
double ispeed = (maxdecel < 1e-10) ? 0.0 : calc_ivel(targdist, m_endvel, maxdecel);
// cap target speed according to spare fuel remaining
double fuelspeed = m_ship->GetSpeedReachedWithFuel();
if (m_target && m_target->IsType(Object::SHIP)) fuelspeed -=
m_ship->GetVelocityRelTo(Pi::game->GetSpace()->GetRootFrame()).Length();
if (ispeed > curspeed && curspeed > 0.9*fuelspeed) ispeed = curspeed;
// Don't exit a frame faster than some fraction of radius
// double maxframespeed = 0.2 * m_frame->GetRadius() / timestep;
// if (m_frame->GetParent() && ispeed > maxframespeed) ispeed = maxframespeed;
// cap perpspeed according to what's needed now
perpspeed = std::min(perpspeed, 2.0*sidefactor*timestep);
// cap sdiff by thrust...
double sdiff = ispeed - curspeed;
double linaccel = sdiff < 0 ?
std::max(sdiff, -m_ship->GetAccelFwd()*timestep) :
std::min(sdiff, m_ship->GetAccelFwd()*timestep);
// linear thrust application, decel check
vector3d vdiff = linaccel*reldir + perpspeed*perpdir;
bool decel = sdiff <= 0;
m_ship->SetDecelerating(decel);
if (decel) m_ship->AIChangeVelBy(vdiff * m_ship->GetOrient());
else m_ship->AIChangeVelDir(vdiff * m_ship->GetOrient());
// work out which way to head
vector3d head = reldir;
if (!m_state && sdiff < -1.2*maxdecel*timestep) m_state = 1;
if (m_state && sdiff < maxdecel*timestep*60) head = -head;
if (!m_state && decel) sidefactor = -sidefactor;
head = head*maxdecel + perpdir*sidefactor;
// face appropriate direction
if (m_state >= 3) m_ship->AIMatchAngVelObjSpace(vector3d(0.0));
else m_ship->AIFaceDirection(head);
if (body && body->IsType(Object::PLANET) && m_ship->GetPosition().LengthSqr() < 2*erad*erad)
m_ship->AIFaceUpdir(m_ship->GetPosition()); // turn bottom thruster towards planet
// termination conditions: check
if (m_state >= 3) return true; // finished last adjustment, hopefully
if (m_endvel > 0.0) { if (reldir.Dot(m_reldir) < 0.9) return true; }
else if (targdist < 0.5*m_ship->GetAccelMin()*timestep*timestep) m_state = 3;
return false;
}
bool AICmdFlyTo::TimeStepUpdate()
{
double timestep = Pi::game->GetTimeStep();
vector3d targvel = GetVelInFrame(m_ship->GetFrame(), m_targframe, m_posoff);
vector3d relvel = m_ship->GetVelocity() - targvel;
vector3d targpos = GetPosInFrame(m_ship->GetFrame(), m_targframe, m_posoff);
// only if pursuing a ship -- repeat actions from contructor again, since the position of the ship changed
if(m_targetShip != 0) {
m_targframe = m_targetShip->GetFrame();
m_posoff = m_ship->GetPositionRelTo(m_targframe);
m_posoff += m_targetShip->GetPosition();
targpos = m_targetShip->GetPositionRelTo(m_ship->GetFrame());
targpos.x += VICINITY_MIN/2; // avoid collisions, set target a bit away
targvel = m_targetShip->GetVelocityRelTo(m_ship->GetFrame()); // todo: check general frame!
relvel = m_ship->GetVelocity() - targvel;
}
bool safe = ParentSafetyAdjust(m_ship, m_targframe, m_posoff, targpos);
double endvel = safe ? 0.0 : m_endvel; // don't use endvel if safety-adjusted
vector3d relpos = targpos - m_ship->GetPosition();
vector3d reldir = relpos.NormalizedSafe();
double targdist = relpos.Length();
double haveFuelToReachThisVelSafely;
m_fuelEconomy = Clamp(m_fuelEconomy, 0.0f, 1.0f);
haveFuelToReachThisVelSafely = m_ship->GetVelocityReachedWithFuelUsed(1.0/(6-3*m_fuelEconomy) * m_ship->GetFuel());
// sort out gear, launching
if (m_ship->GetFlightState() == Ship::FLYING) m_ship->SetWheelState(false);
else { LaunchShip(m_ship); return false; }
// frame switch stuff - clear children/collision state
if (m_frame != m_ship->GetFrame()) {
if (m_child) { delete m_child; m_child = 0; }
if (m_frame && m_tangent) return true; // regen tangent on frame switch
m_frame = m_ship->GetFrame();
m_reldir = reldir; // for +vel termination condition
}
#ifdef DEBUG_AUTOPILOT
if (m_ship->IsType(Object::PLAYER))
printf("Autopilot dist = %.1f, speed = %.1f, zthrust = %.2f, term = %.3f, crit = %.3f, fuel = %.3f, exhaust = %.0f, safeVel = %.0f, state = %i\n",
targdist, relvel.Length(), m_ship->GetThrusterState().z, reldir.Dot(m_reldir),
relvel.Dot(reldir)/(relvel.Length()+1e-7), m_ship->GetFuel(),
m_ship->GetEffectiveExhaustVelocity() , haveFuelToReachThisVelSafely, m_state);
#endif
Body *body = m_frame->GetBodyFor();
double erad = MaxEffectRad(body, m_ship);
if (m_targetShip == 0 && (!m_tangent || !(body == m_targframe->GetBodyFor())))
{
// process path collisions with frame body
int coll = CheckCollision(m_ship, reldir, targdist, targpos, endvel, erad);
if (coll == 0) { // no collision
if (m_child) { delete m_child; m_child = 0; m_state = -1; }
}
else if (coll == 1) { // below feature height, target not below
double ang = m_ship->AIFaceDirection(m_ship->GetPosition());
m_ship->AIMatchVel(ang < 0.05 ? 1000.0 * m_ship->GetPosition().Normalized() : vector3d(0.0));
m_state = -3; return false;
}
else { // same thing for 2/3/4
if (!m_child) m_child =
new AICmdFlyAround(m_ship, body, erad, 0.0, m_targframe, m_posoff, m_fuelEconomy);
ProcessChild(); m_state = -5; return false;
}
}
// if dangerously close to local body, pretend target isn't moving
if (body) {
double localdist = m_ship->GetPosition().Length();
if (targdist > localdist && localdist < 1.5*MaxFeatureRad(body))
relvel += targvel;
}
// regenerate state to flipmode if we're off course
bool overshoot = CheckOvershoot(m_ship, reldir, targdist, relvel, endvel);
if (m_tangent && m_state == -4 && !overshoot) return true; // bail out
if (m_state < 0) m_state = GetFlipMode(m_ship, relpos, relvel);
// linear thrust
double ang, maxdecel = GetMaxDecel(m_ship, reldir, m_state, &ang);
maxdecel -= GetGravityAtPos(m_targframe, m_posoff);
if(maxdecel <= 0) { m_ship->AIMessage(Ship::AIERROR_GRAV_TOO_HIGH); return true; }
bool cap = m_ship->AIMatchPosVel2(reldir, targdist, relvel, endvel, maxdecel);
// path overshoot check, response
if (m_state < 3 && overshoot) {
double ispeed = calc_ivel(targdist, endvel, maxdecel);
m_ship->AIFaceDirection(ispeed*reldir - relvel);
m_state = -4; return false;
}
// turn thrusters off when in acceleration phase and low on fuel
double angleCos = relvel.Dot(reldir)/(relvel.Length()+1e-7);
if(m_state == 1 && relvel.Length() > haveFuelToReachThisVelSafely &&
// match direction precisely if close, if more than a day far, save the fuel and do not be so strict about the direction
(angleCos > 0.9999 || (angleCos > 0.995 && targdist/(haveFuelToReachThisVelSafely+1e-7) > 86400))
) {
m_ship->SetThrusterState(vector3d(0.0));
} else if(m_state == 1 && relvel.Length() > 1.1*haveFuelToReachThisVelSafely) {
// match direction without increasing speed .. saves fuel in deceleration phase
vector3d v;
matrix4x4d m;
m_ship->GetRotMatrix(m);
v = targvel + relvel.NormalizedSafe()*haveFuelToReachThisVelSafely;
m_ship->AIMatchVel(v);
}
// flip check - if facing forward and not accelerating at maximum
if (m_state == 1 && ang > 0.99 && !cap) m_state = 2;
// termination conditions
if (m_state == 3) m_state++; // finished last adjustment, hopefully
else if (endvel > 0.0) { if (reldir.Dot(m_reldir) < 0.9) m_state = 4; }
else if (targdist < 0.5*m_ship->GetAccelMin()*timestep*timestep) m_state = 3;
// set heading according to current state
if (m_state < 2) { // this shit still needed? yeah, sort of
vector3d newrelpos = targpos - m_ship->AIGetNextFramePos();
if ((newrelpos + reldir*100.0).Dot(relpos) <= 0.0)
m_ship->AIFaceDirection(reldir); // last frames turning workaround
else m_ship->AIFaceDirection(newrelpos);
}
else if (m_state == 2) m_ship->AIFaceDirection(-reldir); // hmm. -relvel instead?
else m_ship->AIMatchAngVelObjSpace(vector3d(0.0));
if (m_state == 4) return true;
return false;
}
