1
\$\begingroup\$

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

    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!

\$\endgroup\$

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.