Skip to content
GitLab
Commit 1bac302a authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

[GJK] Factorize some code.

parent 175a0a88
Hide whitespace changes
Inline Side-by-side
src/narrowphase/gjk.cpp
View file @ 1bac302a
......@@ -573,43 +573,93 @@ bool GJK::encloseOrigin()
return false;
}
FCL_REAL GJK::projectLineOrigin(const Simplex& current, Simplex& next)
inline void originToPoint (
const GJK::Simplex& current, int a,
const Vec3f& A,
GJK::Simplex& next,
Vec3f& ray)
{
const int a = 1, b = 0;
// A is the last point we added.
const Vec3f& A = current.vertex[a]->w;
const Vec3f& B = current.vertex[b]->w;
const Vec3f AB = B - A;
const FCL_REAL d = AB.dot(-A);
assert (d <= AB.squaredNorm());
if (d <= 0) {
// A is the closest to the origin
ray = A;
next.vertex[0] = current.vertex[a];
next.rank = 1;
free_v[nfree++] = current.vertex[b];
// To ensure backward compatibility
next.coefficient[0] = 1;
return A.squaredNorm();
} else {
}
inline void originToSegment (
const GJK::Simplex& current, int a, int b,
const Vec3f& A, const Vec3f& B,
const Vec3f& AB,
const FCL_REAL& ABdotAO,
GJK::Simplex& next,
Vec3f& ray)
{
// ray = - ( AB ^ AO ) ^ AB = (AB.B) A + (-AB.A) B
ray = AB.dot(B) * A + d * B;
ray = AB.dot(B) * A + ABdotAO * B;
next.vertex[0] = current.vertex[b];
next.vertex[1] = current.vertex[a];
next.rank = 2;
// To ensure backward compatibility
FCL_REAL alpha = d / AB.squaredNorm();
FCL_REAL alpha = ABdotAO / AB.squaredNorm();
next.coefficient[1] = 1 - alpha; // A
next.coefficient[0] = alpha; // B
ray /= AB.squaredNorm();
}
return ray.squaredNorm();
}
inline void originToTriangle (
const GJK::Simplex& current,
int a, int b, int c,
const Vec3f& A, const Vec3f& AB, const Vec3f& AC,
const Vec3f& ABC,
GJK::Simplex& next,
Vec3f& ray)
{
ray = ABC;
next.vertex[0] = current.vertex[c];
next.vertex[1] = current.vertex[b];
next.vertex[2] = current.vertex[a];
next.rank = 3;
// To ensure backward compatibility
FCL_REAL s = ABC.squaredNorm();
ray *= A.dot(ABC) / s;
Vec3f AQ (ray - A);
Vec3f m (ABC.cross(AB));
FCL_REAL _u2 = 1/AB.squaredNorm();
FCL_REAL wu_u2 = AQ.dot (AB) * _u2;
FCL_REAL vu_u2 = AC.dot (AB) * _u2;
FCL_REAL wm_vm = AQ.dot(m) / AC.dot (m);
next.coefficient[0] = wm_vm; // C
next.coefficient[1] = wu_u2 - wm_vm*vu_u2; // B
next.coefficient[2] = 1 - next.coefficient[0] - next.coefficient[1]; // A
assert (0. <= next.coefficient[0] && next.coefficient[0] <= 1.);
assert (0. <= next.coefficient[1] && next.coefficient[1] <= 1.);
assert (0. <= next.coefficient[2] && next.coefficient[2] <= 1.);
}
FCL_REAL GJK::projectLineOrigin(const Simplex& current, Simplex& next)
{
const int a = 1, b = 0;
// A is the last point we added.
const Vec3f& A = current.vertex[a]->w;
const Vec3f& B = current.vertex[b]->w;
const Vec3f AB = B - A;
const FCL_REAL d = AB.dot(-A);
assert (d <= AB.squaredNorm());
if (d <= 0) {
// A is the closest to the origin
originToPoint (current, a, A, next, ray);
free_v[nfree++] = current.vertex[b];
} else
originToSegment (current, a, b, A, B, AB, d, next, ray);
return ray.squaredNorm();
}
FCL_REAL GJK::projectTriangleOrigin(const Simplex& current, Simplex& next)
......@@ -628,48 +678,18 @@ FCL_REAL GJK::projectTriangleOrigin(const Simplex& current, Simplex& next)
if (edgeAC2o >= 0) {
FCL_REAL towardsC = AC.dot (-A);
if (towardsC >= 0) { // Region 1
// ray = - ( AC ^ AO ) ^ AC = (AC.C) A + (-AC.A) C
ray = AC.dot(C) * A + towardsC * C;
next.vertex[0] = current.vertex[c];
next.vertex[1] = current.vertex[a];
next.rank = 2;
originToSegment (current, a, c, A, C, AC, towardsC, next, ray);
free_v[nfree++] = current.vertex[b];
// To ensure backward compatibility
FCL_REAL alpha = towardsC / AC.squaredNorm();
next.coefficient[1] = 1 - alpha; // A
next.coefficient[0] = alpha; // C
ray /= AC.squaredNorm();
return ray.squaredNorm();
} else { // Region 4 or 5
FCL_REAL towardsB = AB.dot(-A);
if (towardsB < 0) { // Region 5
// A is the closest to the origin
ray = A;
next.vertex[0] = current.vertex[a];
next.rank = 1;
originToPoint (current, a, A, next, ray);
free_v[nfree++] = current.vertex[c];
free_v[nfree++] = current.vertex[b];
// To ensure backward compatibility
next.coefficient[0] = 1;
return A.squaredNorm();
} else { // Region 4
// ray = - ( AB ^ AO ) ^ AB = (AB.B) A + (-AB.A) B
ray = AB.dot(B) * A + towardsB * B;
next.vertex[0] = current.vertex[b];
next.vertex[1] = current.vertex[a];
next.rank = 2;
originToSegment (current, a, b, A, B, AB, towardsB, next, ray);
free_v[nfree++] = current.vertex[c];
// To ensure backward compatibility
FCL_REAL alpha = towardsB / AB.squaredNorm();
next.coefficient[1] = 1 - alpha; // A
next.coefficient[0] = alpha; // B
ray /= AB.squaredNorm();
return ray.squaredNorm();
}
}
} else {
......@@ -678,83 +698,28 @@ FCL_REAL GJK::projectTriangleOrigin(const Simplex& current, Simplex& next)
FCL_REAL towardsB = AB.dot(-A);
if (towardsB < 0) { // Region 5
// A is the closest to the origin
ray = A;
next.vertex[0] = current.vertex[a];
next.rank = 1;
originToPoint (current, a, A, next, ray);
free_v[nfree++] = current.vertex[c];
free_v[nfree++] = current.vertex[b];
// To ensure backward compatibility
next.coefficient[0] = 1;
return A.squaredNorm();
} else { // Region 4
// ray = - ( AB ^ AO ) ^ AB = (AB.B) A + (-AB.A) B
ray = AB.dot(B) * A + towardsB * B;
next.vertex[0] = current.vertex[b];
next.vertex[1] = current.vertex[a];
next.rank = 2;
originToSegment (current, a, b, A, B, AB, towardsB, next, ray);
free_v[nfree++] = current.vertex[c];
// To ensure backward compatibility
FCL_REAL alpha = towardsB / AB.squaredNorm();
next.coefficient[1] = 1 - alpha; // A
next.coefficient[0] = alpha; // B
ray /= AB.squaredNorm();
return ray.squaredNorm();
}
} else {
// I don't think there is any need to check whether we are above or below
// the triangle.
originToTriangle (current, a, b, c, A, AB, AC, ABC, next, ray);
/*
FCL_REAL aboveTri = ABC.dot(-A);
if (aboveTri) { // Region 2
ray = ABC;
next.vertex[0] = current.vertex[c];
next.vertex[1] = current.vertex[b];
next.vertex[2] = current.vertex[a];
next.rank = 3;
// To ensure backward compatibility
FCL_REAL s = ABC.squaredNorm();
ray *= A.dot(ABC) / s;
Vec3f AQ (ray - A);
Vec3f m (ABC.cross(AB));
FCL_REAL _u2 = 1/AB.squaredNorm();
FCL_REAL wu_u2 = AQ.dot (AB) * _u2;
FCL_REAL vu_u2 = AC.dot (AB) * _u2;
FCL_REAL wm_vm = AQ.dot(m) / AC.dot (m);
next.coefficient[0] = wm_vm; // C
next.coefficient[1] = wu_u2 - wm_vm*vu_u2; // B
next.coefficient[2] = 1 - next.coefficient[0] - next.coefficient[1]; // A
assert (0. <= next.coefficient[0] && next.coefficient[0] <= 1.);
assert (0. <= next.coefficient[1] && next.coefficient[1] <= 1.);
assert (0. <= next.coefficient[2] && next.coefficient[2] <= 1.);
return ray.squaredNorm();
originToTriangle (current, a, b, c, A, AB, AC, ABC, next, ray);
} else { // Region 3
ray = ABC;
//next.vertex[0] = current.vertex[b];
next.vertex[0] = current.vertex[c];
next.vertex[1] = current.vertex[b];
next.vertex[2] = current.vertex[a];
next.rank = 3;
// To ensure backward compatibility
FCL_REAL s = ABC.squaredNorm();
ray *= A.dot(ABC) / s;
next.coefficient[2] = -1;
next.coefficient[1] = -1;
next.coefficient[0] = -1;
assert (0. <= next.coefficient[0] && next.coefficient[0] <= 1.);
assert (0. <= next.coefficient[1] && next.coefficient[1] <= 1.);
assert (0. <= next.coefficient[2] && next.coefficient[2] <= 1.);
return ray.squaredNorm();
// Flip the ray
}
*/
}
}
return ray.squaredNorm();
}
void EPA::initialize()
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment