## Using torque to always push a rigidbody upright while on ground

nev6502
Posts: 3
Joined: Sun Jun 16, 2019 5:52 am

### Using torque to always push a rigidbody upright while on ground

Hello; this is my first post so please take it easy on me I have recently switched to C++ / Pullet from a combination of BlitzMax / ODE.

I want to start by showing what I'm trying to achieve. Here is a quick video of a first person hockey game where a character ends up being toppled by various means and always pushes back up to a standing position:

https://streamable.com/go15l

I was able to achieve this by applying torque to a rigidbody on BlitzMax because the ODE library came with Pitch/Yaw/Roll (0-360) retrieval built in, which I'm entirely comfortable with. I could use how far the character's pitch and roll were from zero (upright) when applying torque to achieve a standing up state and eventually built in measures to not overshoot.

I think you know where I'm going with this; I've entered the world of transforms & quaternions and I'm lost. I have scoured this forum and found similar posts about applying various forces to keep a rigidbody upright, and I've tried to find information with regards to retrieving / converting what bullet provides to Pitch Yaw and Roll. I've tried and tried so many examples and everything I've produced either flat out doesn't work properly or starts acting strangely once a rigid body's yaw goes 90 degrees in either direction.

I'm hoping that someone can help me along with this either via pseudocode or real code to demonstrate what I'm actually looking for. The character in the example above is built with a collection of spheres (I've accomplished this with btCompound). The part I don't understand how to do with this combination of tools is making the character stand up without affecting the yaw rotation. I don't know if it's possible to retrieve 360 degree Pitch/Yaw/Roll vectors with Bullet or if there is possibly a much better method at achieving this behavior. At this stage I'm happy to admit I'm a bit ignorant on the subject; but I've spent many days researching and trying.

It seems I've been able to learn so much but I don't even know where to begin here Thank you for your time
drleviathan
Posts: 839
Joined: Tue Sep 30, 2014 6:03 pm
Location: San Francisco

### Re: Using torque to always push a rigidbody upright while on ground

Here is how I would do it:

Rather than apply impulse I would implement a custom "action" (e.g. derive UprightCharacterAction from btActionInterface) which behaves like an "angular spring" and slams the velocity of the character toward an upright position. The main reason I don't like to operate in impulse space is because the impulse required to achieve a desired deltaAngularVelocity must be scaled by the inverse inertia tensor, else you get odd results and even instabilities.

So your UprightCharacterAction class might look something like this:

Code: Select all

``````const btScalar MIN_ANGLE(0.017453); // 0.017453 radians ~= 1 degree
const btScalar MIN_DOT_PRODUCT = btCos(MIN_ANGLE);
const btScalar SUBSTEP_DURATION(1.0 / 60.0);
const btScalar MIN_TIMESCALE = 2.0 * SUBSTEP_DURATIOB; // for stability
const btVector3 DEFAULT_UP_AXIS(0.0, 1.0, 0.0);

class UprightCharacterAction : public btActionInterface {
public:
UprightCharacterAction(btRigidBody* body, const btVector3& up = DEFAULT_UP_AXIS) : m_body(body) {}

void setWorldUp(const btVector3& worldUp) {
m_worldUp = worldUp;
m_worldUp.normalize();
}

void setLocalUp(const btVector3& localUp) {
m_localUp = localUp;
m_localUp.normalize();
}

void setSpringTimescale(btScalar timescale) { m_springTimescale = btMax(MIN_TIMESCALE, timescale); }

void setBlendTimescale(btScalar timescale) { m_blendTimescale = btMax(MIN_TIMESCALE, timescale); }

void updateAction(btCollisionWorld* collisionWorld, btScalar deltaTimeStep) override {
if (m_body->isActive()) {
// compute localUpInWorldFrame
btTransform worldTransform = m_body->getWorldTransform();
worldTransform.setOrigin(btVector3(0.0, 0.0, 0.0));
btVector3 localUpInWorldFrame = worldTransform * m_localUp;

// compute targetAngularVelocity
btVector3 targetAngularVelocity(0.0, 0.0, 0.0);
// Checking the dotProduct is cheaper/faster than computing the angle, so we do that first.
// (When two normalized vectors are parallel their dotProduct is 1.0.  Otherwise it is less.)
btScalar dotProduct = localUpInWorldFrame.dot(m_worldUp);
if (dotProduct < MIN_DOT_PRODUCT) {
btVector3 axis = localUpInWorld.cross(m_worldUp).normalize();
btScalar angle = btAcos(dotProduct);
// NOTE: for very fast upright behavior set m_springTimescale very small (two substeps is as low as allowed)
// for very slow upright behavior set m_springTimescale longer (several seconds would be very slow)
targetAngularVelocity = (angle / m_springTimescale) * axis;
}

// decompose currentAngularVelocity into spin and upright parts
// because we only want to modify the part that orients the character toward UP
btVector3 currentAngularVelocity = m_body->getAngularVelocity;
btVector3 spinVelocity = currentAngularVelocity.dot(m_worldUp) * m_worldUp;
btVector3 uprightVelocity = currentAngularVelocity - spinVelocity;

// blend targetAngularVelocity into uprightAngularVelocity
// NOTE: for very agressive upright behavior set the m_blendTimescale very small (two substeps is as low as allowed)
// for weak upright behavior set it longer (several seconds would be very weak)
btScalar del = btMin(btScalar(0.99), deltaTimStep / m_blendTimescale); // must clamp for stability
btVector3 newUprightVelocity = (btScalar(1.0) - del) * uprightAngularVelocity + del * targetAngularVelocity;

// set newAngularVelocity
btVector3 newAngularVelocity = spinVelocity + newUprightVelocity;
m_body->setAngularVelocity(spinVelocity + newUprightVelocity);
}
}

private:
btVector3 m_worldUp { DEFAULT_UP_AXIS };
btVector3 m_localUp { DEFAULT_UP_AXIS };
btScalar m_springTimescale { 0.5 };
btScalar m_blendTimescale { 0.5 };
btRigidBody* m_body;
};
``````
Note: I wrote that from scratch and did not test. I don't know if it compiles and there may very well be errors, but it should be close.

What is that class doing?

There are two "timescales". Each timescale is used to perform an exponential attraction toward a target. You can tune the behavior of your action by adjusting the timescales. I picked some default likely values but they might not be what you want.

The m_springTimescale determines the "uprighting" velocity as if you had a "critically damped angular spring with zero velocity history" constantly pulling the character upright. To make this spring fast you would reduce the timescale to be short. To make the spring slow and lazy you would use longer values. A good way to tune this is to ask yourself the following question:

"Ignoring any external bumps, how many seconds should it take for the character to upright from sideways?"

Suppose the answer was "two seconds". Then you would divide your answer by three to get a good timescale:

Code: Select all

``uprightAction.setSpringTimescale(timeToGetUp / 3.0);``
The reason is because the timescale dictates exponential decay and the delta angle decreases to 1/e of its original value after each timescale. After three timescales the delta angle would be (1/e)^3 which is pretty small.

The m_blendTimescale is used to prevent the upright behavior from completely overwriting whatever current angular velocity it already has, since otherwise the action would completely erase any external bumps. If you make springTimescale very short but the blendTimescale longer then you tend to get a bouncy spring. If you make the blendTimescale very short then you just get the exponential decay of the spring and all external bumps tend to get "erased". Tuning this value is a little trickier. My advice: set it to the m_springTimescale and then adjust up/down to see how it affects things.

Finally I will mention how the btActionInterface works: In this specific example you would need to instantiate one UprightCharacterAction, passing it a pointer to the RigidBody on which it is supposed to act, and then give it to the world:

Code: Select all

``````UprightCharacterAction* action = new UprightCharacterAction(characterBody);
The world will then call action->updateAction(world, dt) once every substep. It is the developer's duty to implement that method to do the Right Thing.

I hope that helps. Good luck.
nev6502
Posts: 3
Joined: Sun Jun 16, 2019 5:52 am

### Re: Using torque to always push a rigidbody upright while on ground

I wanted to quickly post thank you so much for such a detailed answer! I will work on this in the evening and let you know if there is anything that I'm unable to understand

Again I really appreciate your help
nev6502
Posts: 3
Joined: Sun Jun 16, 2019 5:52 am

### Re: Using torque to always push a rigidbody upright while on ground

Thank you for your help with this drleviathan! I haven't been able to get the action itself working, but by isolating the meat of it I was able to get it working and boy does it work perfectly (in fact, it is much more stable than my old method of using torque)! I still don't fully understand why it works, but you have given me enough to start studying and thinking about this problem differently.

I will look further into how the btActionInterface works to get that working. A couple of questions:

a) Are there any guides or tutorials out there that you would recommend to understand this different way of thinking? For many years I've always used Pitch/Yaw/Roll to figure these types of things out. When it comes to transforms / quaternions I'm lost, but I'm beginning to see the light and why it's better to go about my business using them.

