Getting and setting angular velocity in body space?

GeanGean
Posts: 7
Joined: Sat Feb 04, 2023 11:04 pm

Getting and setting angular velocity in body space?

Post by GeanGean »

I'm trying to create a motorcycle, the standard gyroscopic effect doesn't seem to be enough to keep it from tipping over.
I've been trying to find a way to basically apply a rotation around the long axis of the bike inversely proportionate to how far tilted the bike is on that axis.
I thought this would be really simple, but pretty much everything I try causes the bike to glitch out horribly eventually.

My original plan went something like this:

Code: Select all

btQuaternion quat = carBody->getWorldTransform().getRotation();
float angX,angY,angZ;
quat.getEulerZYX(&angZ,&angY,&angX);
btVector3 angVel = carBody->getAngularVelocity();
angVel.setZ(angVel.getZ() + -angZ);
carBody->setAngularVelocity(angVel * someScalingFactor);
When this didn't work, I tried something really simple:

Code: Select all

btVector3 angVel = carBody->getAngularVelocity();
angVel.setZ(-angVel.getZ());
carBody->setAngularVelocity(angVel);
And I noticed that actually did work... so long as the car was generally facing a straight direction. As soon as you turned it, it would glitch out. Which I assume is because it's getting angular velocity according to the world's Z axis, not the car's Z axis.

How can I get and set angular velocity according to the body's orientation, or more generally correct an object tipping over on one axis?
User avatar
drleviathan
Posts: 849
Joined: Tue Sep 30, 2014 6:03 pm
Location: San Francisco

Re: Getting and setting angular velocity in body space?

Post by drleviathan »

For a rolling motorcycle the tilt of the vehicle is a balance between the torque from gravity and that of the centripetal force. In the case where the road is flat and the motorcycle has a forward speed and some tilt angle (from the vertical) then the torque balance relation looks like:

(1) mass * speed^2 * cos(angle) / radius = mass * gravity * sin(angle)

Solving for radius we get:

(2) radius = speed^2 * cot(angle) / gravity

The centripetal force provides relation between speed, radius, and turn_rate

(3) speed = turn_rate * radius

Solving (3) for turn_rate and substituting with (2) we get:

(4) turn_rate = cot(angle) / (gravity * speed)

The more general case, where the road could be tilted is trickier but can be done using 3D math. Assuming the motorcycle has both wheels on the road (no wheelies) and is in a balanced turn on a slanted road...

First define some direction vectors:

up = unit vector pointing opposite of gravity
normal = normal of plane of the road
dorsal = unit vector pointing up from motorcycle top and rider's head
forward = direction of the motorcycle's velocity (assuming no fading around turn)
sideways = forward (X) normal (where (X) is the cross product operator)

The torque balance equation becomes:

