Normalize rotation after applying constraint delta

Adding *body1.rotation.0 = *body1.rotation.0.normalize(); after rotational updates in position_constraints.rs fixes Jondolf#235. (Doesn't seem to be necessarily in angular_constraints.rs, but I think that's because it happens that the constraints are applied with positional_constraints running after in all existing joints which use both — so that should also be done just in case.)

I think rather than sticking the normalization after every rotation, it could be done by refactoring get_delta_rot() — this would mean less code duplication, but probably also it getting called a few more times than necessary. Maybe it's actually better to put it up into the solver step somewhere, so it only gets called once.

If joints end up getting unified into one 6dof universal joint, that might change this entirely. So, I'm going to submit a PR for quick-and-easy fix now, but am also filing this so we can later look at putting the normalization at the most optimal level.

src/constraints/angular_constraint.rs

@@ -67,9 +67,11 @@ pub trait AngularConstraint: XpbdConstraint<2> {
         // Apply rotational updates
         if body1.rb.is_dynamic() {
             *body1.rotation += Self::get_delta_rot(rot1, inv_inertia1, p);
+            *body1.rotation.0 = *body1.rotation.0.normalize();
         }
         if body2.rb.is_dynamic() {
             *body2.rotation -= Self::get_delta_rot(rot2, inv_inertia2, p);
+            *body2.rotation.0 = *body2.rotation.0.normalize();
         }
 
         p

src/constraints/position_constraint.rs

@@ -34,10 +34,21 @@ pub trait PositionConstraint: XpbdConstraint<2> {
         if body1.rb.is_dynamic() && body1.dominance() <= body2.dominance() {
             body1.accumulated_translation.0 += p * inv_mass1;
             *body1.rotation += Self::get_delta_rot(rot1, inv_inertia1, r1, p);
+            // This isn't the optimal place to do this, but it prevents
+            // explosion. See https://github.com/Jondolf/bevy_xpbd/issues/235
+            // Code is repeated below for body2 and also in angular_constraints
+            #[cfg(feature = "3d")]
+            {
+                *body1.rotation.0 = *body1.rotation.0.normalize();
+            }
         }
         if body2.rb.is_dynamic() && body2.dominance() <= body1.dominance() {
             body2.accumulated_translation.0 -= p * inv_mass2;
             *body2.rotation -= Self::get_delta_rot(rot2, inv_inertia2, r2, p);
+            #[cfg(feature = "3d")]
+            {
+                *body2.rotation.0 = *body2.rotation.0.normalize();
+            }
         }
 
         p