Angle hiccups when rotation wraps +-PI, and other rotational musings.

[ninnghazad]

Hey @louis-langholtz ,
i have a problem with spurious hiccups in rotation when the angle goes from/to +PI / -PI.
Wrote a GravityJoint, which pulls bodies together. Now i want to rotate the second body to be upright wrt the direction of gravity towards the first body.
In a test a small and a big disk are connected using this joint, and the small disk slides around the surface of the big disk. (see code below)
This seemingly works, however on some rotations, when the small disk passes the point where it's angle flips from +PI to -PI the body has wrong angle values for a frame or two.
The location is not affected, only the rotation. This does not happen every time, but very often.

Console output, see how when the angle wraps, even if the joint sets 3.1258, after Step() the body has a rotation of -1.5201 instead of 3.1258 for a single step, and then continues with the correct values set by the joint in the next step:

Main loop: angle: -3.1135 location: 0.0631x-10.2480
Inside GravityJoint: angle: -3.1354  impulse: -0.0221x3.5819 slop: 0.0349 err: 0.0219  body angle: -3.1354
Main loop: angle: -3.1354 location: -0.1615x-10.2494
Inside GravityJoint: angle: 3.1258  impulse: 0.0564x3.5798 slop: 0.0349 err: 6.2613  body angle: 3.1258
Inside GravityJoint: angle: 3.1258  impulse: 0.0564x3.5798 slop: 0.0349 err: 0.0000  body angle: 3.1258
Main loop: angle: -1.5201 location: -0.3861x-10.2385
Inside GravityJoint: angle: 3.1039  impulse: 0.1351x3.5812 slop: 0.0349 err: 4.6240  body angle: 3.1039
Inside GravityJoint: angle: 3.1039  impulse: 0.1351x3.5812 slop: 0.0349 err: 0.0000  body angle: 3.1039
Main loop: angle: 3.1039 location: -0.6108x-10.2300

A test using said joint:

//g++ -std=c++17 -O3 -march=native -Isubmodules/PlayRho ideas_and_tests/cpp_playrho_gravity.cpp -o /tmp/test libs/libPlayRho.a

#include <PlayRho/PlayRho.hpp>
#include <iostream>
#include <iomanip>

using namespace playrho;
using namespace playrho::d2;

int main()
{
	auto world = World{};
	std::cout << std::fixed << std::setprecision(4);

	const auto planet = world.CreateBody(BodyConf{}
		.UseLocation(Length2{0, 0})
		.UseType(BodyType::Static));

	planet->CreateFixture(Shape{DiskShapeConf{}
		.UseRadius(1_m*10)
		.UseFriction(10.0)
		.UseRestitution(0)
		.UseDensity(0.0)
	});

	auto location = Length2{1_m*10+.25,0};
	auto body = world.CreateBody(BodyConf{}
		.UseLocation(location)
		.UseType(BodyType::Dynamic));
	body->CreateFixture(Shape{DiskShapeConf{}
		.UseLocation(Length2{0,0})
		.UseRadius(1_m*0.25)
		.UseFriction(0.0)
		.UseRestitution(0)
		.UseDensity(1.0)
	});
	body->SetFixedRotation(true);

	GravityJointConf def{};
	def.bodyA = planet;
	def.bodyB = body;
	def.factor = 2000;
	def.radius = 50;
	auto joint = world.CreateJoint(def);

	auto dt = playrho::Second / 50.0;
	StepConf stepConfWarm;
	stepConfWarm.SetTime(dt);
	stepConfWarm.doWarmStart = true;


	ApplyLinearImpulse(*body,Length2{0,-2},body->GetWorldCenter());

	bool flip{false};
	std::size_t i{0},abrt{0};
	while(++i < 100000) {
		world.Step(stepConfWarm);
		std::cout << "Main loop: angle: " << body->GetAngle() << " location: " << body->GetLocation()[0] << "x" << body->GetLocation()[1] << std::endl;

		// Abort 4 frames after error detected
		if(!abrt) {
			if(flip && std::abs(body->GetAngle()) < 2.7) ++abrt;
			else if(std::abs(body->GetAngle()) > 3) flip = true;
			else flip = false;
		} else {
			if(++abrt > 4) return 1;
		}
	}
	return 0;
}

Setting rotation in joint solver:

