Quaternion and axis of rotation - c++

Question

This question has been answered. Below is most of the code that you need to make it work! Hope it helps others.
Thanks to #Aki Suihkonen, #David Hammen, and #MBo.
Both Angle functions give the right answer.
I have three points:
A: 12 4 5
B: 6 8 -10
C: 5 6 7
I have implemented quaternions. I want to rotate point C so that Angle( A, B, C ) would be 40 degrees higher than before.
My question is: According to which axis do I have to rotate? I imagined that because A,B, and C create a plane I have to rotate point C according to the perpendicular axis to the vectors BA and BC. I obtained it with the CrossProduct of their unit vectors but when I try to get the Angle( A, B, C ) it does not give me the right result.
This is how I get the angle: (The old way)
results:
Angle of ABC before rotation = 67.3895.
Angle of ABC after rotation = 107.389.
float Angle( float x1, float y1, float z1,
float x2, float y2, float z2 )
{
float x, y, z;
CrossProduct( x1, y1, z1, x2, y2, z2, &x, &y, &z );
float result = atan2 ( L2Norm( x, y, z ),
DotProduct( x1, y1, z1, x2, y2, z2 ) );
return result;
}
Where the x1, y1, z1, x2, y2, z2 are the results from the unit vectors B-A and B-C.
Updated angle function:
results:
Angle of ABC before rotation = 67.3895.
Angle of ABC after rotation = 107.389.
Angle( Atom &atom1, Atom &atom2, Atom &atom3 )
{
float px1, py1, pz1;
float px2, py2, pz2;
UnitVector( atom1.X(), atom1.Y(), atom1.Z(),
atom2.X(), atom2.Y(), atom2.Z(),
&px1, &py1, &pz1 );
UnitVector( atom3.X(), atom3.Y(), atom3.Z(),
atom2.X(), atom2.Y(), atom2.Z(),
&px2, &py2, &pz2 );
float dot_product = DotProduct( px1, py1, pz1, px2, py2, pz2 );
float length_BA = sqrt( px1*px1 + py1*py1 + pz1*pz1 );
float length_BC = sqrt( px2*px2 + py2*py2 + pz2*pz2 );
return acos( dot_product / ( length_BA * length_BC ) );
}
float DotProduct( float x1, float y1, float z1,
float x2, float y2, float z2 )
{
return x1*x2 + y1*y2 + z1*z2;
}
void CrossProduct( float x1, float y1, float z1,
float x2, float y2, float z2,
float *ox, float *oy, float *oz )
{
*ox = (y1*z2) -(z1*y2);
*oy = -((x1*z2) -(z1*x2));
*oz = (x1*y2) -(y1*x2);
}
So my question is: According to which axis do I have to rotate point C so that Angle(A,B,C) would be 40 degrees larger than before?
// The 3 points.
Atom A( "A", -4, 2 , 8 );
Atom B( "B", -1, 3 , 4 );
Atom C( "C", -2, -4 , 5 );
float x1, y1, z1;
float x2, y2, z2;
float x, y, z;
// Get the cross product. Create the perpendicular vector to the BA and BC vectors.
PointVector( A.X(), A.Y(), A.Z(), B.X(), B.Y(), B.Z(), &x1, &y1, &z1 );
PointVector( C.X(), C.Y(), C.Z(), B.X(), B.Y(), B.Z(), &x2, &y2, &z2 );
CrossProduct( x1, y1, z1, x2, y2, z2, &x, &y, &z );
// Normalize the coordinates.
float length = sqrt( x*x + y*y + z*z );
length = 1.0 / length;
x *= length;
y *= length;
z *= length;
// Create the 40 degrees angle. It is supposed to increment the current ABC angle by 40 degrees.
float angle = 40*M_PI/180;
float sinAngleOver2 = sin(angle/2);
float w = cos(angle/2);
// Create the axis quat.
Quatd q(w, x * sinAngleOver2, y * sinAngleOver2, z * sinAngleOver2);
// Create the point quaternal. The angle of it equals to the current ABC angle.
angle = Angle( A, B, C ) *180/M_PI;
angle *= M_PI/180;
sinAngleOver2 = sin(angle/2);
w = cos(angle/2);
// Normalize the coordinates. The coordinates are the C point coordinates.
x = C.X() - B.X();
y = C.Y() - B.Y();
z = C.Z() - B.Z();
length = sqrt( x*x + y*y + z*z );
length = 1.0 / length;
x *= length;
y *= length;
z *= length;
Quatd qpt(w, x * sinAngleOver2, y * sinAngleOver2, z * sinAngleOver2);
// Rotate.
qpt = q*qpt*q.unitInverse();
Atom new_C;
new_C.X( qpt.x + B.X() );
new_C.Y( qpt.y + B.Y() );
new_C.Z( qpt.z + B.Z() );
cout << "Angle: " << Angle( A, B, new_C )*180/M_PI << '\n';

Answer 1

This code is incorrect:
// Normalize the coordinates. The coordinates are the C point coordinates.
length = sqrt( C.X()*C.X() + C.Y()*C.Y() + C.Z()*C.Z() );
length = 1.0 / length;
x = C.X() / length;
y = C.Y() / length;
z = C.Z() / length;
You've confused yourself by reusing your length variable to store 1 / length. You need to multiply the components of C by length to normalize your vector here.
I'm not familiar enough with quaternion maths to understand what you are trying to do at the end, but it does seem like you are rotating C around the origin. You want to rotate around B, which would mean subtracting B from C, performing your rotation around the origin, and then adding B onto the result to get back into your original space.
Personally, I would implement some kind of quaternion-vector multiplication, or convert the quaternion to a matrix in order to perform the transformation.