Commit 519ab5d5 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Remove useless code.

parent c676c3ab
......@@ -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:
......
Markdown is supported
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