bool GravityJoint::SolvePositionConstraints(BodyConstraintsMap& bodies, const ConstraintSolverConf& conf) const
{
	auto& bodyConstraintB = At(bodies, GetBodyB());
	auto posB = bodyConstraintB->GetPosition();

	// get direction of gravitational force from impulse
	const auto targetU = UnitVec::Get(m_impulse[0],m_impulse[1]).first;

	const auto targetAngle = GetAngle(targetU.Rotate(UnitVec::GetTop()));
	const auto err = abs(posB.angular - targetAngle);

	posB.angular = targetAngle;
	bodyConstraintB->SetPosition(posB);

	std::cout << "Inside GravityJoint: angle: " << targetAngle << " "
	<< " impulse: " << m_impulse[0] << "x" << m_impulse[1]
	<< " slop: " << conf.angularSlop << " err: " << err << " "
	<< " body angle: " << bodyConstraintB->GetPosition().angular
	<< std::endl;
	return err < conf.angularSlop;
}

Do you have any idea what i am doing wrong here?
This has me clueless.

The full source for the GravityJoint can be found here https://github.com/ninnghazad/PlayRho/tree/gravity_joint .

[ninnghazad]

I think i found the problem. Some serious gdb foo brought me to GetPosition, the one that interpolates Positions. Take a look at this:

#include <PlayRho/PlayRho.hpp>
#include <iostream>
#include <iomanip>

using namespace playrho;
using namespace playrho::d2;

int main()
{
        std::cout << std::fixed << std::setprecision(4);

        Position p0{Length2{-0.1615,-10.2494},-3.1354};
        Position p1{Length2{-0.3850,-10.1851},3.1258};
        auto p = GetPosition(p0,p1,0.2580);

        std::cout << p0.linear[0] << "x" << p0.linear[1] << ":" << p0.angular << std::endl;
        std::cout << p1.linear[0] << "x" << p1.linear[1] << ":" << p1.angular << std::endl;
        std::cout << p.linear[0] << "x" << p.linear[1] << ":" << p.angular << std::endl;

        return 0;
}

Output:

-0.1615x-10.2494:-3.1354
-0.3850x-10.1851:3.1258
-0.2192x-10.2328:-1.5200

See that -1.52 in there? That can't be right.

[louis-langholtz]

Angular encoding is complicated by decreasing precision with increasingly large values due to the way IEEE floating point arithmetic works. I've previously at least with one issue & related effort tried unsuccessfully to address that. Something to factor into this bug you've identified.

Ideally we need a way to wrap-around angles so that:

1. Using a float for them can't result in arbitrarily large values for say a continuously rotating body.
2. The code can correctly recognize which direction the body turned in.

[ninnghazad]

I think the PR #332 referenced above should satisfy those two conditions.

It does however limit possible rotation to +/- PI - meaning one cannot use the angle to determine the number of full rotations something has made. Which would kind of clash with 1. in any way.

The code compiles down quite well, and is a lot faster than the other method (sin/cos unit-vector thingy) i could think of.

I am not sure if it compiles using Fixed reals.

Another minor issue would be that commonly radians use a range of [-PI,PI) (or was it (-PI,PI]? nvm) but this will clip to [-PI,PI].
Haven't had any issues with that so far - the only one i can think of would be certain comparisons where (-PI == PI) should be true but ofc isn't.
An if would take care of that, but i tried to keep branches low.
Edit: hm, and the / twoPi should be turned into a * rTwoPi with a const 1/(2*PI). Just saw that g++9 does not do that automatically, there was still a divss instruction in there.

[louis-langholtz]

Just saw that g++9 does not do that automatically, there was still a divss instruction in there.

Oh wow!

I hadn't realized divss would get used instead of mulss even with maximum standards-compliant optimizations turned on. I recall having switched a bunch of places in the code base to using division to more directly express the intent of division under the assumption that the compiler would always convert these to using multiplication by the reciprocals.

Looking at things more closely on Godbolt reveals that it's divisions by floating point values that can't exactly be expressed reciprocally which are done via the divss instruction unless the -freciprocal-math optimization argument is added. Gcc, clang, and msvc all appear to convert any divisions by values that are exactly expressed reciprocally with mulss however at least in versions 9.2, 8.0.0, and 19.22.

