This article discusses an approach to making physics bodies rotate smoothly, whether they are stationary or actively moving. I used cocos2d-x and Box2D, but the basic approach will work for any physics body (even a 3D one if you are trying to rotate in a 2D plane).
The approach uses a Proportional-Integral-Derivative (PID) control loop on the angle of the body to apply rotational force to it in a well controlled and predictable manner. Two examples of using the approach are shown in the video, one for a missile that can only move in the direction it is facing and another for an "Entity" like a game character that moves independent of its facing direction.
The video below shows this in action. You don't have to watch the whole video to get the idea, but there is a lot in there to see...
Whether your game is in 2-D or 3-D, you often have the need to make an object "turn" to face another direction. This could be a character's walking direction as they are moving, the direction they are shooting while sitting crouched, the direction a missile is flying in, the direction a car is racing towards, etc. This is the job of the "character controller", the piece of code in your system responsible for the basic "movement" operations that a character must undergo (seek, turn, arrive, etc.).
Building games that use physics engines is a lot of fun and adds a level of realism to the game that can dramatically improve the gameplay experience. Objects collide, break, spin, bounce, and move in more realistic ways.
Moving in more realistic ways is not what you usually think about, though, when you think about facing direction. Your usual concern is something like "The character needs to turn to face left in 0.5 seconds." From the standpoint of physics, this means you want to apply forces to make it turn 90° left in 0.5 seconds. You want it to stop exactly on the spot. You don't want to worry about things like angular momentum, which will tend to keep it turning unless you apply counter force. You really don't want to think about applying counter force to make it stop "on a dime". Box2D will allow you to manually set the position and angle of a body. However, if you manually set the position and angle of a physics body in every frame, it can interfere (in my experience) with the collision response of the physics engine.
Most important of all, this is a physics engine. You should be using it as such to make bodies move as expected.
Our goal is to create a solution to change the facing direction of the body by applying turning force (torque) to it.
If we decouple the problem of "how it turns" from "how it moves", we can use the same turning solution for other types of moving bodies where the facing direction needs to be controlled. For this article, we are considering a missile that is moving towards its target.
Here, the missile is moving in a direction and has a given velocity. The angle of the velocity is measured relative to the x-axis. The "facing direction" of the missile is directly down the nose and the missile can only move forward. We want to turn it so that it is facing towards the target, which is at a different angle. For the missile to hit the target, it has to be aiming at it. Note that if we are talking about an object that is not moving, we can just as easily use the angle of the body relative to the x-axis as the angle of interest.
The basic idea behind a control system is to take the difference of "what you want the value to be" and "what the value is" and adjust your input to the system so that, over time, the system converges to your desired value.
From this wikipedia article:
There is a huge body of knowledge in controls system theory. Polynomials, poles, zeros, time domain, frequency domain, state space, etc. It can seem daunting to the uninitiated. It can seem daunting to the initiated as well! That being said, while there are more "modern" solutions to controlling the facing direction, we're going to stick with PID Control. PID control has the distinct advantages of having only three parameters to "tune" and a nice intuitive "feel" to it.
Let's start with the basic variable we want to "control", the difference between the angle we want to be facing and the angle of the body/velocity:
Here, \(e(t)\) is the "error". We want to drive the error to 0. We apply forces to the body to make it turn in a direction to make it turn so that it moves \(e(t)\) towards 0. To do this, we create a function \(f(.)\), feed \(e(t)\) into it, and apply torque to the body based on it. Torque makes bodies turn:
The first and most obvious choice is to apply a torque that is proportional to the \(e(t)\) itself. When the error is large, large force is applied. When the error is small, small force is applied. Something like:
And this would work. Somewhat. The problem is that when the error is small, the corrective force is small as well. So as the body is turning \(e(t)\) gets small (nearing the intended angle), the retarding force is small also. So the body overshoots and goes past the angle intended. Then you start to swing back and eventually oscillate to a steady state. If the \(K_p\) is not too large, it should settle into "damped" (exponentially decaying) sinusoid, dropping a large amount each oscillation (stable solution). It may also spiral off towards infinity (unstable solution), or just oscillate forever around the target point (marginally stable solution). If you reduce \(K_p\) so that it is not moving so fast, then when \(e(t)\) is large, you don't have a lot of driving force to get moving.
A pure proportional error also has a bias (Steady State Error) that keeps the final output different from the input. The error is a function of \(K_p\) of the form:
So increasing the \(K_p\) value makes the bias smaller (good). But this will also make it oscillate more (bad).
The next term to add is the integral term, the "I" in PID:
For each time step, if \(e(t)\) has a constant value, the integral term will work to counter it:
We don't have to calculate the actual integral. We probably don't want to anyway since it stretches back to \(-\infty\) and an error back in the far past should have little effect on our near term decisions.
We can estimate the integral over the short term by summing the value of \(e(t)\) over the last several cycles and multiplying by the time step (Euler Integration) or some other numerical technique. In the code base, the Composite Simpson's Rule technique was used.
Most PID controllers stop at the "PI" version. The proportional part gets the output swinging towards the input and the integral part knocks out the bias or any steady external forces that might be countering the proportional control. However, we still have oscillations in the output response. What we need is a way to slow down as the body is heading towards the target angle. The proportional and integral components work to push towards it. By looking at the derivative of \(e(t)\), we can estimate its value in the near term and apply force to drive it towards not changing. This is a counter-force to the proportional and integral components:
Consider what happens when \(e(t)\) is oscillating. Its behavior is like a sine function. The derivative of this is a cosine function and its maximum occurs when sin(e(t)) = 0. That is to say, the derivative is largest when \(e(t)\) is swinging through the position we want to achieve. Conversely, when the oscillation is at the edge, about to change direction, its rate of change switches from positive to negative (or vice versa), so the derivative is smallest (minimum). So the derivative term will apply counter force hardest when the body is swinging towards the point we want to be at, countering the oscillation, and least when we are at either edge of the "swing".
Just like the integral, the derivative can be estimated numerically. This is done by taking differences over the last several \(e(t)\) values (see the code).
Because we are software minded, whatever algorithm we want to use for a PID controller, we want to wrap it into a convenient package, give it a clean interface, and hide everything except what the user needs. This needs to be "owned" by the entity that is doing the turning.
The MovingEntityInterface represents a "Moving Entity". In the case of this demo, it can be an entity like a Missile, which moves forward only, or a "character", which can turn while moving. While they have different methods internally for "applying thrust" they both have nearly identical methods for controlling turning. This allows the implementation of a "seek" behavior tailored more to the entity type.
The interface itself is generic so that the MainScene class can own an instance and manipulate it without worrying about what type it is.
The PIDController class itself has this interface:
This is a very simple class to use. You set it up calling the SetKXXX functions as needed, set the time step for integration, and call AddSample(...) each update cycle with the error term.
Looking at the Missile class, which owns an instance of this, the step update (called in Update) looks like this:
If you look carefully at the video, there is a distinct difference in the way path following works for the missile vs. the character (called the MovingEntity in the code). The missile can overshoot the path easily, especially when its turn rate is turned down and it is only moving forward.
The MovingEntity always moves more directly towards the points because it is using a "vector feedback" of its position vs. the target position to adjust its velocity. This is more like a traditional "seek" behavior than the missile.
I have also, quite deliberately, left out a bit of key information on how to tune the constants for the PID controller. There are numerous articles on Google for how to tune a PID control loop, and I have to leave something for you to do, after all.
You will also note that the default value for _dt, the time step, is set for 0.01 seconds. You can adjust this value to match the timestep you actually intend to use, but there will be tradeoffs in the numerical simulation (error roundoff, system bandwidth concerns, etc.) that you will encounter. In practice, I use the same controller with the same constants across multipe sized physical entities without tweaking and the behavior seems realistic enough (so far) that I have not had to go and hunt for minor tweaks to parameters. Your mileage may vary.
The source code for this, written in cocos2d-x/C++, can be found on github here. The PIDController class has no dependencies other than standard libraries and should be portable to any system.
29 Oct 2014: Initial release
The approach uses a Proportional-Integral-Derivative (PID) control loop on the angle of the body to apply rotational force to it in a well controlled and predictable manner. Two examples of using the approach are shown in the video, one for a missile that can only move in the direction it is facing and another for an "Entity" like a game character that moves independent of its facing direction.
The video below shows this in action. You don't have to watch the whole video to get the idea, but there is a lot in there to see...
Facing Direction
Whether your game is in 2-D or 3-D, you often have the need to make an object "turn" to face another direction. This could be a character's walking direction as they are moving, the direction they are shooting while sitting crouched, the direction a missile is flying in, the direction a car is racing towards, etc. This is the job of the "character controller", the piece of code in your system responsible for the basic "movement" operations that a character must undergo (seek, turn, arrive, etc.).
Building games that use physics engines is a lot of fun and adds a level of realism to the game that can dramatically improve the gameplay experience. Objects collide, break, spin, bounce, and move in more realistic ways.
Moving in more realistic ways is not what you usually think about, though, when you think about facing direction. Your usual concern is something like "The character needs to turn to face left in 0.5 seconds." From the standpoint of physics, this means you want to apply forces to make it turn 90° left in 0.5 seconds. You want it to stop exactly on the spot. You don't want to worry about things like angular momentum, which will tend to keep it turning unless you apply counter force. You really don't want to think about applying counter force to make it stop "on a dime". Box2D will allow you to manually set the position and angle of a body. However, if you manually set the position and angle of a physics body in every frame, it can interfere (in my experience) with the collision response of the physics engine.
Most important of all, this is a physics engine. You should be using it as such to make bodies move as expected.
Our goal is to create a solution to change the facing direction of the body by applying turning force (torque) to it.
If we decouple the problem of "how it turns" from "how it moves", we can use the same turning solution for other types of moving bodies where the facing direction needs to be controlled. For this article, we are considering a missile that is moving towards its target.
Here, the missile is moving in a direction and has a given velocity. The angle of the velocity is measured relative to the x-axis. The "facing direction" of the missile is directly down the nose and the missile can only move forward. We want to turn it so that it is facing towards the target, which is at a different angle. For the missile to hit the target, it has to be aiming at it. Note that if we are talking about an object that is not moving, we can just as easily use the angle of the body relative to the x-axis as the angle of interest.
Feedback Control Systems 101
The basic idea behind a control system is to take the difference of "what you want the value to be" and "what the value is" and adjust your input to the system so that, over time, the system converges to your desired value.
From this wikipedia article:
A familiar example of a control loop is the action taken when adjusting hot and cold faucets (valves) to maintain the water at a desired temperature. This typically involves the mixing of two process streams, the hot and cold water. The person touches the water to sense or measure its temperature. Based on this feedback they perform a control action to adjust the hot and cold water valves until the process temperature stabilizes at the desired value.
There is a huge body of knowledge in controls system theory. Polynomials, poles, zeros, time domain, frequency domain, state space, etc. It can seem daunting to the uninitiated. It can seem daunting to the initiated as well! That being said, while there are more "modern" solutions to controlling the facing direction, we're going to stick with PID Control. PID control has the distinct advantages of having only three parameters to "tune" and a nice intuitive "feel" to it.
PID Control
Let's start with the basic variable we want to "control", the difference between the angle we want to be facing and the angle of the body/velocity:
\(e(t) = desired - actual\)
Here, \(e(t)\) is the "error". We want to drive the error to 0. We apply forces to the body to make it turn in a direction to make it turn so that it moves \(e(t)\) towards 0. To do this, we create a function \(f(.)\), feed \(e(t)\) into it, and apply torque to the body based on it. Torque makes bodies turn:
\(torque(t) = I * f(e(t)), I \equiv Angular Inertia \)
Proportional Feedback
The first and most obvious choice is to apply a torque that is proportional to the \(e(t)\) itself. When the error is large, large force is applied. When the error is small, small force is applied. Something like:
\(f(e(t)) = K_p * e(t)\)
And this would work. Somewhat. The problem is that when the error is small, the corrective force is small as well. So as the body is turning \(e(t)\) gets small (nearing the intended angle), the retarding force is small also. So the body overshoots and goes past the angle intended. Then you start to swing back and eventually oscillate to a steady state. If the \(K_p\) is not too large, it should settle into "damped" (exponentially decaying) sinusoid, dropping a large amount each oscillation (stable solution). It may also spiral off towards infinity (unstable solution), or just oscillate forever around the target point (marginally stable solution). If you reduce \(K_p\) so that it is not moving so fast, then when \(e(t)\) is large, you don't have a lot of driving force to get moving.
A pure proportional error also has a bias (Steady State Error) that keeps the final output different from the input. The error is a function of \(K_p\) of the form:
\(Steady State Error = \frac{desired}{ [1 + constant * K_p]} \)
So increasing the \(K_p\) value makes the bias smaller (good). But this will also make it oscillate more (bad).
Integral Feedback
The next term to add is the integral term, the "I" in PID:
\(f(e(t)) = K_p * e(t) + \int\limits_{-\infty}^{now} K_i * e(t) \)
For each time step, if \(e(t)\) has a constant value, the integral term will work to counter it:
- If direction to the target suddenly changes a small amount, then over each time step, this difference will build up and create turning torque.
- If there is a bias in the direction (e.g. Steady State Error), this will accumulate over the time steps and be countered.
We don't have to calculate the actual integral. We probably don't want to anyway since it stretches back to \(-\infty\) and an error back in the far past should have little effect on our near term decisions.
We can estimate the integral over the short term by summing the value of \(e(t)\) over the last several cycles and multiplying by the time step (Euler Integration) or some other numerical technique. In the code base, the Composite Simpson's Rule technique was used.
Derivative Feedback
Most PID controllers stop at the "PI" version. The proportional part gets the output swinging towards the input and the integral part knocks out the bias or any steady external forces that might be countering the proportional control. However, we still have oscillations in the output response. What we need is a way to slow down as the body is heading towards the target angle. The proportional and integral components work to push towards it. By looking at the derivative of \(e(t)\), we can estimate its value in the near term and apply force to drive it towards not changing. This is a counter-force to the proportional and integral components:
\(f(e(t)) = K_p * e(t) + \int\limits_{-\infty}^{now} K_i * e(t) dt + K_d * \frac{de(t)}{dt}\)
Consider what happens when \(e(t)\) is oscillating. Its behavior is like a sine function. The derivative of this is a cosine function and its maximum occurs when sin(e(t)) = 0. That is to say, the derivative is largest when \(e(t)\) is swinging through the position we want to achieve. Conversely, when the oscillation is at the edge, about to change direction, its rate of change switches from positive to negative (or vice versa), so the derivative is smallest (minimum). So the derivative term will apply counter force hardest when the body is swinging towards the point we want to be at, countering the oscillation, and least when we are at either edge of the "swing".
Just like the integral, the derivative can be estimated numerically. This is done by taking differences over the last several \(e(t)\) values (see the code).
Note: Using derivative control is not usually a good idea in real control systems. Sensor noise can make it appear as if \(e(t)\) is changing rapidly back and forth, causing the derivative to spike back and forth with it. However, in our case, unless we are looking at a numerical issue, we should not have a problem.
Classes and Sequences
Because we are software minded, whatever algorithm we want to use for a PID controller, we want to wrap it into a convenient package, give it a clean interface, and hide everything except what the user needs. This needs to be "owned" by the entity that is doing the turning.
The MovingEntityInterface represents a "Moving Entity". In the case of this demo, it can be an entity like a Missile, which moves forward only, or a "character", which can turn while moving. While they have different methods internally for "applying thrust" they both have nearly identical methods for controlling turning. This allows the implementation of a "seek" behavior tailored more to the entity type.
The interface itself is generic so that the MainScene class can own an instance and manipulate it without worrying about what type it is.
The PIDController class itself has this interface:
/********************************************************************
* File : PIDController.h
* Project: Interpolator
*
********************************************************************
* Created on 10/13/13 By Nonlinear Ideas Inc.
* Copyright (c) 2013 Nonlinear Ideas Inc. All rights reserved.
********************************************************************
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any
* damages arising from the use of this software.
*
* Permission is granted to anyone to use this software for any
* purpose, including commercial applications, and to alter it and
* redistribute it freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must
* not claim that you wrote the original software. If you use this
* software in a product, an acknowledgment in the product
* documentation would be appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and
* must not be misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source
* distribution.
*/
#ifndef __Interpolator__PIDController__
#define __Interpolator__PIDController__
#include "CommonSTL.h"
#include "MathUtilities.h"
/* This class is used to model a Proportional-
* Integral-Derivative (PID) Controller. This
* is a mathemtical/control system approach
* to driving the state of a measured value
* towards an expected value.
*
*/
class PIDController
{
private:
double _dt;
uint32 _maxHistory;
double _kIntegral;
double _kProportional;
double _kDerivative;
double _kPlant;
vector<double> _errors;
vector<double> _outputs;
enum
{
MIN_SAMPLES = 3
};
/* Given two sample outputs and
* the corresponding inputs, make
* a linear pridiction a time step
* into the future.
*/
double SingleStepPredictor(
double x0, double y0,
double x1, double y1,
double dt) const
{
/* Given y0 = m*x0 + b
* y1 = m*x1 + b
*
* Sovle for m, b
*
* => m = (y1-y0)/(x1-x0)
* b = y1-m*x1
*/
assert(!MathUtilities::IsNearZero(x1-x0));
double m = (y1-y0)/(x1-x0);
double b = y1 - m*x1;
double result = m*(x1 + dt) + b;
return result;
}
/* This funciton is called whenever
* a new input record is added.
*/
void CalculateNextOutput()
{
if(_errors.size() < MIN_SAMPLES)
{ // We need a certain number of samples
// before we can do ANYTHING at all.
_outputs.push_back(0.0);
}
else
{ // Estimate each part.
size_t errorSize = _errors.size();
// Proportional
double prop = _kProportional * _errors[errorSize-1];
// Integral - Use Extended Simpson's Rule
double integral = 0;
for(uint32 idx = 1; idx < errorSize-1; idx+=2)
{
integral += 4*_errors[idx];
}
for(uint32 idx = 2; idx < errorSize-1; idx+=2)
{
integral += 2*_errors[idx];
}
integral += _errors[0];
integral += _errors[errorSize-1];
integral /= (3*_dt);
integral *= _kIntegral;
// Derivative
double deriv = _kDerivative * (_errors[errorSize-1]-_errors[errorSize-2]) / _dt;
// Total P+I+D
double result = _kPlant * (prop + integral + deriv);
_outputs.push_back(result);
}
}
public:
void ResetHistory()
{
_errors.clear();
_outputs.clear();
}
void ResetConstants()
{
_kIntegral = 0.0;
_kDerivative = 0.0;
_kProportional = 0.0;
_kPlant = 1.0;
}
PIDController() :
_dt(1.0/100),
_maxHistory(7)
{
ResetConstants();
ResetHistory();
}
void SetKIntegral(double kIntegral) { _kIntegral = kIntegral; }
double GetKIntegral() { return _kIntegral; }
void SetKProportional(double kProportional) { _kProportional = kProportional; }
double GetKProportional() { return _kProportional; }
void SetKDerivative(double kDerivative) { _kDerivative = kDerivative; }
double GetKDerivative() { return _kDerivative; }
void SetKPlant(double kPlant) { _kPlant = kPlant; }
double GetKPlant() { return _kPlant; }
void SetTimeStep(double dt) { _dt = dt; assert(_dt > 100*numeric_limits<double>::epsilon());}
double GetTimeStep() { return _dt; }
void SetMaxHistory(uint32 maxHistory) { _maxHistory = maxHistory; assert(_maxHistory >= MIN_SAMPLES); }
uint32 GetMaxHistory() { return _maxHistory; }
void AddSample(double error)
{
_errors.push_back(error);
while(_errors.size() > _maxHistory)
{ // If we got too big, remove the history.
// NOTE: This is not terribly efficient. We
// could keep all this in a fixed size array
// and then do the math using the offset from
// the beginning and module math. But this
// gets complicated fast. KISS.
_errors.erase(_errors.begin());
}
CalculateNextOutput();
}
double GetLastError() { size_t es = _errors.size(); if(es == 0) return 0.0; return _errors[es-1]; }
double GetLastOutput() { size_t os = _outputs.size(); if(os == 0) return 0.0; return _outputs[os-1]; }
virtual ~PIDController()
{
}
};
This is a very simple class to use. You set it up calling the SetKXXX functions as needed, set the time step for integration, and call AddSample(...) each update cycle with the error term.
Looking at the Missile class, which owns an instance of this, the step update (called in Update) looks like this:
void ApplyTurnTorque()
{
Vec2 toTarget = GetTargetPos() - GetBody()->GetPosition();
float32 angleBodyRads = MathUtilities::AdjustAngle(GetBody()->GetAngle());
if(GetBody()->GetLinearVelocity().LengthSquared() > 0)
{ // Body is moving
Vec2 vel = GetBody()->GetLinearVelocity();
angleBodyRads = MathUtilities::AdjustAngle(atan2f(vel.y,vel.x));
}
float32 angleTargetRads = MathUtilities::AdjustAngle(atan2f(toTarget.y, toTarget.x));
float32 angleError = MathUtilities::AdjustAngle(angleBodyRads - angleTargetRads);
_turnController.AddSample(angleError);
// Negative Feedback
float32 angAcc = -_turnController.GetLastOutput();
// This is as much turn acceleration as this
// "motor" can generate.
if(angAcc > GetMaxAngularAcceleration())
angAcc = GetMaxAngularAcceleration();
if(angAcc < -GetMaxAngularAcceleration())
angAcc = -GetMaxAngularAcceleration();
float32 torque = angAcc * GetBody()->GetInertia();
GetBody()->ApplyTorque(torque);
}
Nuances
If you look carefully at the video, there is a distinct difference in the way path following works for the missile vs. the character (called the MovingEntity in the code). The missile can overshoot the path easily, especially when its turn rate is turned down and it is only moving forward.
The MovingEntity always moves more directly towards the points because it is using a "vector feedback" of its position vs. the target position to adjust its velocity. This is more like a traditional "seek" behavior than the missile.
I have also, quite deliberately, left out a bit of key information on how to tune the constants for the PID controller. There are numerous articles on Google for how to tune a PID control loop, and I have to leave something for you to do, after all.
You will also note that the default value for _dt, the time step, is set for 0.01 seconds. You can adjust this value to match the timestep you actually intend to use, but there will be tradeoffs in the numerical simulation (error roundoff, system bandwidth concerns, etc.) that you will encounter. In practice, I use the same controller with the same constants across multipe sized physical entities without tweaking and the behavior seems realistic enough (so far) that I have not had to go and hunt for minor tweaks to parameters. Your mileage may vary.
The source code for this, written in cocos2d-x/C++, can be found on github here. The PIDController class has no dependencies other than standard libraries and should be portable to any system.
Article Update Log
3 Nov 2014: Text correction in "Integral" section.29 Oct 2014: Initial release