My question itself is possibly quite simple and I guess that if someone can answer me they probably does not need a wall of text that is my background to the problem, but I figured I should provide as much information as possible just in case. The question (or rather, my problem) is at the bottom.
I am working on a (impulse-based) rigid body simulation, following two papers:
- An Introduction to Physically Based Modeling (D. Baraff)
- Fast Contact Force Computation for Nonpenetrating Rigid Bodies (D. Baraff)
These papers describe how to (among other things) calculate resting contact forces by implementing a quadratic program (there is pseudo code on page 26 of (2)).
First of all; although this is a borderline programming question, I have already implemented the quadratic program described by (2) for frictionless resting contacts, and there is no problem with this.
Background
The problem of computing contact forces is a problem of computing the vector $f$ satisfying the conditions along the (two bodies) collision-normal:
$$ 1. \; a_i \geq 0, \\ 2. \; f_i \geq 0, \\ 3. \; f_i a_i=0 $$ In essence: 1. to prevent interpenetration acceleration has to be positive (separating), 2. indicates a repulsive force between the bodies (can only push apart), and 3. says that at least one of $a_i$ or $f_i$ has to be $0$.
The collision-normal is (somewhat simplified) defined as a unit vector perpendicular to the edge of the collision (with a direction outward from the body containing the edge, which will be defined as body $B$).
The solution involves forming and solving a system $Ax=b$ in order to calculate the force $f_i$ to apply to contact $i$ ($f_i$ is added as an impulse, $f_i n_i$ to $A$ and $-f_i n_i$ to $B$). Since I would not be able to describe it half as well (and it would take more space), if it is unclear what $Ax=b$ represents in this case, please see page 49-52 of (1).
Anyway, I am getting closer to the problem now. $b$ is defined as: $$ b_i = n_i . (p_a'' - p_b'') + 2 n_i' . (p_a' - p_b') $$ where $p_a = p_b = c_i$ (points of collision on body $A$ and $B$), $p'$ is the derivative of $p$ (velocity), and $p''$ is the derivative of $p'$ (acceleration). Although $p_a = p_b$ (within numerical tolerance), the velocities can be different. $n_i'$ is defined as the derivative of the normal vector, and since the normal is always perpendicular to the edge of collision on body $B$, $$n_i' = AngularVelocity_b \times n_i$$ (since $n_i$ will be rotating at the same rate as body $B$). Very easy to calculate.
This is all done, and working without problem.
Bilateral Constraints
As cited from page 27 of (2): "In addition, it is trivial to make the algorithm handle standard bilateral constraints. For a bilateral constraint, we introduce a pair $f_i$ and $a_i$, and we constrain $a_i$ to always be zero while letting $f_i$ be either positive or negative. Given $k$ such constraints, we initially solve a square linear system of size $k$ to compute compute initial values for all the bilateral $f_i$’s so that all the corresponding $a_i$’s are zero."
My goal is to implement a hinge point, and again, the implementation part of the quadratic program for bilateral constraints is done (and I am fairly sure it is correct).
Problem
Whereas the normal $n$ and its derivative $n'$ of a unilateral constraint is intuitive (and easy to define), I have a hard time understanding the normal of a bilateral constraint.
So far, the only sensible thing I have managed to come up with is that the normal represents the unit vector defined by the difference $p_a - p_b$, assuming that $p_a \neq p_b$ (and if $p_a = p_b$, the only thing I can think of is that the normal would be defined as some arbitrary unit vector since it makes no difference). Since the relative acceleration of $p_a$ and $p_b$ (along $n$) has to be $0$, this should mean that these points will not separate.
If this is correct, then that is fine, but I have still have to calculate $n'$ (and if this is incorrect I have no clue what to do).
Assuming that this definition of $n$ (for a bilateral constraint) is correct, I figured, why not define $n'$ the same way as for a unilateral constraint, that is: determine the rate at which this normal rotates and cross it with the normal. But unlike the unilateral constraint, this one is not bound to body $B$.
What I first came up with is something like this: $$ n = p_a - p_b, \\ n_v = n + (p_a' - p_b'),\\ angle = angle(\hat{n}, \hat{n_v}),\\ n' = angle * \hat{n} $$ where $$angle(v1, v2) = tan^{-1}(v2.y, v2.x) - tan^{-1}(v1.y, v1.x),\\ -\pi \leq angle \leq \pi$$
This has proven to be wrong. I have tried to come up with something more sensible for days, searching for clues and trying different approaches but I just cannot figure it out. The closest I have come to something working is when defining the $2n'.(p_a'-p_b')$ part of $b$ to something that I know is wrong but has caused the constraint to oscillate (with a force far greater than should be necessary). Other than that the constraint always immediately falls apart.
Again, I am fairly sure that my quadratic program is correct, and that the problem then only is to formulate the $Ax=b$ system for all bilateral constraints correctly.
Any and all help is greatly appreciated.