Good news is this means there's places in the code that can be rewritten to be faster then. Bad news is that I don't recall all the places in the code where this will be.

[louis-langholtz]

I think i found the problem. Some serious gdb foo brought me to GetPosition, the one that interpolates Positions. Take a look at this:

#include <PlayRho/PlayRho.hpp>
#include <iostream>
#include <iomanip>

using namespace playrho;
using namespace playrho::d2;

int main()
{
        std::cout << std::fixed << std::setprecision(4);

        Position p0{Length2{-0.1615,-10.2494},-3.1354};
        Position p1{Length2{-0.3850,-10.1851},3.1258};
        auto p = GetPosition(p0,p1,0.2580);

        std::cout << p0.linear[0] << "x" << p0.linear[1] << ":" << p0.angular << std::endl;
        std::cout << p1.linear[0] << "x" << p1.linear[1] << ":" << p1.angular << std::endl;
        std::cout << p.linear[0] << "x" << p.linear[1] << ":" << p.angular << std::endl;

        return 0;
}

Output:

-0.1615x-10.2494:-3.1354
-0.3850x-10.1851:3.1258
-0.2192x-10.2328:-1.5200

See that -1.52 in there? That can't be right.

How is -1.52 wrong? That is 0.2580 of the way from -3.1354 to 3.1258 with precision of 4: -3.1354 + (3.1258 - -3.1354) * 0.2580 = -1.52001040.

I do believe that you found a problem though. I don't think that the problem is where PR #332 is currently changing however; it's not in that GetPosition I don't believe.

I recall there being some code which sometimes normalizes angles though not as regularly as doing what you've shown in this PR. I suspect that's where the bug, that's causing angle hiccups, really is -- though allowing angles to grow arbitrarily large as the current GetPosition code does is IMO a bug too as I'd mentioned and I do like the way this PR regularly normalizes the angle.

[ninnghazad]

How is -1.52 wrong? That is 0.2580 of the way from -3.1354 to 3.1258 with precision of 4: -3.1354 + (3.1258 - -3.1354) * 0.2580 = -1.52001040.

Ofc, however only if one allows angle interpolation to take the "long way" between angles. Interpolating over the shorter angle should give -3.141 or something like that.

Allowing to interpolate over the long angle would mean that like in my example a rotating body would jump from rotation -3.1 on frame 0 to rotation -1.5 on frame 1 and to rotation 3.1 on frame 2.
Well i didn't mention to imagine a rotating body while reading that snippet.

With that in mind, another example might be that with the code as is, a pendulum swinging from -3 to 3 over +-PI, imagining +-PI as lowest point, might suddenly point upwards for a frame, just to return to its hanging position on the next.

I noticed that because my sprites would sometimes "flip" for a very brief moment while rolling around, as the sprites are rendered using a body's rotation.

[ninnghazad]

Thinking more about the pendulum example, i realized that my solution might not even suffice. The way i wrote it, interpolation would always be along the shorter of the two possible distances between two angles. But while it might in reality not happen, there is the theoretical edge case that interpolation would have to happen along the long side. Should there be a pendulum swinging from -3 to 3 over 0, 0 being the lowest point - swinging so fast that getPosition would be called with the pendulums extreme angles - the pendulum would flip. Say when interpolating -3 to 3 with r=.5, would give +-PI the way i wrote it, but depending on the direction of rotation, would have to be 0.
I wouldn't have noticed that happening, because my sprites would have to spin at about 3000rpm for that to happen - at which point it isn't possible to notice with my weak human eyes.
Nevertheless, i guess the direction of rotation would have to be accounted for when interpolating the angles - so as to choose either the short or the long way around the circle.

[louis-langholtz]

@ninnghazad What are your thoughts about the GetNormalized(Angle value) function in PlayRho/Common/Math.hpp and the code that uses it? I ask because I suspect it has something to do with the hiccups.

[ninnghazad]

@ninnghazad What are your thoughts about the GetNormalized(Angle value) function in PlayRho/Common/Math.hpp and the code that uses it? I ask because I suspect it has something to do with the hiccups.

Hm, it seems ok. Maybe not fastest, but solid. Did a quick bruteforce-ish test on it and compared it to the method i use. Both passed the same tests and returned reasonably similar results. (Used the core of the trunc-mod version).
Only thing confusing to me would be the / Radian, * Radian - isn't playrho using radians anyway, and isn't the method supposed to be called with an angle in radians? Is that an actual conversion from/to radians or some esoteric typing thing? Degrees looks like it could be used to convert between rad and deg.

