void btGeneric6DofConstraint::calculateAngleInfo()
{
btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
// in euler angle mode we do not actually constrain the angular velocity
// along the axes axis[0] and axis[2] (although we do use axis[1]) :
//
// to get constrain w2-w1 along ...not
// ------ --------------------- ------
// d(angle[0])/dt = 0 ax[1] x ax[2] ax[0]
// d(angle[1])/dt = 0 ax[1]
// d(angle[2])/dt = 0 ax[0] x ax[1] ax[2]
//
// constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
// to prove the result for angle[0], write the expression for angle[0] from
// GetInfo1 then take the derivative. to prove this for angle[2] it is
// easier to take the euler rate expression for d(angle[2])/dt with respect
// to the components of w and set that to 0.
btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
m_calculatedAxis[1] = axis2.cross(axis0);
m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
m_calculatedAxis[0].normalize();
m_calculatedAxis[1].normalize();
m_calculatedAxis[2].normalize();
}
void Generic6DOFJointSW::calculateAngleInfo()
{
Matrix3 relative_frame = m_calculatedTransformA.basis.inverse()*m_calculatedTransformB.basis;
matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
// in euler angle mode we do not actually constrain the angular velocity
// along the axes axis[0] and axis[2] (although we do use axis[1]) :
//
// to get constrain w2-w1 along ...not
// ------ --------------------- ------
// d(angle[0])/dt = 0 ax[1] x ax[2] ax[0]
// d(angle[1])/dt = 0 ax[1]
// d(angle[2])/dt = 0 ax[0] x ax[1] ax[2]
//
// constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
// to prove the result for angle[0], write the expression for angle[0] from
// GetInfo1 then take the derivative. to prove this for angle[2] it is
// easier to take the euler rate expression for d(angle[2])/dt with respect
// to the components of w and set that to 0.
Vector3 axis0 = m_calculatedTransformB.basis.get_axis(0);
Vector3 axis2 = m_calculatedTransformA.basis.get_axis(2);
m_calculatedAxis[1] = axis2.cross(axis0);
m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
// if(m_debugDrawer)
// {
//
// char buff[300];
// sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ",
// m_calculatedAxisAngleDiff[0],
// m_calculatedAxisAngleDiff[1],
// m_calculatedAxisAngleDiff[2]);
// m_debugDrawer->reportErrorWarning(buff);
// }
}
void btGeneric6DofSpring2Constraint::calculateAngleInfo()
{
btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
switch (m_rotateOrder)
{
case RO_XYZ : matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff); break;
case RO_XZY : matrixToEulerXZY(relative_frame,m_calculatedAxisAngleDiff); break;
case RO_YXZ : matrixToEulerYXZ(relative_frame,m_calculatedAxisAngleDiff); break;
case RO_YZX : matrixToEulerYZX(relative_frame,m_calculatedAxisAngleDiff); break;
case RO_ZXY : matrixToEulerZXY(relative_frame,m_calculatedAxisAngleDiff); break;
case RO_ZYX : matrixToEulerZYX(relative_frame,m_calculatedAxisAngleDiff); break;
default : btAssert(false);
}
// in euler angle mode we do not actually constrain the angular velocity
// along the axes axis[0] and axis[2] (although we do use axis[1]) :
//
// to get constrain w2-w1 along ...not
// ------ --------------------- ------
// d(angle[0])/dt = 0 ax[1] x ax[2] ax[0]
// d(angle[1])/dt = 0 ax[1]
// d(angle[2])/dt = 0 ax[0] x ax[1] ax[2]
//
// constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
// to prove the result for angle[0], write the expression for angle[0] from
// GetInfo1 then take the derivative. to prove this for angle[2] it is
// easier to take the euler rate expression for d(angle[2])/dt with respect
// to the components of w and set that to 0.
switch (m_rotateOrder)
{
case RO_XYZ :
{
//Is this the "line of nodes" calculation choosing planes YZ (B coordinate system) and xy (A coordinate system)? (http://en.wikipedia.org/wiki/Euler_angles)
//The two planes are non-homologous, so this is a Tait–Bryan angle formalism and not a proper Euler
//Extrinsic rotations are equal to the reversed order intrinsic rotations so the above xyz extrinsic rotations (axes are fixed) are the same as the zy'x" intrinsic rotations (axes are refreshed after each rotation)
//that is why xy and YZ planes are chosen (this will describe a zy'x" intrinsic rotation) (see the figure on the left at http://en.wikipedia.org/wiki/Euler_angles under Tait–Bryan angles)
// x' = Nperp = N.cross(axis2)
// y' = N = axis2.cross(axis0)
// z' = z
//
// x" = X
// y" = y'
// z" = ??
//in other words:
//first rotate around z
//second rotate around y'= z.cross(X)
//third rotate around x" = X
//Original XYZ extrinsic rotation order.
//Planes: xy and YZ normals: z, X. Plane intersection (N) is z.cross(X)
btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
m_calculatedAxis[1] = axis2.cross(axis0);
m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
break;
}
case RO_XZY :
{
//planes: xz,ZY normals: y, X
//first rotate around y
//second rotate around z'= y.cross(X)
//third rotate around x" = X
btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
btVector3 axis1 = m_calculatedTransformA.getBasis().getColumn(1);
m_calculatedAxis[2] = axis0.cross(axis1);
m_calculatedAxis[0] = axis1.cross(m_calculatedAxis[2]);
m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0);
break;
}
case RO_YXZ :
{
//planes: yx,XZ normals: z, Y
//first rotate around z
//second rotate around x'= z.cross(Y)
//third rotate around y" = Y
btVector3 axis1 = m_calculatedTransformB.getBasis().getColumn(1);
btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
m_calculatedAxis[0] = axis1.cross(axis2);
m_calculatedAxis[1] = axis2.cross(m_calculatedAxis[0]);
m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1);
break;
}
case RO_YZX :
{
//planes: yz,ZX normals: x, Y
//first rotate around x
//second rotate around z'= x.cross(Y)
//third rotate around y" = Y
btVector3 axis0 = m_calculatedTransformA.getBasis().getColumn(0);
btVector3 axis1 = m_calculatedTransformB.getBasis().getColumn(1);
m_calculatedAxis[2] = axis0.cross(axis1);
m_calculatedAxis[0] = axis1.cross(m_calculatedAxis[2]);
m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0);
break;
}
case RO_ZXY :
{
//planes: zx,XY normals: y, Z
//first rotate around y
//second rotate around x'= y.cross(Z)
//third rotate around z" = Z
btVector3 axis1 = m_calculatedTransformA.getBasis().getColumn(1);
btVector3 axis2 = m_calculatedTransformB.getBasis().getColumn(2);
m_calculatedAxis[0] = axis1.cross(axis2);
m_calculatedAxis[1] = axis2.cross(m_calculatedAxis[0]);
m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1);
break;
}
case RO_ZYX :
{
//planes: zy,YX normals: x, Z
//first rotate around x
//second rotate around y' = x.cross(Z)
//third rotate around z" = Z
btVector3 axis0 = m_calculatedTransformA.getBasis().getColumn(0);
btVector3 axis2 = m_calculatedTransformB.getBasis().getColumn(2);
m_calculatedAxis[1] = axis2.cross(axis0);
m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
break;
}
default:
btAssert(false);
}
m_calculatedAxis[0].normalize();
m_calculatedAxis[1].normalize();
m_calculatedAxis[2].normalize();
}
