Rotating a RigidBody around a pivot point - c++

Question

I'm trying to rotate a rigidbody around a pivot point (in this case the origin), rather than its center of mass.
I had a suggestion to apply three transformations:
Transform the rigidbody to the origin
Rotate the rigidbody on its center of mass
Transform the rigidbody off of the origin.
Here is my code:
btMatrix3x3 orn = btPhys->getWorldTransform().getBasis();
btQuaternion quat;
orn.getRotation(quat);
btVector3 axis = quat.getAxis();
//Move rigidbody 2 units along its axis to the origin
btPhys->translate(btVector3(-2.0 * axis.getX(), 0.0, -2.0 * axis.getZ()));
//Rotate the rigidbody by 1 degree on its center of mass
orn *= btMatrix3x3(btQuaternion( btVector3(1, 0, 0), btScalar(degreesToRads(-1))));
btPhys->getWorldTransform().setBasis(orn);
//Update axis variable to apply transform on
orn.getRotation(quat);
axis = quat.getAxis();
//Move the rigidbody 2 units along new axis
btPhys->translate(btVector3(2.0 * axis.getX(), 0.0, 2.0 * axis.getZ()));
However, the pivot points appears to be moving around instead of staying in one place (the origin). Is there a better way (that actually works) to rotate a rigidbody around a pivot point?
EDIT:
I added some sanity-check code for the rotate function:
//Code that doesn't work
btVector3 invTrans = btPhys->offsetToPivot.rotate(btVector3(1.0, 0.0, 0.0), btScalar(degreesToRads(-1)));
//Values printed out are identical to offsetToPivot
printf("invTrans: %f %f %f\n", invTrans.getX(), invTrans.getY(), invTrans.getZ());
//Sanity code that DOES work
//Arbitrary vector
btVector3 temp = btVector3(0.0, 2.0, 0.0);
temp = temp.rotate(btVector3(1.0, 0.0, 0.0), btScalar(degreesToRads(-1)));
printf("temp %f %f %f\n", temp.getX(), temp.getY(), temp.getZ());

Answer 1

This method actually works, you're just applying it incorrectly. Your second translation is performed along world axis but you have rotated the object, so you have to translate it back along the rotated vector.
Correct code should look more or less like this:
btMatrix3x3 orn = btPhys->getWorldTransform().getBasis();
btQuaternion quat;
orn.getRotation(quat);
btVector3 axis = quat.getAxis();
//Move rigidbody 2 units along its axis to the origin
btPhys->translate(btVector3(-2.0 * axis.getX(), 0.0, -2.0 * axis.getZ()));
//Rotate the rigidbody by 1 degree on its center of mass
orn *= btMatrix3x3(btQuaternion( btVector3(1, 0, 0), btScalar(degreesToRads(-1))));
btPhys->getWorldTransform().setBasis(orn);
//Get rotation matrix
btTransform invRot(btQuaternion(btVector3(1, 0, 0), btScalar(degreesToRads(-1))),btVector3(0,0,0));
//Rotate your first translation vector with the matrix
btVector3 invTrans(-2.0 * axis.getX(), 0.0, -2.0 * axis.getZ());
invTrans = invRot * invTrans;
//Update axis variable to apply transform on
orn.getRotation(quat);
axis = quat.getAxis();
//Translate back by rotated vector
btPhys->translate(-invTrans);
I'm not sure if the rotation shouldn't be with minus (I can't check it right now) but you can easily try both.
EDIT.
Ok, so you forgot to mention that you perform a continuous rotation instead of a single one. This procedure is correct for a single rotation around pivot (eg. 30 degrees rotation). I've looked into your code once more and I understand that you try to perform your first translation along local x and z-axis. However it is not what happens. In this line:
btVector3 axis = quat.getAxis();
the variable axis is a unit vector representing the axis around which your object is rotated. It is NOT its coordinate system. I haven't noticed this part before. Quaternions are tricky and you should read more about them because many people missuse them.
A solution that will work in a continuous case is to store the last translation (from center of mass to pivot - in my example it is represented by invTrans) in your object and use it to perform the first translation, then rotate it in the same way it is done, and use it to move to the right position.
The corrected code will look like this:
btMatrix3x3 orn = btPhys->getWorldTransform().getBasis();
btQuaternion quat;
orn.getRotation(quat);
//Move rigidbody 2 units along its axis to the origin
btPhys->translate(btPhys->offsetToPivot);
//Rotate the rigidbody by 1 degree on its center of mass
orn *= btMatrix3x3(btQuaternion( btVector3(1, 0, 0), btScalar(degreesToRads(-1))));
btPhys->getWorldTransform().setBasis(orn);
//Get rotation matrix
btTransform invRot(btQuaternion(btVector3(1, 0, 0), btScalar(degreesToRads(-1))),btVector3(0,0,0));
//Rotate your first translation vector with the matrix
btVector3 invTrans = invRot * btPhys->offsetToPivot;
//Update axis variable to apply transform on
orn.getRotation(quat);
axis = quat.getAxis();
//Translate back by rotated vector
btPhys->translate(-invTrans);
btPhys->offsetToPivot = invTrans;
However before starting this whole procedure you have to set offsetToPivot into its position relative to the center of mass.
I have an impression that the main source of your problems is the lack of understanding of linear algebra and basic spatial transformations. If you are planning to continue in this field, I strongly recommend reading into this topic. Also drawing your problem on paper really helps.
EDIT2.
Ok, I've tried your code:
btVector3 temp = vec3(0,2,0);
btTransform invRot(btQuaternion(btVector3(1, 0, 0), btScalar(-0.017453f)),btVector3(0,0,0));
temp = invRot * temp;
After this, temp is equal to {0.000000000, 1.99969542, -0.0349042267}.

Answer 2

In the below function, these transformations perform the three steps you've described:
int x = cos(angRads) * (initial.x - axisOfRotation.x) - sin(angRads) * (initial.y - axisOfRotation.y) + axisOfRotation.x;
int y = sin(angRads) * (initial.x - axisOfRotation.x) + cos(angRads) * (initial.y - axisOfRotation.y) + axisOfRotation.y;
namely:
Step 1:Transform the rigidbody to the origin.
initial.x - axisOfRotation.x
initial.y - axisOfRotation.y
Step 2:Rotate the rigidbody on its center of mass.
cos(angRads) * initial.x - sin(angRads) * initial.y
sin(angRads) * initial.x + cos(angRads) * initial.y
Step 3:Transform the rigidbody off of the origin.
+axisOfRotation.x;
+axisOfRotation.y;
Here is a recursive function that performs exactly what you need and returns
all the consecutively rotated points in a vector: (use it as a benchmark)
rotateCoordinate(vector<Point>& rotated, Point& axisOfRotation, Point initial,
float angRads, int numberOfRotations){
// base case: when all rotations performed return vector holding the rotated points
if(numberOfRotations <= 0) return;
else{
// apply transformation on the initial point
int x = cos(angRads) * (initial.x - axisOfRotation.x) - sin(angRads) * (initial.y - axisOfRotation.y) + axisOfRotation.x;
int y = sin(angRads) * (initial.x - axisOfRotation.x) + cos(angRads) * (initial.y - axisOfRotation.y) + axisOfRotation.y;
// save the result
rotated.push_back(Point(x, y));
// call the same function this time on the rotated point and decremented number of rotations
rotateCoordinate(rotated, axisOfRotation, Point(x,y), angRads, numberOfRotations -1);
}
}
where Point is:
struct Point {
int x, y;
Point(int xx, int yy) : x(xx), y(yy) { }
Point() :x(0), y(0) { }
};
For further reading that explains the math behind it here.