# Converging at position and angle using motor-based rigidbody movement

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. 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. 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. 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 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;

? 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);
return;
}

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

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;
}