Remove useless code.

src/narrowphase/narrowphase.cpp

@@ -46,133 +46,6 @@ namespace fcl
 
 namespace details
 {
-#if 0
-  // Clamp n to lie within the range [min, max]
-  float clamp(float n, float min, float max) {
-    if (n < min) return min;
-    if (n > max) return max;
-    return n;
-  }
-
-  // Computes closest points C1 and C2 of S1(s)=P1+s*(Q1-P1) and
-  // S2(t)=P2+t*(Q2-P2), returning s and t. Function result is squared
-  // distance between between S1(s) and S2(t)
-  float closestPtSegmentSegment(Vec3f p1, Vec3f q1, Vec3f p2, Vec3f q2,
-				float &s, float &t, Vec3f &c1, Vec3f &c2)
-  {
-    const float EPSILON = 0.001;
-    Vec3f d1 = q1 - p1; // Direction vector of segment S1
-    Vec3f d2 = q2 - p2; // Direction vector of segment S2
-    Vec3f r = p1 - p2;
-    float a = d1.dot(d1); // Squared length of segment S1, always nonnegative
-
-    float e = d2.dot(d2); // Squared length of segment S2, always nonnegative
-    float f = d2.dot(r);
-    // Check if either or both segments degenerate into points
-    if (a <= EPSILON && e <= EPSILON) {
-      // Both segments degenerate into points
-      s = t = 0.0f;
-      c1 = p1;
-      c2 = p2;
-      Vec3f diff = c1-c2;
-      float res = diff.dot(diff);
-      return res;
-    }
-    if (a <= EPSILON) {
-      // First segment degenerates into a point
-      s = 0.0f;
-      t = f / e; // s = 0 => t = (b*s + f) / e = f / e
-      t = clamp(t, 0.0f, 1.0f);
-    } else {
-      float c = d1.dot(r);
-      if (e <= EPSILON) {
-	// Second segment degenerates into a point
-	t = 0.0f;
-	s = clamp(-c / a, 0.0f, 1.0f); // t = 0 => s = (b*t - c) / a = -c / a
-      } else {
-	// The general nondegenerate case starts here
-	float b = d1.dot(d2);
-	float denom = a*e-b*b; // Always nonnegative
-	// If segments not parallel, compute closest point on L1 to L2 and
-	// clamp to segment S1. Else pick arbitrary s (here 0)
-	if (denom != 0.0f) {
-	  std::cerr << "demoninator equals zero, using 0 as reference" << std::endl;
-	  s = clamp((b*f - c*e) / denom, 0.0f, 1.0f);
-	} else s = 0.0f;
-	// Compute point on L2 closest to S1(s) using
-	// t = Dot((P1 + D1*s) - P2,D2) / Dot(D2,D2) = (b*s + f) / e
-	t = (b*s + f) / e;
-
-	//
-	//If t in [0,1] done. Else clamp t, recompute s for the new value
-	//of t using s = Dot((P2 + D2*t) - P1,D1) / Dot(D1,D1)= (t*b - c) / a
-	//and clamp s to [0, 1]
-	if(t < 0.0f) {
-	  t = 0.0f;
-	  s = clamp(-c / a, 0.0f, 1.0f);
-	} else if (t > 1.0f) {
-	  t = 1.0f;
-	  s = clamp((b - c) / a, 0.0f, 1.0f);
-	}
-      }
-    }
-    c1 = p1 + d1 * s;
-    c2 = p2 + d2 * t;
-    Vec3f diff = c1-c2;
-    float res = diff.dot(diff);
-    return res;
-  }
-
-
-  // Computes closest points C1 and C2 of S1(s)=P1+s*(Q1-P1) and
-  // S2(t)=P2+t*(Q2-P2), returning s and t. Function result is squared
-  // distance between between S1(s) and S2(t)
-
-  bool capsuleCapsuleDistance(const Capsule& s1, const Transform3f& tf1,
-			      const Capsule& s2, const Transform3f& tf2,
-			      FCL_REAL* dist, Vec3f* p1_res, Vec3f* p2_res)
-  {
-
-    Vec3f p1(tf1.getTranslation());
-    Vec3f p2(tf2.getTranslation());
-
-    // line segment composes two points. First point is given by the origin, second point is computed by the origin transformed along z.
-    // extension along z-axis means transformation with identity matrix and translation vector z pos
-    Transform3f transformQ1(Vec3f(0,0,s1.lz));
-    transformQ1 = tf1 * transformQ1;
-    Vec3f q1 = transformQ1.getTranslation();
-
-
-    Transform3f transformQ2(Vec3f(0,0,s2.lz));
-    transformQ2 = tf2 * transformQ2;
-    Vec3f q2 = transformQ2.getTranslation();
-
-    // s and t correspont to the length of the line segment
-    float s, t;
-    Vec3f c1, c2;
-
-    float result = closestPtSegmentSegment(p1, q1, p2, q2, s, t, c1, c2);
-    *dist = sqrt(result)-s1.radius-s2.radius;
-
-    // getting directional unit vector
-    Vec3f distVec = c2 -c1;
-    distVec.normalize();
-
-    // extend the point to be border of the capsule.
-    // Done by following the directional unit vector for the length of the capsule radius
-    *p1_res = c1 + distVec*s1.radius;
-
-    distVec = c1-c2;
-    distVec.normalize();
-
-    *p2_res = c2 + distVec*s2.radius;
-
-    return true;
-  }
-
-#endif
-
-
 // Compute the point on a line segment that is the closest point on the
 // segment to to another point. The code is inspired by the explanation
 // given by Dan Sunday's page: