Commit 1bac302a by Joseph Mirabel

### [GJK] Factorize some code.

parent 175a0a88
 ... ... @@ -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!