b) Using ODE I was able to use CFM on rigidbodies to allow various amounts of penetration between rigidbodies (softer collisions but not softbody collisions). Does bullet have anything like this? I understand that in most cases this is not something that would be desired, but in my case there are some neat things that can be done with this. Any ideas? An example can be provided if I'm not describing this very well
drleviathan
Posts: 839
Joined: Tue Sep 30, 2014 6:03 pm
Location: San Francisco

### Re: Using torque to always push a rigidbody upright while on ground

If you have Quaternion or 3D math questions you can ask on this forum or on the live IRC channel on freenode.

When it comes to how my example works I can offer this insight:

Assume the displacement has an exponential decay time function of the form x(t) = X * exp(-t / tau) where tau is the timescale.

From calculus class we know: velocity is the first derivative of displacement. Fortunately the derivative of an exponential curve is very simple: has the same form except for a different constant out front.

v(t) = d/dt x(t)
v(t) = X * d/dt exp(-t / tau)
v(t) = - (1 / tau) * X * exp(- t / tau)
v(t) = - (1 / tau) * X(t)

That is why: if you know the displacement at some time t AND you assume exponential decay then you can easily calculate the velocity at time t that would make it so: you just multiply by 1/tau and make sure it is pointing in the correct direction. By forcing the velocity toward the assumption the displacement behaves in the assumed manner.

Aside from the 3D math stuff that is all there is to the algorithm.

I do not know what CFM means. Perhaps you're asking about changing the material properties such as restitution and friction? If so, yes the RigidBody supports such properties.