I am given a rigidbody positioned in \$\mathbb{R}^2\$. It is a box of length \$l\$ and height \$\epsilon\$. The position of the box's left center is described by \$\vec{p}\$, and its angle from the x-axis is \$\theta\$. The body is in a vacuum (no friction), and its mass is \$m\$ (uniformly distributed). Here's a quick diagram.

enter image description here

Next, I am given a target position and angle to reach. It is not guaranteed that the body will reach the target position and angle the next time frame.

enter image description here

I need to calculate the necessary force \$F\$ and torque \$T\$ that will push the body closest to the target. I am given constraints for max force and max torque.

enter image description here

To make things complicated, the body might be affected by external forces. One of them is gravity \$F_g\$, which will always remain constant. Sometimes, normal forces exist as well. The applied forces or the total force is unknown, but the velocity or the angular velocity is available.

I tried to solve this problem using steering behaviors. However, the angle seems to fluctuate constantly. Here's part of my code in C# (Unity).

\$\vec{p}_0=\$transform.position \$\theta_0=\$transform.eulerAngles.z \$\vec{p}_t=\$position \$\theta_t=\$angle

private RigidBody _body;

public float SlowDownRadius = 2f;
public float MaxVelocity = 50f;
public float MaxForce = 10000f;

public float SlowDownDelta = 15f;
public float MaxAngVelocity = 180f;
public float MaxTorque = 10000f;

public void Next(Vector3 position, float angle) // angle in degrees
    // force
    Vector2 to = position;
    var from = (Vector2) transform.position;
    var desired = to - from;
    var dist = desired.sqrMagnitude;

    desired = desired.SetMagnitude(dist < SlowDownRadius * SlowDownRadius
        ? MaxVelocity * Mathf.Sqrt(dist) / SlowDownRadius
        : MaxVelocity);

    Vector2 vel = _body.velocity;
    var steering = desired - vel;

    var mass = _body.mass;
    Vector2 force = mass * UnityEngine.Physics.gravity;
    var time = Time.deltaTime;

    var newForce =
        (time <= 0 ? new Vector2() : (mass / time * steering)) - force;

    _body.AddForce(newForce.Clamp(MaxForce)); // limits magnitude of newForce to MaxForce

    // torque
    var toAngle = angle;
    var fromAngle = transform.eulerAngles.z;
    var desiredAngle = AngleDistance(fromAngle, toAngle);
    var distAngle = Mathf.Abs(desiredAngle);

    if (distAngle < 1f)
        transform.eulerAngles = new Vector3(0, 0, angle);

    desiredAngle = Mathf.Sign(desiredAngle) * (distAngle < SlowDownDelta
                  ? MaxAngVelocity * distAngle / SlowDownDelta
                  : MaxAngVelocity) * Mathf.Deg2Rad;

    var angVel = _body.angularVelocity.z; // in radians
    var steeringAngle = desiredAngle - angVel;

    var newTorque = (time <= 0 ? 0f : (mass / time * steeringAngle));

    _body.AddTorque(0, 0, Mathf.Sign(newTorque) * Mathf.Min(Mathf.Abs(newTorque), MaxTorque));

public static float AngleDistance(float from, float to)
    var dist = (to - from + 180f) % 360f - 180f;
    return dist <= -180f ? dist + 360 : dist;

If I made any errors, please let me know. Thank you in advance!


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.