Rotating a joint relative to a parent's orientation

[Spankenstein]

When moving an object attached to a joint I also translate the joint so the anchor point remains relative to its parent object.

Code: Select all

            set
            {
                entity.Position = value;

                // If we are allowing object to be translated when it possesses a joint
                if (joint != null)
                {
                    Vector3 jP = joint.AngularJoint.ConnectionA.Position + joint.BallSocketJoint.OffsetA;
                    Vector3 freeAxis = joint.AngularJoint.WorldFreeAxisA;

                    game.Space.Remove(joint);

                    // Anchor must be translated if the entity is moved
                    // Free axis argument must be in world space
                    joint = new RevoluteJoint(entity, null, jP, freeAxis);

                    game.Space.Add(joint);
                }
            }

When the joint is attached to the body the difference in orientation is calculated.

Code: Select all

                qDiff = Quaternion.Conjugate(entity.Orientation);
                joint = value; 

I would then like to update to joint's orientation so it remains at the same relative orientation to its parent's orientation.

Code: Select all

            set
            {
                entity.Orientation = value;

                if (joint != null)
                {
                    Vector3 jP = joint.AngularJoint.ConnectionA.Position + joint.BallSocketJoint.OffsetA;
                    Vector3 freeAxis = joint.AngularJoint.WorldFreeAxisA;

                    game.Space.Remove(joint);

                    Quaternion newOrientation = entity.Orientation * qDiff;

                    // Anchor and free axis must be rotated if the entity is rotated
                    Matrix m = Matrix.CreateFromQuaternion(newOrientation);

                    joint = new RevoluteJoint(
                        entity,
                        null,
                        Vector3.Transform(jP, m),                         // Error
                        Vector3.TransformNormal(freeAxis, m)       // Error
                        );

                    game.Space.Add(joint);
                }
            }

The problem is that the joint will not rotate correctly and will instead spin rapidly.

I have tested the code above with a child entity. Its orientation will correctly rotate relative to its parent's orientation so the orientation values are correct.

However, when transforming the normal and anchor point by the same rotation matrix it will not work as intended. How can I apply the same method to the RevoluteJoint class?

[Norbo]

Rotating the world space anchor by the rotation matrix rotates the anchor around the origin of world space.

Presumably, you want to rotate around a different origin, perhaps the parent's center. Compute the offset from the desired origin to the current world anchor, transform it by the rotation, and add the rotated offset to the desired origin to get the new world space anchor.

[Spankenstein]

Compute the offset from the desired origin to the current world anchor, transform it by the rotation, and add the rotated offset to the desired origin to get the new world space anchor.

Following your instructions I came up with the following. The freeAxis and worldAnchor still spins around the entity rather than rotating correctly.

Code: Select all

                if (joint != null)
                {
                    Vector3 origin = entity.Position;
                    Vector3 worldAnchor = joint.AngularJoint.ConnectionA.Position + joint.BallSocketJoint.OffsetA;
                    Vector3 freeAxis = joint.AngularJoint.WorldFreeAxisA;

                    // Compute offset from desired origin of rotation to world space anchor
                    Vector3 orientationOffset = origin - worldAnchor;

                    Quaternion newOrientation = entity.Orientation * qDiff;
                    Matrix m = Matrix.CreateFromQuaternion(newOrientation);

                    // Transform the 'orientationOffset' by the new orientation
                    orientationOffset = Vector3.Transform(orientationOffset, m);

                    // Remove current joint from the physics space
                    game.Space.Remove(joint);

		    // Create new joint with the updated orientation
                    joint = new RevoluteJoint(
                        entity,
                        null,
                        orientationOffset + origin,       // Add rotated offset to the desired origin
                        Vector3.TransformNormal(freeAxis, m)
                        );

                    // Add the joint to the physics space
                    game.Space.Add(joint);
                }

Have I calculated something incorrectly?

[Norbo]

It appears the orientationOffset is negated because it is computed using origin - worldAnchor. That produces a vector pointing from the worldAnchor to the origin as opposed to a vector pointing from the origin to the world anchor.

[Spankenstein]

I've attached a simple demo (source code and executable) that shows the problem I am having.

Controls
Right Mouse Button - Allows control of the camera (WASD keys to move, move mouse to look)
Left Mouse Button - Use the mouse cursor to select any of the 3 rings. Drag the rings to rotate the object.

The yellow line represents the joint which does not orientate properly when rotating the object.
The PhysicsObject class contains the code for updating the joint's orientation.
The RotationWidget class orientates the PhysicsObject class via the RotateSelectedObjectWithWidget method.

Why does the joint not orientate itself correctly?

[Norbo]

qDiff is assigned only to Quaternion.Identity.

Because of this, newOrientation will always be entity.Orientation.

Every frame where the orientation is set, the joint's anchor and normal get rotated by entity.Orientation. As the entity is pulled farther and farther from the identity orientation, the joint spins faster and faster since the rotation is larger and larger.

Further, the referenced version of BEPUphysics in the demo is still the one that returns localAxisA for WorldFreeAxisA.

[Spankenstein]

Further, the referenced version of BEPUphysics in the demo is still the one that returns localAxisA for WorldFreeAxisA.

Sorry about this. It was done late at night before I went to bed. My local copy (large project) refers to the correct one and I have updated the problem demo to the new version for further issues.

How do I assign qDiff when I do not know the joint's orientation?

Code: Select all

qDiff = Quaternion.Conjugate(entity.Orientation) * ???

[Norbo]

The joint's full orientation is not needed (which is fortunate, because constraints do not have full orientations in general).

You could compute the difference in the entity's orientation before the set and after the set and apply that change to the joint anchor and axis. This would work, but would drift over time.

A better solution is to keep the anchor and axis of the joint in the local space of the entity. When the entity is given a new orientation, you compute the new joint state by transforming the anchor and axis into world space.

In the case where the entity being rotated is involved in the constraint, the constraint already has appropriate local space representations of both the anchor and axis. In fact, you don't even need to change the joint.BallSocketJoint.OffsetA or joint.AngularJoint.WorldFreeAxisA; they will update with the new entity orientation. You do, however, have to change the joint.BallSocketJoint.OffsetB and joint.AngularJoint.WorldFreeAxisB or their local versions because they are attached (in this case) to the world entity. If they weren't changed, the constraint would just try to pull the rotated entity back to where it was before.

Note that you can modify the existing joint's anchor and free axis without recreating it.

[Spankenstein]

A better solution is to keep the anchor and axis of the joint in the local space of the entity. When the entity is given a new orientation, you compute the new joint state by transforming the anchor and axis into world space.

Could you please post some code demonstrating this idea? I thought I had already tried this but it didn't work as intended.

[Norbo]

Could you please post some code demonstrating this idea?

You can see examples in the various world vs. local properties of constraints. It's just a matter of jumping from local space into world space and vice versa by applying the transform or inverse transform.

To bring a world space point into the local space of an object, transform the point by the inverse world transform of that object. To bring a point from the local space of an object to world space, transform the point by the world transform of the object.

To bring a world space direction into the local space of an object, transform the direction by the inverse of the orientation of that object (i.e. transpose of the orientation matrix or conjugate of the orientation quaternion). To bring a direction from the local space of an object to world space, transform the direction by the object's orientation.