Vector3 SlowDogCurve()
{
min.Clear();
var line = GetComponent <LineRenderer>();
//find desired velocity vector
var Vt = target.GetComponent <Rigidbody>().velocity;
var Vm0 = rb.velocity;
var Pt0 = target.transform.position;
var Pm0 = transform.position;
float Sm = 20; //speed of missile, fixed constant
//loop over all time t = [0, 100)
line.SetWidth(.1f, 1f);
line.SetPosition(0, new Vector3(40, 0, 0));
line.SetPosition(1, Vector3.zero);
line.SetPosition(2, new Vector3(0, 40, 0));
int i = 0;
for (float t = 0.1f; t < 97; t += 0.1f)
{
//assuming constant acceleration
var Pt = t * Vt + Pt0;
//assuming constant velocity
Vector3 Vm, Pm;
if (false) //constant velocity
{
Vm = (Pt - Pm0).normalized * Sm;
Pm = t * Vm + Pm0;
}
else
{
//a = 2 * (Vt * t + Pt0 - Pm0 - Vm0 * t) / (t*t);
var Pm1 = Vm0 * t;
Vm = Pt - Pm0 - Pm1;
var a = Vm.normalized * MaxAcceleration;
Pm = Pm0 + Pm1 + a * t * t / 2;
}
var dist = (Pt - Pm).magnitude;
min.Update(dist, t, Vm);
line.SetPosition(i + 3, new Vector3((float)i / 10, dist / 10, 0));
i++;
}
Logdump.Log("t=" + min.time + " minDist: " + min.index + " minV: " + min.result.magnitude);
//InterceptionMarker.transform.position = min.time * Vt + Pt0;
//if too slow, use target velocity vector
//var desiredVm = (min.index > 1 ) ? min.result : Vt;
var desiredVm = min.result;
transform.LookAt(desiredVm.normalized + transform.position);
//InterceptionMarker.transform.position = desiredVm.normalized * 5 + transform.position;
var changeReq = MaxAcceleration * desiredVm.normalized;// desiredVm - rb.velocity;
//changeReq = changeReq.normalized * MaxAcceleration;// Mathf.Min(changeReq.magnitude, MaxAcceleration); //cap change speed, but still in direction of desired vel, not necessarily orthogonal to current Vm though...
//rb.velocity = minVm;
return(changeReq);
}
void Update()
{
var x = SpaceScale * 60 * Mathf.Sin(Time.time / 10);
var y = SpaceScale * 30 * Mathf.Cos(Time.time / 10);
var z = SpaceScale * 20 * Mathf.Sin(Mathf.PI * Time.time / 10);
transform.position = new Vector3(x, y, z);
var velocity = (transform.position - lastPos);
var speed = velocity.magnitude / Time.deltaTime;
lastPos = transform.position;
Logdump.Log("target speed: " + speed);
var accel = (velocity - lastVel).magnitude / Time.deltaTime;
Logdump.Log("target accel: " + accel);
}
void ProportionalNavigation()
{
//accel = N * Vc * LOS_rate
//N = navigation gain/constant
//Vc = closing velocity
//line of sight rotation rate
//to get LOS rate
var newRTM = target.transform.position - transform.position;
if (oldRTM == Vector3.zero)
{
oldRTM = newRTM;
Logdump.Log("no oldRtm");
return;
}
Vector3 aimLoc;
{
//thanks to Desprez from Unity forum for the pro nav implementation!
//check out his Unity assets at: http://mobfarmgames.weebly.com/
float pnGain = N;
Vector3 dirDelta = newRTM - oldRTM;
dirDelta -= Vector3.Project(dirDelta, newRTM);
// basic pro nav
//Vector3 a = dirDelta * pnGain;
// augmented pro nav
Vector3 a = dirDelta * (pnGain + (dirDelta.magnitude * pnGain * .5f));
aimLoc = (newRTM.normalized * rb.velocity.magnitude * Time.fixedDeltaTime) + a;
}
//limit turn rate
var turnRate = Vector3.Angle(transform.forward, aimLoc);
Logdump.Log("Turn rate: " + turnRate);
turnRate = Mathf.Min(turnRate, MaxTurnRate * Time.deltaTime);
var aimLocQuat = Quaternion.LookRotation(aimLoc);
transform.rotation = Quaternion.Slerp(transform.rotation, aimLocQuat, turnRate / MaxTurnRate);
//transform.LookAt(aimLoc + transform.position);
oldRTM = newRTM;
}
// Update is called once per frame
void FixedUpdate()
{
if (Time.time - BornTime > DieAfterTime)
{
Logdump.Log("Born time: " + BornTime + " Now: " + Time.time);
Destroy(gameObject);
return;
}
if (Time.time - BornTime > FireDelay)
{
if (target == null)
{
particles.SetActive(false);
return;
}
particles.SetActive(true);
var rocketAccel = Vector3.zero;
//rocketAccel += ProportionalNavigation();
if (true) // proportional navigation
{
ProportionalNavigation();
var throttle = 1;// Mathf.Min(LOSRateReal*LOSRateReal/180f, 1);
rocketAccel += transform.forward * MaxAcceleration * (throttle) * ManualThrottle;
//SlowDogCurve();
}
else // slow dog curve
{
rocketAccel = SlowDogCurve();
}
Logdump.Log("accel: " + rocketAccel.magnitude);
Logdump.Log("velocity: " + rb.velocity.magnitude);
rb.velocity += rocketAccel * Time.deltaTime;
//tail chase
//rb.velocity += (target.transform.position - transform.position).normalized * MaxAcceleration;
//rb.velocity = rocketAccel;
//transform.rotation = Quaternion.LookRotation(rb.velocity);
}
}
