Anyway the code is here if anyone wants to comment.
Code: Select all
- (double) missileTrackPrimaryTarget:(double) delta_t
{
Vector relPos;
GLfloat d_forward, d_up, d_right, range2;
Entity *target = [self primaryTarget];
if (!target) // leave now!
return 0.0;
Vector leading = [target velocity];
double damping = 0.5 * delta_t;
double rate2 = 4.0 * delta_t;
double rate1 = 2.0 * delta_t;
double stick_roll = 0.0; //desired roll and pitch
double stick_pitch = 0.0;
relPos = vector_subtract(target->position, position);
//how long till I get there
float lead = sqrt(magnitude2(relPos)) / [self speed];
//adjust where we are going to take into account his velocity
relPos.x += (lead * leading.x);
relPos.y += (lead * leading.y);
relPos.z += (lead * leading.z);
range2 = magnitude2(relPos);
if (!vector_equal(relPos, kZeroVector)) relPos = vector_normal(relPos);
else relPos.z = 1.0;
d_right = dot_product(relPos, v_right); // = cosine of angle between angle to target and v_right
d_up = dot_product(relPos, v_up); // = cosine of angle between angle to target and v_up
d_forward = dot_product(relPos, v_forward); // = cosine of angle between angle to target and v_forward
// begin rule-of-thumb manoeuvres
stick_roll = 0.0;
if (pitching_over)
pitching_over = (stick_pitch != 0.0);
if ((d_forward < -pitch_tolerance) && (!pitching_over))
{
pitching_over = YES;
if (d_up >= 0)
stick_pitch = -max_flight_pitch;
if (d_up < 0)
stick_pitch = max_flight_pitch;
}
if (pitching_over)
{
pitching_over = (d_forward < 0.5);
}
else
{
stick_pitch = -max_flight_pitch * d_up;
stick_roll = -max_flight_roll * d_right;
}
// end rule-of-thumb manoeuvres
// apply damping
if (flightRoll < 0)
flightRoll += (flightRoll < -damping) ? damping : -flightRoll;
if (flightRoll > 0)
flightRoll -= (flightRoll > damping) ? damping : flightRoll;
if (flightPitch < 0)
flightPitch += (flightPitch < -damping) ? damping : -flightPitch;
if (flightPitch > 0)
flightPitch -= (flightPitch > damping) ? damping : flightPitch;
// apply stick movement limits
if (flightRoll + rate1 < stick_roll)
stick_roll = flightRoll + rate1;
if (flightRoll - rate1 > stick_roll)
stick_roll = flightRoll - rate1;
if (flightPitch + rate2 < stick_pitch)
stick_pitch = flightPitch + rate2;
if (flightPitch - rate2 > stick_pitch)
stick_pitch = flightPitch - rate2;
// apply stick to attitude
flightRoll = stick_roll;
flightPitch = stick_pitch;
//
// return target confidence 0.0 .. 1.0
//
if (d_forward < 0.0)
return 0.0;
return d_forward;
}