Move a rigid body to a point with an orientation

Started by
5 comments, last by LorenzoGatti 7 months, 2 weeks ago

Hi all, I'm a n00b at game programming, but an experienced coder, and this is driving me nuts.

I have a top-down NPC spaceship which has a thruster which pushes it forward.

It also has Left and Right steering controls (a float of -1 to 1) which cause the ship to rotate around its Y (up) axis by some `rotation_amount*delta`.

Lets assume thrust is always 1 so the ship is headed forward.

I want to specify “Go to (x,y) but make sure when you get there you're facing a particular direction”.

With the ship having +1 thrust, this will be some kind of curve or combination of curves. It may not be able to even get there because of the turning circle.

I could just slow the ship down as it got towards the point, but wondering if theres an elegant solution I'm not quite smart enough to grasp...? What should I be looking for?

Thank you!

Advertisement

Thanks @fleabay I did try playing with Bézier curves, but I wasn't sure how to cope with the issue where the curve gets too sharp. And I also wasn't sure how it would translate to the control system.

The main idea was that the NPC had the same control system as the player, and the AI would more or less do what a human would do. But struggling with it, and I assume this is a pretty common req in games?

I guess theres 2 things, a linear direction and an ending angle, and the challenge is how to smoothly merge those two requirements. Hmm… 🤔

You can try with a parametrized cubic function, with the parameter (think “time”) goes between 0 (current) and 1 (target). In the code below, you can specify the position and velocity at 0 and at 1 (current and target), and it will produce an entire path of values.

#include <iostream>
#include <cmath>

template <typename T>
struct CubicSpline {
  T a, b, c, d;
  CubicSpline(T p0, T v0, T p1, T v1) :
    a(2.0 * p0 + v0 - 2.0 * p1 + v1),
    b(-3.0 * p0 - 2.0 * v0 + 3.0 * p1 - v1),
    c(v0),
    d(p0) {
  }
  
  T operator()(double x) {
    return d + x * (c + x * (b + x * a));
  }
};

struct Vector3 {
  double x, y, z;
};

Vector3 operator+(Vector3 v1, Vector3 v2) {
  return Vector3{v1.x+v2.x, v1.y+v2.y, v1.z+v2.z};
}

Vector3 operator-(Vector3 v1, Vector3 v2) {
  return Vector3{v1.x-v2.x, v1.y-v2.y, v1.z-v2.z};
}

Vector3 operator*(double s, Vector3 v) {
  return Vector3{s*v.x, s*v.y, s*v.z};
}

std::ostream &operator<<(std::ostream &os, Vector3 v) {
  return os << '(' << v.x << ',' << v.y << ',' << v.z << ')';
}

int main() {
  Vector3 current_position{0.0, 0.0, 0.0};
  Vector3 current_velocity{0.0, 1.0, 0.0};
  Vector3 target_position{5.0, 6.0, 7.0};
  Vector3 target_velocity{0.0, 1.0, 1.0};
  
  CubicSpline<Vector3> f(current_position, current_velocity, target_position, target_velocity);
  for (int step = 0; step <= 32; ++step) {
    double x = step / 32.0;
    Vector3 position = f(x);
    // I'll compute an approximation to the derivative, to check that the velocities are what we want
    const double epsilon = 1.0e-6;
    Vector3 velocity = (1.0 / epsilon) * (f(x+epsilon) - f(x));
    std::cout << step << ' ' << position << ' ' << velocity << '\n';
  }
}

greg6 said:
but wondering if theres an elegant solution I'm not quite smart enough to grasp...? What should I be looking for?

There are many ways to tackle such control problems. I can quickly list a few of them…

The first thing you might want to to do is figuring out how to bring a rigid body to a target for our next frame.
If you use rigid body simulation, you might want to avoid setting velocities directly, but instead you usually try to apply force and torque so the desired velocities are generated from that.
That's easy for the linear part to calculate force, but the angular part is harder, as you need to factor in nonuniform inertia to calculate torque.

After you make this work, there is a problem: Your body is at target position in the next frame as desired, but it also still has high velocity at this point, so in the frame after that you ‘overshoot’ the target and miss it again.
If you calculate forces again to bring it back, your body will oscillate forth and back, which is not what we want.
So we realize: We do not only want to drive to target, we also want to control velocity. E.g. we want it to be zero at the target, so the body sticks there.

