Fixed add_angle

[hocop]

Objective

Fixes 415 and 416

Solution

Old implementation was only for small angles. When impact on RevoluteJoint is big, XPBD tries to apply a big update on angles and fails

Implemented add_angle function using precise computation of sin and cos

[hocop]

Although I get that the linearized implementation of add_angle makes more sense for XBPD updates, as they are linear.

Maybe then add another function add_angle_linearized and remove assert from it?

[Khanoto]

Shouldn't we use these to add angle ?

I think something like that may do it

[Jondolf]

The existing implementation was based on Solver2D's s2IntegrateRot.

We should not compute it by computing sin and cos like that, as it is more expensive, and sin and cos also produce varying results on different architectures, so this would need to use libm for cross-platform determinism when enhanced-determinism is enabled, which is also more expensive.

We should just do the normalization in an optimized way before using from_sin_cos, similarly to what Box2D V3 does.

/// Adds the given counterclockiwise angle in radians to the [`Rotation`].
#[inline]
#[must_use]
pub fn add_angle(&self, radians: Scalar) -> Self {
    let (sin, cos) = (self.sin + radians * self.cos, self.cos - radians * self.sin);
    let magnitude_squared = sin * sin + cos * cos;
    let magnitude_recip = if magnitude_squared > 0.0 {
        magnitude_squared.sqrt().recip()
    } else {
        0.0
    };
    Rotation::from_sin_cos(sin * magnitude_recip, cos * magnitude_recip)
}

[hocop]

Thanks! I changed to your implementation and checked that it works

[hocop]

@Khanoto I think it is identical to what I initially proposed, just more verbose

[Jondolf]

Thanks!