I've come across a section of CustomLimitBallAndSocket::SubmitConstrainst (2.0 beta 17) that initially puzzled me. However, after a bit of algebra I think it simplifies to something that is much more interpretable. Here is the original code:
- Code: Select all
if ((twistDirUp % twistDirUp) > 0.25f)
{
dFloat x;
dFloat y;
y = twistDirUp % matrix0.m_up; // scalar projection of twistDirUp onto matrix0.m_up
x = (matrix0.m_up * twistDirUp) % matrix0.m_front;
angle = dAtan2 (x, y);
}
and here is my simplification:
- Code: Select all
if ((twistDirUp % twistDirUp) > 0.25f)
{
dFloat x;
dFloat y;
y = matrix1.m_up % matrix0.m_up;
x = matrix1.m_up % matrix0.m_right;
angle = dAtan2 (x, y);
}
My algebraic justification is as follows (hopefully I've got it right):
y = twistDirUp % matrix0.m_up = (matrix1.m_up - matrix0.m_front.Scale (matrix1.m_up % matrix0.m_front)).matrix0.m_up = matrix1.m_up % matrix0.m_up
since matrix0.m_front is prependicular to matrix0.m_up
x = (matrix0.m_up * matrix1.m_up - matrix0.m_up * matrix0.m_front.Scale(...)) % matrix0.m_front
= (matrix0.m_up * matrix1.m_up) % matrix0.m_front , since matrix0.m_up * matrix0.m_front.Scale(...) is prependicular to matrix0.m_front
taking matrix1.m_up = a * matrix0.m_up + b * matrix0.m_right + c * matrix0.m_front gives:
matrix0.m_up * matrix1.m_up = b * matrix0.m_front - c * matrix0.m_right
so (matrix0.m_up * matrix1.m_up) % matrix0.m_front = b = matrix1.m_up % matrix0.m_right
so x = matrix1.m_up % matrix0.m_right
This gives an easier interpretation of x and y as the projection of matrix1.m_up onto the rotation plane with axis matrix0.m_front. And therefore the angle is the relative twist between the two bodies around matrix0.m_front.
I hope I've got all this right. If so: Julio, how did you arrive at the form in the original code? Does it have a useful methodology to its derivation that could be applied elsewhere?
Also, I think there is a mistake in the definition of dAtan2(x,y). It should be dFloat (atan2 (dFloat(y), dFloat(x))). In other words, the x and y should be swapped. Actually, for the above code I think it helps because by definition atan2 gives the angle from the x-axis so if there was no twist it would give pi/2. With the correct form of atan2 I think it would be better to replace matrix1.m_up with matrix1.m_right.
What are people's thoughts? Is my maths wrong?
Thanks, Joe