Apart from that the only way i could see the method not doing what one would expect would be to feed it degrees or something weird.

I compared it to:

constexpr float twoPi{M_PI*2.0};
constexpr float rTwoPi{1.0/(M_PI*2.0)};
float normalize2(float r) {
        return (r - twoPi * floor((r + M_PI) * rTwoPi));
}

Oh, and i think we might be talking about multiple issues about angles by now. I'm pretty sure that the way angles get interpolated is wrong, then there is the giant-angles thing, and maybe stuff that went right past me in the discussion. Hm, and none of that has anything to do anymore with custom joints. I'ma rename this issue a bit.

[ninnghazad]

[snip blablabla] Nevertheless, i guess the direction of rotation would have to be accounted for when interpolating the angles - so as to choose either the short or the long way around the circle.

Thinking more about it i remembered there being a maximum rotation per step limit. Finding a few sentences about it in the docs there was this:

If this value is less than half a turn (less than Pi), then the turning direction will be the direction of the smaller change in angular orientation.

And the default seems to be .5 Pi. So my fears of ambiguous angles in sweep interpolation are unfounded - the limit would make it so interpolation along the short side is always ok.

So the PR is ok for preventing the 'close-to-PI-flip' cause of the interpolation, but it does not limit bodies angles in general it seems. Did a quick spinning body test against the PR and i get large angles from body->GetAngle() and normalized angles from GetAngle(body->GetTransformation().q) (as expected cause .q is a unitvec).

[louis-langholtz]

Experimenting with a call to GetNormalized in WorldImpl.cpp, all seems well with the exceptions of MotorJointConf and GearJointConf. More specifically, I'm experimenting with changing line 1125 of WorldImpl.cpp:

if (UpdateBody(body, bc.GetPosition())) {

to:

if (UpdateBody(body, GetNormalized(bc.GetPosition()))) {

I think I prefer normalization here, since it seems like point in the code where GetNormalized would get called the least while still normalizing any moving body. There's also a call to UpdateBody in the TOI phase processing at line1613, but I'm thinking that would only result in a limited overflow that eventually would get normalized again in the regular phase processing.

Changing the setting of object.angularError in MotorJointConf to object.angularError = ::playrho::GetNormalized((posB.angular - posA.angular) - object.angularOffset); seems to resolve the issue for the motor joint.

[louis-langholtz]

Only thing confusing to me would be the / Radian, * Radian - isn't playrho using radians anyway, and isn't the method supposed to be called with an angle in radians? Is that an actual conversion from/to radians or some esoteric typing thing?

Code like / Radian, * Radian, or equivalently / 1_rad and * 1_rad, is conceptually either creating dimensionless values (scalars) in the division cases or is creating dimensional values (angles) in the multiplication cases. Unless the macro USE_BOOST_UNITS is defined however (see line 46 of Units.hpp), this is really just syntactic sugar of sorts.

It's my desire to enable the use of strongly typed quantities by default (like for angles, lengths, etc.) but so far in testing with boost units, it hasn't been "zero overhead", performance drops significantly, and so I've made it a compile time option instead. See The Physical Units Interface document for more information about enabling strongly typed quantities/units.

[louis-langholtz]

Do angle hiccups also occur in any code that's currently available in the PlayRho library?

I ask because I've only seen angle hiccups when I've tried to add angle normalization to the library code. My interest in angle normalization is particularly to do with the body sweep angles and keeping them from otherwise being able to grow unbounded from say something just under constant rotation in a single direction.

This seems akin to the linear problem ShiftOrigin(World& world, Length2 newOrigin) is meant to address. Perhaps it's also best addressed similarly, by having users explicitly normalize sweep angles only if or when there application accumulates too much rotation in a single direction.

As for what might be going wrong in the GravityJoint::SolvePositionConstraints code, I'm leaning on the subtraction of posB.angular - targetAngle. targetAngle is a naturally normalized angle (due to UnitVec), while posB.angular is a sweep angle (that's not normalized naturally). They're not directly comparable. Additionally, I don't think the subtraction relates properly to the angular positional error of a gravitationally orbiting body.