Skip to content

Instantly share code, notes, and snippets.

@omikun

omikun/DogCurve.cs

Last active July 18, 2017 02:23
Show Gist options
  • Save omikun/12cf269b3536c085e4ddbbea0ac57125 to your computer and use it in GitHub Desktop.
Save omikun/12cf269b3536c085e4ddbbea0ac57125 to your computer and use it in GitHub Desktop.
Download ZIP
Missile homing algorithm
Raw
constant_velocity_interception.cs
//from http://jaran.de/goodbits/2011/07/17/calculating-an-intercept-course-to-a-target-with-constant-direction-and-velocity-in-a-2-dimensional-plane/
//convert that to 3D
//from wolfram alpha
//s * t = sqrt((x + t * v)^2 + (y + t * u) ^2 + (z + t * w)^2), find t
//t = (sqrt((-2 u x - 2 v y - 2 w z)^2 - 4 (-x^2 - y^2 - z^2) (s^2 - u^2 - v^2 - w^2)) + 2 u x + 2 v y + 2 w z)/(2 (s^2 - u^2 - v^2 - w^2))
//where x,y,z are position coordinates of target at t=0
//where u,v,w are velocity coordinates for target (target assumed to have constant velocity)
//once we find the time of interception, we can find the position of the interception I
//I = targetPos + t * targetVel
//we know the vector for our projectile from:
//projectileVel = (I - ProjectilePos)/t
class Intercept {
Vector3 targetPos, projectilePos;
Vector3 targetVel;
float projectileSpeed;
float GetTime()
{
var offsetPos = targetPos - projectilePos;
var x = offsetPos.x;
var y = offsetPos.y;
var z = offsetPos.z;
var u = targetVel.x;
var v = targetVel.y;
var w = targetVel.z;
var x2 = x * x;
var y2 = y * y;
var z2 = z * z;
var u2 = u * u;
var v2 = v * v;
var w2 = w * w;
var s = projectileSpeed;
var s2 = s * s;
var t = (Mathf.Sqrt(Mathf.Pow(-2 * u * x - 2 * v * y - 2 * w * z, 2f)
- 4 * (-x2 - y2 - z2) (s2 - u2 - v2 - w2)
)
+ 2 * u * x + 2 * v * y + 2 * w * z)
/(2 (s2 - u2 - v2 - w2))
return t;
}
//I = targetPos + t * targetVel
Vector3 GetInterception()
{
var t = GetTime();
var I = targetPos + t * targetVel;
return I;
}
//we know the vector for our projectile from:
//projectileVel = (I - ProjectilePos)/t
Vector3 GetProjectileVelocity()
{
var t = GetTime();
var I = targetPos + t * targetVel;
return (I - projectilePos) / t;
}
}
Raw
DogCurve.cs
//now supports missile acceleration!
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++;
}
Debug.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;
}
Raw
missilehoming.cs
//https://pastebin.com/yu9ZmKbS - normal missile guidance
//https://pastebin.com/mpqWFVbX - this is for orbital interception
public float turn = 20f; //turning rate
void FixedUpdate()
{
var dir = target.transform.position - transform.position;
var lookAtDir = Quaternion.LookRotation(dir);
//smoothly turn towards desired vector!
transform.MoveRotation(Quaternion.RotateTowards(transform.rotation, lookAtDir, turn));
}
Raw
MissileLogic.cs
//still doesn't work, youtube link at https://www.youtube.com/watch?v=fECzNWnftrM&feature=youtu.be
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;
//Vc m/s towards target; closing rate
newRTM = newRTM;
if (oldRTM == Vector3.zero)
{
oldRTM = newRTM;
Debug.Log("no oldRtm");
return;
}
var LOSDelta = (newRTM.normalized - oldRTM.normalized);
var Vc = -1 * (newRTM.magnitude - oldRTM.magnitude) / Time.deltaTime;
//var LOSDelta = (oldRTM - newRTM);
oldRTM = newRTM;
var LOSRate = Vector3.Angle(transform.forward, LOSDelta); //ideal
Vc = 1;
var turnRate = N * Vc * LOSRate;
var origTurnRate = turnRate;
turnRate = Mathf.Min(turnRate, MaxTurnRate * Time.deltaTime); //overturn/realistic
turnRate = Mathf.Max(turnRate, -MaxTurnRate * Time.deltaTime); //overturn/realistic
var turnAxis = Vector3.Cross(transform.forward, LOSDelta); //TODO might need to referse the order
transform.Rotate(turnAxis.normalized * turnRate );
Debug.Log("LOSDelta: " + LOSDelta.magnitude/Time.deltaTime);
Debug.Log("LOSRate " + LOSRate + " origTurn: " + origTurnRate + " turnRate " + turnRate + " Vc: " + Vc);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment