Anyway, I think I've got a joint that I like. Its like the CustomLimitBallAndSocket but has two independent limit angles forming a pyramid, as well as twist limits.
Julio, I think that the code for CustomLimitBallAndSocket produced an incorrect twist angle because the projection was onto a slanting frame (sometimes). I may be wrong but I think I'm right. In my code, I construct a new twist reference frame from the parent pin by rotating it so that the parent and child pin m_fronts align. This is done without rotating around m_front so a suitable base reference frame is formed in order to determine the twist angle. Hopefully my code makes sense and is correct.
I've used some non-standard things in my code but I hope its pretty readable. I went down a complete dead end with euler angles which I don't think are at all helpful for this kind of thing. However, its a good learning exercise.
- Code: Select all
void CustomLimitBallAndSocket::SubmitConstrainst (dFloat timestep, int threadIndex)
{
// get global version of the pin and pivot from the perspective of each body
dMatrix global_pin_pivot0=mPinPivot0;
mpParentBody->ConvertLocalToGlobalMatrix(global_pin_pivot0);
dMatrix global_pin_pivot1=mPinPivot1;
mpChildBody->ConvertLocalToGlobalMatrix(global_pin_pivot1);
// end get global version of the pin and pivot from the perspective of each body
// first add linear contraints to fix the pivot point relative to both bodies
NewtonUserJointAddLinearRow(mpNewtonJoint,&global_pin_pivot0.m_posit[0],&global_pin_pivot1.m_posit[0],&global_pin_pivot0.m_front()[0]);
NewtonUserJointAddLinearRow(mpNewtonJoint,&global_pin_pivot0.m_posit[0],&global_pin_pivot1.m_posit[0],&global_pin_pivot0.m_up()[0]);
NewtonUserJointAddLinearRow(mpNewtonJoint,&global_pin_pivot0.m_posit[0],&global_pin_pivot1.m_posit[0],&global_pin_pivot0.m_right()[0]);
// end first add linear contraints
// 'twist' is rotation around m_front on the child body.
// to do this we need to project onto to orthogonal axes.
// However, we can't use the two on the child body because they
// rotate with the child. Also, we can't use the two of the parents because rotations
// around the other two axes put this reference plane at an angle. This then makes
// the determination of the angle incorrect.
// The solution here is to take a reference frame that starts as the parents pin frame.
// This is then rotated so that the m_front vector points in the same direction as for the
// child. However, the rotation does not rotate around m_front and so provides a solid reference
// frame.
// In order to perform this rotation. First, we get a pin frame for the child that is relative
// to the parent's pin frame. This allows a frame of reference that is relative to the parent's
// pin (and so the parent's m_front is {1,0,0} ).
// So, we have to m_front's - one for the parent at {1,0,0} and one for the child at {a,b,c}.
// An axis of rotation is found by taking the cross product. Then a rotation matrix is found to
// rotate around this axis. Below this is shown as one matrix (twist_reference_frame). I worked
// this out using mathematica and http://en.wikipedia.org/wiki/Rotation_matrix#Rotation_about_an_arbitrary_vector
// A couple of signs needed changing - I'm not sure why but who cares.
// Then this matrix is multiplied with the parent's pin frame.
// This gives a reference frame that has m_front pointing in the same direction as for the child.
// But the other two axes are in a position that hasn't been rotated around m_front.
// Therefore this set of axes can be used to project onto in order to determine the twist angle
dMatrix rotation_0,rotation_1,inverse_rotation_0,relative_frame;
rotation_0=global_pin_pivot0;
rotation_0.m_posit=dVector(0.0,0.0,0.0);
rotation_1=global_pin_pivot1;
rotation_1.m_posit=dVector(0.0,0.0,0.0);
inverse_rotation_0=rotation_0.Inverse();
relative_frame=rotation_1*inverse_rotation_0;
// get rotated reference frame that doesn't rotate around the twist axis (axis 0)
dMatrix twist_reference_frame(GetIdentityMatrix());
dFloat a,b,c;
a=relative_frame.m_front().m_x;
b=relative_frame.m_front().m_y;
c=relative_frame.m_front().m_z;
if (fabs(b)>0.001 || fabs(c)>0.001)
{
dFloat axis0_angle=atan2(sqrt(b*b+c*c),a);
twist_reference_frame.m_front()=dVector(cos(axis0_angle),b*sin(axis0_angle)/sqrt(b*b+c*c),c*sin(axis0_angle)/sqrt(b*b+c*c),0.0);
twist_reference_frame.m_up()=dVector(-b*sin(axis0_angle)/sqrt(b*b+c*c),(c*c+b*b*cos(axis0_angle))/(b*b+c*c),(b*c*(-1+cos(axis0_angle)))/(b*b+c*c),0.0);
twist_reference_frame.m_right()=dVector(-c*sin(axis0_angle)/sqrt(b*b+c*c),(b*c*(-1+cos(axis0_angle)))/(b*b+c*c),(b*b+c*c*cos(axis0_angle))/(b*b+c*c),0.0);
}
twist_reference_frame=twist_reference_frame*rotation_0;
// end get rotated reference frame that doesn't rotate around the twist axis (axis 0)
double x,y,angle,constrained_angle_1,constrained_angle_2,min_angle,max_angle;
// twist around axis 0
y = global_pin_pivot1.m_up() % twist_reference_frame.m_right();
x = global_pin_pivot1.m_up() % twist_reference_frame.m_up();
angle = atan2(y,x);
min_angle=mMinAngle.X*PI/180.0f;
max_angle=mMaxAngle.X*PI/180.0f;
if (angle > max_angle + 0.001f)
{
NewtonUserJointAddAngularRow (mpNewtonJoint, angle - max_angle, &global_pin_pivot1.m_front()[0]);
}
else if (angle < min_angle - 0.001f)
{
NewtonUserJointAddAngularRow (mpNewtonJoint, angle - min_angle, &global_pin_pivot1.m_front()[0]);
}
// end twist around axis 0
// As well as contraining the twist, we want to constrain rotations around the other two angles to fall
// inside a pyramid. This is done by projecting the m_front vector of the child onto two pairs of axes
// of the parent (as shown below).
// If outside the pyramid, the limited position is calculated and constrained to
// Limits are ok if comfortably in [-pi/2, pi/2] but I think the twist angle gets messed up if outside
// this range.
bool constrain_pyramid=false;
// around axis 1 (pyramid)
y = global_pin_pivot1.m_front() % global_pin_pivot0.m_right();
x = global_pin_pivot1.m_front() % global_pin_pivot0.m_front();
constrained_angle_1 = angle = atan2(y,x);
min_angle=mMinAngle.Y*PI/180.0f;
max_angle=mMaxAngle.Y*PI/180.0f;
if (angle > max_angle + 0.001f)
{
constrained_angle_1=max_angle;
constrain_pyramid=true;
}
else if (angle < min_angle - 0.001f)
{
constrained_angle_1=min_angle;
constrain_pyramid=true;
}
// end around axis 1 (pyramid)
// around axis 2 (pyramid)
y = global_pin_pivot1.m_front() % global_pin_pivot0.m_up();
x = global_pin_pivot1.m_front() % global_pin_pivot0.m_front();
constrained_angle_2 = angle = atan2(y,x);
min_angle=mMinAngle.Z*PI/180.0f;
max_angle=mMaxAngle.Z*PI/180.0f;
if (angle > max_angle + 0.001f)
{
constrained_angle_2=max_angle;
constrain_pyramid=true;
}
else if (angle < min_angle - 0.001f)
{
constrained_angle_2=min_angle;
constrain_pyramid=true;
}
// end around axis 2 (pyramid)
if (constrain_pyramid)
{
dFloat constrained_axis_0_length=global_pin_pivot1.m_front() % global_pin_pivot0.m_front();
dFloat constrained_axis_1_length=constrained_axis_0_length * tan(constrained_angle_1);
dFloat constrained_axis_2_length=constrained_axis_0_length * tan(constrained_angle_2);
dVector constrained_vector( global_pin_pivot0.m_front().Scale(constrained_axis_0_length)
+ global_pin_pivot0.m_right().Scale(constrained_axis_1_length)
+ global_pin_pivot0.m_up().Scale(constrained_axis_2_length)
);
constrained_vector=constrained_vector.Normalise();
dVector point_on_child=( global_pin_pivot1.m_posit
+ global_pin_pivot1.m_front().Scale(MIN_JOINT_PIN_LENGTH)
);
dVector point_on_parent=( global_pin_pivot0.m_posit
+ constrained_vector.Scale(MIN_JOINT_PIN_LENGTH)
);
NewtonUserJointAddLinearRow(mpNewtonJoint,&point_on_parent[0],&point_on_child[0],&global_pin_pivot0.m_front()[0]);
NewtonUserJointAddLinearRow(mpNewtonJoint,&point_on_parent[0],&point_on_child[0],&global_pin_pivot0.m_up()[0]);
NewtonUserJointAddLinearRow(mpNewtonJoint,&point_on_parent[0],&point_on_child[0],&global_pin_pivot0.m_right()[0]);
}
}