Knowing that, we now have a better definition of our problem.
We have current state (position and velocity).
We have a target state (position and velocity).
We want to calculate a trajectory to drive from current to target, and we might also want to calculate the time it will take to get there. Probably we also want to minimize both energy and time to get to the target.

Splines are a common way to calculate a trajectory. If we use a cubic Bezier with 4 control points, the end points represent current position and target, and the lines from end point to the closer internal control point can represent velocity vectors.
However, there is some other spline which is better suited to model physics than Bezier. Unfortunately i have forgotten which one that was.

Another way i often use is to give maximum and minimum accelerations, and then calculate the trajectory respecting those limitations working with the equations of motion directly.
This is nice because it also gives the time to target, and it even gives time to the point where we switch from acceleration to deceleration phase.
Because we apply constant acceleration to the body during those phases, the resulting trajectory is perfectly smooth and natural.

However, it's a bit complicated. I get very similar results from using damping, and that's really easy to use.
Here's some snippets (1D, 3D, and quaternion) using critically damping, which does not overshoot:

static float SmoothCD (float from, float to, float &vel, float smoothTime, float deltaTime)
{
	// Programming Gems 4
	float omega = 2.f/smoothTime;
	float x = omega*deltaTime;
	float exp = 1.f/(1.f+x+0.48f*x*x+0.235f*x*x*x);
	float change = from - to;
	float temp = (vel+omega*change)*deltaTime;
	vel = (vel - omega*temp)*exp;
	return to + (change+temp)*exp;
}

static sVec3 SmoothCD (sVec3 from, sVec3 to, sVec3 &vel, float smoothTime, float deltaTime)
{
	// Programming Gems 4
	float omega = 2.f/smoothTime;
	float x = omega*deltaTime;
	float exp = 1.f/(1.f+x+0.48f*x*x+0.235f*x*x*x);
	sVec3 change = from - to;
	sVec3 temp = (vel+omega*change)*deltaTime;
	vel = (vel - omega*temp)*exp;
	return to + (change+temp)*exp;
}

static sQuat SmoothCD (sQuat from, sQuat to, sVec3 &vel, float smoothTime, float deltaTime)
{
	auto QuatFromAToB = [](const sQuat &qA, const sQuat &qB) // global
	{
		sQuat q;
		if (qA.Dot(qB) < 0.0f) 
		{
			q[0] = qA[0]; q[1] = qA[1]; q[2] = qA[2];
			q[3] = -qA[3];
		}
		else
		{
			q[0] = -qA[0]; q[1] = -qA[1]; q[2] = -qA[2];
			q[3] = qA[3];
		}
		
		return qB * q;
	};

	// Programming Gems 4
	float omega = 2.f/smoothTime;
	float x = omega*deltaTime;
	float exp = 1.f/(1.f+x+0.48f*x*x+0.235f*x*x*x);
	sVec3 change = sQuat(QuatFromAToB (to, from)).ToRotationVector(); // todo: small angle approx?
	sVec3 temp = (vel+omega*change)*deltaTime;
	vel = (vel - omega*temp)*exp;

	//return to + (change+temp)*exp;
	sQuat rot; rot.FromRotationVector((change+temp)*exp); // todo: small angle approx?
	return rot * to;
}

To use it you only need to know current velocity. Target velocity is assumed to be zero, and you don't get any time output.
But for most applications that's good enough, smooth and simple.
deltaTime is the timestep, and a larger value for smoothTime makes the results smooth but less responsive.
The velocity you give gets modified so it becomes the new velocity you want to have.

EDIT: Forgot to mention the returned value is the new position or orientation.

Another thing many people use is PID controllers. But i lack experience and can't talk about that.

Thanks heaps @alvaro @joej and @fleabay really appreciate the points. And the code. This was my first post here, and you are amazingly kind to share.

I need to think about your comments a bit more, and see if I can make it work. It feels like it's just a smidgen out of my reach at the moment, so probably need to sleep on it.

Thank you all again

Slowing the ship down is usually the more elegant solution compared to overshooting the destination and turning back. Reducing lateral thrust is also useful to achieve smooth trajectories.

Constant forward thrust is also problematic; in the simple case of not turning you probably want constant velocity (i.e. no thrust), not constant acceleration.

Omae Wa Mou Shindeiru

This topic is closed to new replies.

Advertisement