# Determine if an object is inside a ship's turn radius, rotation via torque

I have a 3d space game where all ships move around using physics. I am trying to determine if a given object is reachable by a ship at a given velocity and turn radius. I believe the formula for turn radius movementSpeed / rotation_in_radians but that doesn't seem to be working for me. I think it is because the torque rotations are unpredictable and changing rotations takes extra time because of excess torque in a different direction. There is also a bit of a slip factor in AerodyanmicEffect that probably needs to be accounted for.

I have code to tell if something is within a given turn radius that I am pretty sure works

  public bool InsideTurnRadius(Vector3 point) {
float turnRadius = GetTurnRadius();
var toTarget = point - transform.position;
toTarget = Vector3.ProjectOnPlane(toTarget, transform.forward);
var toCenterNormalized = toTarget.normalized;
var turnRadiusCenter = transform.position + toCenterNormalized * turnRadius;

return ((point - turnRadiusCenter).sqrMagnitude) > (turnRadius * turnRadius);
}


But I don't know how to actually find the right turn radius value to plug in there.

Does anybody know how I can account for all the forces to determine if a target point is reachable?

I am using a cut down and modified version of the Unity Standard Assets Plane script.

public class SpaceCraftController : MonoBehaviour {
[SerializeField]
private float m_MaxEnginePower = 40f;        // The maximum output of the engine.
[SerializeField]
private float m_AerodynamicEffect = 0.5f;   // How much aerodynamics affect the speed of the aeroplane.
[SerializeField]
private float m_ThrottleChangeSpeed = 0.3f;  // The speed with which the throttle changes.

public float Throttle { get; private set; }                     // The amount of throttle being used.
public float ForwardSpeed { get; private set; }                 // How fast the aeroplane is traveling in it's forward direction.
public float EnginePower { get; private set; }                  // How much power the engine is being given.
public float MaxEnginePower { get { return m_MaxEnginePower; } }    // The maximum output of the engine.
public float RollAngle { get; private set; }
public float PitchAngle { get; private set; }
public float RollInput { get; private set; }
public float PitchInput { get; private set; }
public float YawInput { get; private set; }
public float ThrottleInput { get; private set; }

public float maxPitchSpeed = 360;
public float maxRollSpeed = 360;
public float maxYawSpeed = 360;
public bool useAeroFactor = true;

private float m_AeroFactor;
private Rigidbody m_Rigidbody;

[Header("Debug")]
public bool drawVelocity = true;

/* THIS IS WHAT I NEED A HAND WITH */
public float TurnRadius {
get {
float pitchRadius = maxPitchSpeed * Mathf.Deg2Rad;
float yawRadius = maxYawSpeed * Mathf.Deg2Rad;
if (pitchRadius > yawRadius) {
return ForwardSpeed / (yawRadius);
} else {
return ForwardSpeed / (pitchRadius);
}
}
}

private void Start() {
m_Rigidbody = GetComponent<Rigidbody>();
}

public void Move(float rollInput, float pitchInput, float yawInput, float throttleInput) {
RollInput = rollInput;
PitchInput = pitchInput;
YawInput = yawInput;
ThrottleInput = throttleInput;

ClampInputs();

CalculateRollAndPitchAngles();

ControlThrottle();

CaluclateAerodynamicEffect();

CalculateTorque();

if (drawVelocity) {
Debug.DrawRay(transform.position, GetComponent<Rigidbody>().velocity.normalized * ForwardSpeed, Color.white);
}
}

private void ClampInputs() {
RollInput = Mathf.Clamp(RollInput, -1, 1);
PitchInput = Mathf.Clamp(PitchInput, -1, 1);
YawInput = Mathf.Clamp(YawInput, -1, 1);
ThrottleInput = Mathf.Clamp(ThrottleInput, -1, 1);
}

/* ALSO not 100% sure why this works, if anybody knows I'd be very happy to learn*/
private void CalculateRollAndPitchAngles() {
// Calculate roll & pitch angles
// Calculate the flat forward direction (with no y component).
var flatForward = transform.forward;
flatForward.y = 0;
if (flatForward.sqrMagnitude > 0) {
flatForward.Normalize();
// calculate current pitch angle
var localFlatForward = transform.InverseTransformDirection(flatForward);
PitchAngle = Mathf.Atan2(localFlatForward.y, localFlatForward.z);
// calculate current roll angle
var flatRight = Vector3.Cross(Vector3.up, flatForward);
var localFlatRight = transform.InverseTransformDirection(flatRight);
RollAngle = Mathf.Atan2(localFlatRight.y, localFlatRight.x);
}

}

private void ControlThrottle() {
Throttle = Mathf.Clamp01(Throttle + ThrottleInput * Time.deltaTime * m_ThrottleChangeSpeed);
EnginePower = Throttle * m_MaxEnginePower;
m_Rigidbody.AddForce(EnginePower * transform.forward, ForceMode.Acceleration);
var velocity = m_Rigidbody.velocity;
var magnitude = velocity.magnitude;
if (magnitude > 0 && magnitude > m_MaxEnginePower) {
velocity *= (m_MaxEnginePower / magnitude);
m_Rigidbody.velocity = velocity;
}
// Forward speed is the speed in the forward direction (not the same as its velocity)
var localVelocity = transform.InverseTransformDirection(m_Rigidbody.velocity);
ForwardSpeed = Mathf.Max(0, localVelocity.z);
}

private void CaluclateAerodynamicEffect() {
// "Aerodynamic" calculations. This is a very simple approximation of the effect that a plane
// will naturally try to align itself in the direction that it's facing when moving at speed.
// Without this, the plane would behave a bit like the asteroids spaceship!
if (m_Rigidbody.velocity.sqrMagnitude > 0) {
// compare the direction we're pointing with the direction we're moving
//areoFactor is between 1 and -1
m_AeroFactor = Vector3.Dot(transform.forward, m_Rigidbody.velocity.normalized);
// multipled by itself results in a desirable rolloff curve of the effect
m_AeroFactor *= m_AeroFactor;
// Finally we calculate a new velocity by bending the current velocity direction towards
// the the direction the plane is facing, by an amount based on this aeroFactor
var newVelocity = Vector3.Lerp(m_Rigidbody.velocity, transform.forward * ForwardSpeed,
m_AeroFactor * ForwardSpeed *
m_AerodynamicEffect * Time.deltaTime);
m_Rigidbody.velocity = newVelocity;

// also rotate the plane towards the direction of movement
m_Rigidbody.rotation = Quaternion.Slerp(m_Rigidbody.rotation,
Quaternion.LookRotation(m_Rigidbody.velocity, transform.up),
m_AerodynamicEffect * Time.deltaTime);
}
}

private void CalculateTorque() {
var torque = Vector3.zero;
torque += PitchInput * (maxPitchSpeed * Time.deltaTime) * transform.right;
torque += YawInput * (maxYawSpeed * Time.deltaTime) * transform.up;
torque += -RollInput * (maxRollSpeed * Time.deltaTime) * transform.forward;
if (useAeroFactor) {
m_Rigidbody.AddTorque(torque * m_AeroFactor, ForceMode.Acceleration);
} else {
m_Rigidbody.AddTorque(torque, ForceMode.Acceleration);
}


## 1 Answer

You may use splines to predict body trajectory based on short history. For example, script below interpolates trajectory of object with cubic splines then use these spline to extrapolate future movement of the object.

using UnityEngine;
using System.Collections;
using System.Collections.Generic;

// Natural cubic spline

public class CubicSpline
{
#region Public
public struct Cubic
{
public float a;
public float b;
public float c;
public float d;

public void Set(float A, float B, float C, float D)
{
a = A; b = B; c = C; d = D;
}

public float Evaluate(float T)
{
return ( ( ( d * T) + c ) * T + b ) * T + a;
}
};

public float[] ControlPoints = new float;
public Cubic[] Cubics = new Cubic;
public int NumControlPoints { get { return ControlPoints.Length; } }
#endregion

#region Private
private float[] _gamma;
private float[] _delta;
private float[] _D;
#endregion

#region Interface
public CubicSpline(int numControlPoints)
{
Resize( Mathf.Max( numControlPoints, 3 ) );
}

public void Resize(int numControlPoints)
{
ControlPoints = new float[numControlPoints];
Cubics = new Cubic[numControlPoints];
_gamma = new float[numControlPoints];
_delta = new float[numControlPoints];
_D = new float[numControlPoints];
}

public void Calculate(int numControlPoints)
{
if( Cubics.Length < numControlPoints ) System.Array.Resize<Cubic>( ref Cubics, numControlPoints );
if( _gamma.Length < numControlPoints ) System.Array.Resize<float>( ref _gamma, numControlPoints );
if( _delta.Length < numControlPoints ) System.Array.Resize<float>( ref _delta, numControlPoints );
if( _D.Length < numControlPoints ) System.Array.Resize<float>( ref _D, numControlPoints );

int n = numControlPoints-1;
int i;

_gamma = 1.0f/2.0f;
for ( i = 1; i < n; i++)
{
_gamma[i] = 1/(4-_gamma[i-1]);
}
_gamma[n] = 1/(2-_gamma[n-1]);

_delta = 3*(ControlPoints-ControlPoints) * _gamma;
for ( i = 1; i < n; i++)
{
_delta[i] = (3*(ControlPoints[i+1]-ControlPoints[i-1])-_delta[i-1])*_gamma[i];
}
_delta[n] = (3*(ControlPoints[n]-ControlPoints[n-1])-_delta[n-1])*_gamma[n];

_D[n] = _delta[n];
for ( i = n-1; i >= 0; i--)
{
_D[i] = _delta[i] - _gamma[i]*_D[i+1];
}

for( i = 0; i < n; i++ )
{
Cubics[i].Set(
ControlPoints[i],
_D[i],
3*(ControlPoints[i+1] - ControlPoints[i]) - 2*_D[i] - _D[i+1],
2*(ControlPoints[i] - ControlPoints[i+1]) + _D[i] + _D[i+1]
);
}
}
#endregion
};

// Trajectory

public class SplineTrajectory : MonoBehaviour
{
public int NumHistoryPoints = 100;
public int NumControlPoints = 5;

CubicSpline _xSpline = null;
CubicSpline _ySpline = null;
CubicSpline _zSpline = null;

float[] _xControlPoints;
float[] _yControlPoints;
float[] _zControlPoints;

List<Vector3> _xyzHistory = new List<Vector3>();

// Use this for initialization
void Start ()
{
_xControlPoints = new float[NumControlPoints];
_yControlPoints = new float[NumControlPoints];
_zControlPoints = new float[NumControlPoints];
}

// Update is called once per frame
void LateUpdate ()
{
_xyzHistory.Add( transform.position );
if( _xyzHistory.Count > NumHistoryPoints ) _xyzHistory.RemoveAt( 0 );
if( _xyzHistory.Count >= NumControlPoints )
{
if( _xSpline == null ) _xSpline = new CubicSpline( NumControlPoints );
if( _ySpline == null ) _ySpline = new CubicSpline( NumControlPoints );
if( _zSpline == null ) _zSpline = new CubicSpline( NumControlPoints );

for( int i=0; i<NumControlPoints; i++ )
{
int indexInHistory = Mathf.Clamp( i*_xyzHistory.Count/(NumControlPoints-1), 0, _xyzHistory.Count-1 );
_xControlPoints[i] = _xyzHistory[indexInHistory].x;
_yControlPoints[i] = _xyzHistory[indexInHistory].y;
_zControlPoints[i] = _xyzHistory[indexInHistory].z;
}

_xSpline.ControlPoints = _xControlPoints;
_ySpline.ControlPoints = _yControlPoints;
_zSpline.ControlPoints = _zControlPoints;

_xSpline.Calculate( NumControlPoints );
_ySpline.Calculate( NumControlPoints );
_zSpline.Calculate( NumControlPoints );
}
}

void OnDrawGizmos()
{
if( _xSpline != null && _ySpline != null && _zSpline != null )
{
const int Details = 10;

Gizmos.color = Color.red;

Vector3 p0 = new Vector3( _xSpline.ControlPoints, _ySpline.ControlPoints, _zSpline.ControlPoints );
Vector3 p1 = Vector3.zero;

for( int i=0; i<NumControlPoints-1; i++ )
{
for( int j=0; j<Details; j++ )
{
p1.x = _xSpline.Cubics[i].Evaluate( (float)(j) / (float)(Details) );
p1.y = _ySpline.Cubics[i].Evaluate( (float)(j) / (float)(Details) );
p1.z = _zSpline.Cubics[i].Evaluate( (float)(j) / (float)(Details) );

Gizmos.DrawLine( p0, p1 );

p0 = p1;
}
}

Gizmos.color = Color.green;

for( int i=0; i<NumControlPoints-1; i++ )
{
for( int j=0; j<Details; j++ )
{
p1.x = _xSpline.Cubics[NumControlPoints-2].Evaluate( 1.0f + (float)(i*Details+j)/(float)(Details) );
p1.y = _ySpline.Cubics[NumControlPoints-2].Evaluate( 1.0f + (float)(i*Details+j)/(float)(Details) );
p1.z = _zSpline.Cubics[NumControlPoints-2].Evaluate( 1.0f + (float)(i*Details+j)/(float)(Details) );

Gizmos.DrawLine( p0, p1 );

p0 = p1;
}
}
}
}
}

• Thanks for the suggestion, I can see how this would be useful but I don't think it quite solves my problem because a ship can be flying straight for a bit and then the data extrapolated from the spline will not accurately reflect the turn radius. I think the right answer has something to do with computing angular acceleration required to turn to a given orientation. Still unsure of the math there. – weichsem Apr 28 '15 at 12:41