(5) ((mass * speed^2 / radius) * |(sideways (X) dorsal) (.) forward| = (mass * gravity) * |(dorsal (X) up) (.) forward|

Where (X) is the cross operation and (.) is the dot. Note, I've taken the absolute value of the vector operation because we don't need to sweat the sign of each side: we know by assumption the motorcycle is in balance. I've dotted both sides with forward to collapse the vector equation to scalar. Effectively, I'm projecting the vector relation onto the only axis that matters: the one that is parallel to forward velocity of the motorcycle.

I will introduce the following shorthand:

DxU.F = |(dorsal (X) up) (.) forward|
SxD.F = |(sideways (X) dorsal) (.) forward|

And solving for radius gives:

(6) radius = speed^2 * SxD.f / (gravity * DxU.f)

Plug (6) into (3) and solve for turn_rate to get:

(7) turn_rate = speed * SxD.f / (gravity * DxU.f)

This provides the magnitude of the turn_rate as a function of the motorcycle's forward speed and the geometry of the road. The axis of the associated angular_velocity vector is the normal of the road. The sign (+/-) has been lost, but it can be recovered by checking the dot-product of dorsal and sideways, and the pseudo-code would look something like:

if (sideways (.) dorsal) < 0.0 then sign = 1.0, else sign = -1.0
angular_velocity = sign * turn_rate * normal

Note: these vector relations are true in all reference frames, so it is ok do the math in the world-frame (e.g. when all direction vectors have been measured in or transformed to the world-frame), since that is the one in which you need to apply the final angular_velocity.

This derivation really deserves a sketch to help visualize everything but I'm too lazy to draw, snap, and upload one. That is left as an exercise for the reader. I've double-checked my math, but it is possible I made a typo. Best to walk through it and if you spot any problems or have questions please let me know.
GeanGean
Posts: 7
Joined: Sat Feb 04, 2023 11:04 pm

Re: Getting and setting angular velocity in body space?

Post by GeanGean »

Thanks for taking the time to write such an involved reply.
Unfortunately I can't quite seem to get it to work.
Here's my attempt:

Code: Select all


            btRigidBody *car = common.brickCars[a]->body;

            btTransform worldTransform = car->getWorldTransform();
            worldTransform.setOrigin(btVector3(0,0,0));

            btVector3 up = btVector3(0,1,0);
            btVector3 normal = btVector3(0,1,0);
            btVector3 dorsal = worldTransform.inverse() * up;
            btVector3 forward = car->getLinearVelocity().normalized();

            btVector3 sideways = btCross(forward,normal);
            float dxuf = fabs(btDot(btCross(dorsal,up),forward));
            float sxdf = fabs(btDot(btCross(sideways,dorsal),forward));

            if(isnanf(dxuf))
                continue;
            if(isnanf(sxdf))
                continue;
            if(dxuf == 0)
                continue;
            if(sxdf == 0)
                continue;

            std::cout<<"DXUF: "<<dxuf<<" SXDF:"<<sxdf<<" Speed: "<<car->getLinearVelocity().length()<<" Gravity: "<<common.physicsWorld->getGravity().y()<<"\n";
            float turn_rate = (car->getLinearVelocity().length() * sxdf) / (fabs(common.physicsWorld->getGravity().y()) * dxuf);
            std::cout<<turn_rate<<"\n";

            btVector3 angVel = (btDot(sideways,dorsal) < 0.0 ? 1.0 : -1.0) * turn_rate * normal;
            std::cout<<angVel.x()<<","<<angVel.y()<<","<<angVel.z()<<"\n";

            car->setAngularVelocity(angVel);
I think the most obvious problem is that if angular velocity is just turn_rate times the normal, and the normal is 0,1,0, (i.e. a flat ground pointing up) then the ang vel will just be vec3(0,something,0) aka a rotation around the Y axis. This makes me think I misinterpreted your statement "The axis of the associated angular_velocity vector is the normal of the road."
User avatar
drleviathan
Posts: 849
Joined: Tue Sep 30, 2014 6:03 pm
Location: San Francisco

Re: Getting and setting angular velocity in body space?

Post by drleviathan »

I have fixed a few bugs in your code, and added a bunch of comments:

Code: Select all

            btRigidBody *car = common.brickCars[a]->body;

            btTransform worldTransform = car->getWorldTransform();
            worldTransform.setOrigin(btVector3(0,0,0));

            btVector3 up = btVector3(0,1,0);

            // When we set 'normal' to be 'up' we are assuming the road is flat
            // (e.g. the motorcycle is driving on a flat plane).
            // However, if the road banks around turns and/or goes up hills then
            // what we really want to do is "measure" the road normal
            // using custom logic, like ray-trace probes, or analyze the collision
            // points of the vehicle's wheels to extract the true effective normal.
            btVector3 normal = btVector3(0,1,0);

            // worldTransform moves local-frame to world-frame
            // so there is no need to invert it.
            btVector3 dorsal = worldTransform * up;

            // 'forward' is the world-frame version of 'localForward'
            // I assume localForward points along negative Z-axis
            // since that is often how avatars (and maybe vehicles too?) are modeled.
            // If this is not the case then please update with correct local direction.
            btVector3 localForward = btVector3(0, 0, -1);
            btVector3 forward = worldTransform * localForward; // now in world-frame

            // Note: 'sideways' is not 'right direction' of the vehicle
            // but points 'right' along the plane of the road.
            btVector3 sideways = btCross(forward,normal);

            float dxuf = fabs(btDot(btCross(dorsal,up),forward));
            float sxdf = fabs(btDot(btCross(sideways,dorsal),forward));

            if(isnanf(dxuf))
                continue;
            if(isnanf(sxdf))
                continue;
            if(dxuf == 0)
                continue;
            if(sxdf == 0)
                continue;
            std::cout<<"DXUF: "<<dxuf<<" SXDF:"<<sxdf<<" Speed: "<<car->getLinearVelocity().length()<<" Gravity: "<<common.physicsWorld->getGravity().y()<<"\n";

            // The motorcycle might have velocity components perpendicular to 'forward'
            // (e.g. it might be sliding sideways, or rising/falling in the vertical)
            // so we extract just the 'forward' component of its linearVelocity
            // by dotting it with the 'forward' unit direction.
            btVector3 linearVelocity = car->getLinearVelocity();
            btScalar forwardSpeed = btDot(linearVelocity, forward);

            // Assuming motorcycle is in a turn-balanced lean as it drives 'forward'
            // we apply the formula to compute the angular speed of that turn.
            float turn_rate = (forwardSpeed * sxdf) / (fabs(common.physicsWorld->getGravity().y()) * dxuf);
            std::cout<<turn_rate<<"\n";

            // Recover the direction of the turnVelocity by checking how 'dorsal' dots with 'sideways':
            btVector3 turnVelocity = (btDot(sideways,dorsal) < 0.0 ? 1.0 : -1.0) * turn_rate * normal;
            std::cout<<turnVelocity.x()<<","<<turnVelocity.y()<<","<<turnVelocity.z()<<"\n";

            // It is NOT a good idea to completely slam the angular velocity
            // because this will override collision effects and will also prevent
            // the motorcycle from leaning over! (which is a prerequisite for "turn-by-leaning")
            //car->setAngularVelocity(angVel); // BAD!
            //
            // Instead we will "blend" the desired angular velocity onto the vehicle over time.
            // This blend must happen every substep, which means we probably should put this logic
            // in an custom "Action" (e.g. use btActionInterface), rather than applying it between
            // calls to world->stepSimulation().
            //
            // DANGER!: this blend technique is unstable when BLEND_TIMESCALE < SUBSTEP_DURATION!
            // For best results BLEND_TIMESCALE should be at least three SUBSTEP_DURATIONs
            // The easy way to 'tune' it is to set BLEND_TIMESCALE to be about 1/3 of the time
            // you want for the object to achieve 90% of its target angular velocity.
            // Hence in this example I've set it to be about 1/3 of a second, which means after
            // 1 second the vehicle's angular velocity will slave almost completely to the target.
            constexpr btScalar BLEND_TIMESCALE = 0.33;  // seconds
            constexpr btScalar BLEND_COEFFICIENT = SUBSTEP_DURATION / BLEND_TIMESCALE;

            // Blend the velocity by taking a weighted average between the real velocity and the target.
            btVector3 angularVelocity = car->getAngularVelocity();
            btVector3 newAngularVelocity = (1.0 - BLEND_COEFFICIENT) * angularVelocity + BLEND_COEFFICIENT * turnVelocity;
            car->setAngularVelocity(newAngularVelocity);

GeanGean
Posts: 7
Joined: Sat Feb 04, 2023 11:04 pm

Re: Getting and setting angular velocity in body space?

Post by GeanGean »

Wouldn't the cross of dorsal and up be very close to 0 if the vehicle is standing upright?
If it, as dxdf, is the denominator in the calculation for turn_rate wouldn't turn_rate be INF when the vehicle is standing upright? I'd think it should be near 0 then?

Also my question from the previous post still holds. A rotation of btVector3(0,x,0) is a rotation around the up-down axis, i.e. a change in yaw, not roll. Are you sure this is correct?
User avatar
drleviathan
Posts: 849
Joined: Tue Sep 30, 2014 6:03 pm
Location: San Francisco

Re: Getting and setting angular velocity in body space?

Post by drleviathan »

Yes, I believe I made an error in my derivation. Let me try again from scratch...

I tried again on paper and took a photo.

Fig1 shows the leaning motorcycle.

Fig2 shows the balance of forces which apply torque on the center of mass with focal point where the tire touches the road.

Fig3 shows the vector directions used in the equations (although I realized now I forgot to draw the up direction which points opposite gravity and is not necessarily the same as the road normal N, since the road might be banked or going up/downhill).

Fig4 shows some triangle geometry used to derive the fact that line segment AB equals line segment BC. Think of these segments as legs of a right triangle, and their values can be obtained by multiplying the hypotenuse with the dot-product (which provides sin(angle)) of the correct unit vectors that point parallel to the adjacent side.

In the end I get this equation:

w = ( g / v) ( D(x)F (.) u ) / ( D(x)F (.) F(x)N )

where:
w = angular velocity of turn
g = acceleration of gravity
v = forward speed
D = dorsal unit vector
F = forward unit vector
u = up unit vector (opposite gravity)
N = normal of road plane (not necessarily the same as u)

After all of that I realize I made a slight mistake for the most general case where the road normal is not parallel to gravity. The direction of the component of gravitational force that is relevant to the turn balance is not parallel to gravity, but is parallel to the road normal N. In other words: in the derivation you should replace u with u' where:

u' = (u (.) N) N

Note: u' is not a unit vector but includes a sin(angle) factor from the dot product: u(.)N

As to the direction of the angular velocity: We're talking about a changing yaw of the motorcycle as it drives in a circle, NOT the roll of the bike which provides the angle theta, used in the force-balance equation. In other words, the game player controls the roll (via a joystick, or button mashing) and the result is the bike yaws about a circle.

Edited: supplied correct link to google shared photo.
GeanGean
Posts: 7
Joined: Sat Feb 04, 2023 11:04 pm

Re: Getting and setting angular velocity in body space?

Post by GeanGean »

Before I take an in-depth look, it appears your link 404's for me. Maybe you tried to link to a private google drive?
As to the direction of the angular velocity: We're talking about a changing yaw of the motorcycle as it drives in a circle, NOT the roll of the bike which provides the angle theta, used in the force-balance equation. In other words, the game player controls the roll (via a joystick, or button mashing) and the result is the bike yaws about a circle.
I hadn't realized that was what you were trying to do until now.
Bear in mind, this is a raycast vehicle, so the steering is done through that interface. It's just that I don't want the bike to tip over.

Other than that I'll take a look at your picture when the link is fixed.

Thanks!
User avatar
drleviathan
Posts: 849
Joined: Tue Sep 30, 2014 6:03 pm
Location: San Francisco

Re: Getting and setting angular velocity in body space?

Post by drleviathan »

Sorry about that. I updated the link to what I hope is a publicly shared google photo.

Yes, I guess I misunderstood your problem. I still might not understand it but I'll take another swing (let me know if this is in the right direction).

If the vehicle is being steered directly... and you're trying to tilt it so that it remains balanced... I guess you want to do the inverse problem where you measure the yaw component of angular velocity (e.g. the angular speed of the turn) and then roll the vehicle such that it's dorsal direction makes an angle 'theta" relative to "up".

Drawing a sketch and using similar right triangles the scalar balance equation becomes:

(1) (m * V^2 / R) * cos(theta) = m * g * sin(theta)

where:
m = mass
g = magnitude of gravity
V = forward speed
R = radius of turn

Solving for theta gives:

(2) theta = atan( V^2 / (R * g) )

Meanwhile the forward velocity (V) is related to the radius of turn (R) and the angular velocity of the turn (w):

(3) V = w * R

Solving for R and substituting into (2) we get:
(4) theta = atan( V * w / g)

Knowing the correct tilt angle is only half the battle. The rest of the work is figuring out how to ease the vehicle into that orientation. My preferred method is to measure a delta_theta between the current tilt and the target. Then blend the current angular velocity toward a value that would exponentially reduce the delta.