I am doing a C# project on ragdolls based on Thomas Jakobsen's article http://www.gpgstudy.com/gpgiki/GDC%202001%3A%20Advanced%20Character%20Physics
I have converted the logic to 2D and everything seems to be working only I am having problems implementing angle constraints.
This is the code I am using at the moment:
public static void SatisfyAngleConstraint(Particle p1, Particle p, float minAngle, float maxAngle)
{
float a1 = (float)Math.Atan2(p1.m_x.X - p1.getFixPoint().X, p1.m_x.Y - p1.getFixPoint().Y);
float a2 = (float)Math.Atan2(p.m_x.X - p.getFixPoint().X, p.m_x.Y - p.getFixPoint().Y);
float diffAngle = a2 - a1;
Game.currentGame.Window.Title = "a " + diffAngle;
//Vector2 OldPosition = p.m_x;
if (diffAngle > maxAngle)
p.m_x = RotatePoint(p.m_x, p.getFixPoint(), (diffAngle - maxAngle));
else if (diffAngle < minAngle)
p.m_x = RotatePoint(p.m_x, p.getFixPoint(), (diffAngle - minAngle));
//p.m_oldx += p.m_x - OldPosition;
}
Particles p1 and p are the joints and getFixPoint() returns the position of parent joint. RotatePoint(point, centerPoint, angleRad) returns the position of point rotated around centerPoint by angleRad radians.
This code causes severe jitters and due to the fact that I use Verlet integration - I have tried to compensate for this by adding the transformation to the old position as well and that does seem to solve some of my problems but I still experience severe jitters and random force application which leads me to believe that my math is bad.
I am applying this constraint after my distance constraint because the distance between the joints should be constant when I am rotating around the parent joint.
This is my first stackoverflow question so please tell me if my form is bad.