Vector3f N = Stack.alloc(Vector3f.class);
N.cross(AD, BD);
float[] tp = new float[] { N.lengthSquared() };
Vector4f _M = Stack.alloc(Vector4f.class);//plane
if (tp[0] < BulletGlobals.SIMD_EPSILON)//ARE PARALELE
{
// project B over A
boolean invert_b_order = false;
_M.x = vB1.dot(AD);
_M.y = vB2.dot(AD);
if (_M.x > _M.y) {
invert_b_order = true;
//BT_SWAP_NUMBERS(_M[0],_M[1]);
_M.x = _M.x + _M.y;
_M.y = _M.x - _M.y;
_M.x = _M.x - _M.y;
}
_M.z = vA1.dot(AD);
_M.w = vA2.dot(AD);
// mid points
N.x = (_M.x + _M.y) * 0.5f;
N.y = (_M.z + _M.w) * 0.5f;
if (N.x < N.y) {
if (_M.y < _M.z) {
vPointB = invert_b_order ? vB1 : vB2;
vPointA = vA1;
}
else if (_M.y < _M.w) {
vPointB = invert_b_order ? vB1 : vB2;
closest_point_on_segment(vPointA, vPointB, vA1, vA2);
}
else {
vPointA = vA2;
closest_point_on_segment(vPointB, vPointA, vB1, vB2);
}
}
else {
if (_M.w < _M.x) {
vPointB = invert_b_order ? vB2 : vB1;
vPointA = vA2;
}
else if (_M.w < _M.y) {
vPointA = vA2;
closest_point_on_segment(vPointB, vPointA, vB1, vB2);
}
else {
vPointB = invert_b_order ? vB1 : vB2;
closest_point_on_segment(vPointA, vPointB, vA1, vA2);
}
}
return;
}
N.cross(N, BD);
_M.set(N.x, N.y, N.z, vB1.dot(N));
// get point A as the plane collision point
line_plane_collision(_M, AD, vA1, vPointA, tp, 0f, 1f);
/*Closest point on segment*/