/** Solve a line segment using barycentric coordinates. */
public void solve2() {
// Solve a line segment using barycentric coordinates.
//
// p = a1 * w1 + a2 * w2
// a1 + a2 = 1
//
// The vector from the origin to the closest point on the line is
// perpendicular to the line.
// e12 = w2 - w1
// dot(p, e) = 0
// a1 * dot(w1, e) + a2 * dot(w2, e) = 0
//
// 2-by-2 linear system
// [1 1 ][a1] = [1]
// [w1.e12 w2.e12][a2] = [0]
//
// Define
// d12_1 = dot(w2, e12)
// d12_2 = -dot(w1, e12)
// d12 = d12_1 + d12_2
//
// Solution
// a1 = d12_1 / d12
// a2 = d12_2 / d12
final Vec2 w1 = m_v1.w;
final Vec2 w2 = m_v2.w;
e12.set(w2).subLocal(w1);
// w1 region
float d12_2 = -Vec2.dot(w1, e12);
if (d12_2 <= 0.0f) {
// a2 <= 0, so we clamp it to 0
m_v1.a = 1.0f;
m_count = 1;
return;
}
// w2 region
float d12_1 = Vec2.dot(w2, e12);
if (d12_1 <= 0.0f) {
// a1 <= 0, so we clamp it to 0
m_v2.a = 1.0f;
m_count = 1;
m_v1.set(m_v2);
return;
}
// Must be in e12 region.
float inv_d12 = 1.0f / (d12_1 + d12_2);
m_v1.a = d12_1 * inv_d12;
m_v2.a = d12_2 * inv_d12;
m_count = 2;
}
/**
* Get the supporting vertex in the given direction.
*
* @param d
* @return
*/
public final Vec2 getSupportVertex(final Vec2 d) {
int bestIndex = 0;
float bestValue = Vec2.dot(m_vertices[0], d);
for (int i = 1; i < m_count; i++) {
float value = Vec2.dot(m_vertices[i], d);
if (value > bestValue) {
bestIndex = i;
bestValue = value;
}
}
return m_vertices[bestIndex];
}
@Override
public void solveVelocityConstraints(final SolverData data) {
Vec2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
Vec2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
final Vec2 vpA = pool.popVec2();
final Vec2 vpB = pool.popVec2();
final Vec2 PA = pool.popVec2();
final Vec2 PB = pool.popVec2();
Vec2.crossToOutUnsafe(wA, m_rA, vpA);
vpA.addLocal(vA);
Vec2.crossToOutUnsafe(wB, m_rB, vpB);
vpB.addLocal(vB);
float Cdot = -Vec2.dot(m_uA, vpA) - m_ratio * Vec2.dot(m_uB, vpB);
float impulse = -m_mass * Cdot;
m_impulse += impulse;
PA.set(m_uA).mulLocal(-impulse);
PB.set(m_uB).mulLocal(-m_ratio * impulse);
vA.x += m_invMassA * PA.x;
vA.y += m_invMassA * PA.y;
wA += m_invIA * Vec2.cross(m_rA, PA);
vB.x += m_invMassB * PB.x;
vB.y += m_invMassB * PB.y;
wB += m_invIB * Vec2.cross(m_rB, PB);
// data.velocities[m_indexA].v.set(vA);
data.velocities[m_indexA].w = wA;
// data.velocities[m_indexB].v.set(vB);
data.velocities[m_indexB].w = wB;
pool.pushVec2(4);
}
/**
* Solve a line segment using barycentric coordinates.<br>
* Possible regions:<br>
* - points[2]<br>
* - edge points[0]-points[2]<br>
* - edge points[1]-points[2]<br>
* - inside the triangle
*/
public void solve3() {
w1.set(m_v1.w);
w2.set(m_v2.w);
w3.set(m_v3.w);
// Edge12
// [1 1 ][a1] = [1]
// [w1.e12 w2.e12][a2] = [0]
// a3 = 0
e12.set(w2).subLocal(w1);
float w1e12 = Vec2.dot(w1, e12);
float w2e12 = Vec2.dot(w2, e12);
float d12_1 = w2e12;
float d12_2 = -w1e12;
// Edge13
// [1 1 ][a1] = [1]
// [w1.e13 w3.e13][a3] = [0]
// a2 = 0
e13.set(w3).subLocal(w1);
float w1e13 = Vec2.dot(w1, e13);
float w3e13 = Vec2.dot(w3, e13);
float d13_1 = w3e13;
float d13_2 = -w1e13;
// Edge23
// [1 1 ][a2] = [1]
// [w2.e23 w3.e23][a3] = [0]
// a1 = 0
e23.set(w3).subLocal(w2);
float w2e23 = Vec2.dot(w2, e23);
float w3e23 = Vec2.dot(w3, e23);
float d23_1 = w3e23;
float d23_2 = -w2e23;
// Triangle123
float n123 = Vec2.cross(e12, e13);
float d123_1 = n123 * Vec2.cross(w2, w3);
float d123_2 = n123 * Vec2.cross(w3, w1);
float d123_3 = n123 * Vec2.cross(w1, w2);
// w1 region
if (d12_2 <= 0.0f && d13_2 <= 0.0f) {
m_v1.a = 1.0f;
m_count = 1;
return;
}
// e12
if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f) {
float inv_d12 = 1.0f / (d12_1 + d12_2);
m_v1.a = d12_1 * inv_d12;
m_v2.a = d12_2 * inv_d12;
m_count = 2;
return;
}
// e13
if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f) {
float inv_d13 = 1.0f / (d13_1 + d13_2);
m_v1.a = d13_1 * inv_d13;
m_v3.a = d13_2 * inv_d13;
m_count = 2;
m_v2.set(m_v3);
return;
}
// w2 region
if (d12_1 <= 0.0f && d23_2 <= 0.0f) {
m_v2.a = 1.0f;
m_count = 1;
m_v1.set(m_v2);
return;
}
// w3 region
if (d13_1 <= 0.0f && d23_1 <= 0.0f) {
m_v3.a = 1.0f;
m_count = 1;
m_v1.set(m_v3);
return;
}
// e23
if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f) {
float inv_d23 = 1.0f / (d23_1 + d23_2);
m_v2.a = d23_1 * inv_d23;
m_v3.a = d23_2 * inv_d23;
m_count = 2;
m_v1.set(m_v3);
return;
}
// Must be in triangle123
float inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
m_v1.a = d123_1 * inv_d123;
m_v2.a = d123_2 * inv_d123;
m_v3.a = d123_3 * inv_d123;
m_count = 3;
}
