Verified Commit bcd6a0c7 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

core: slight computational improvments

parent 501db789
......@@ -231,7 +231,7 @@ namespace fcl {
const Sphere& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
{
Vec3f diff = tf2.getTranslation() - tf1.getTranslation();
const Vec3f diff = tf2.getTranslation() - tf1.getTranslation();
FCL_REAL len = diff.norm();
if(len > s1.radius + s2.radius)
return false;
......@@ -268,8 +268,8 @@ namespace fcl {
normal = -diff/len;
dist = len - s1.radius - s2.radius;
p1 = o1 - diff * (s1.radius / len);
p2 = o2 + diff * (s2.radius / len);
p1.noalias() = o1 - normal * s1.radius;
p2.noalias() = o2 + normal * s2.radius;
return (dist >=0);
}
......@@ -284,7 +284,7 @@ namespace fcl {
if(t > 0)
{
FCL_REAL dotVV = v.dot(v);
FCL_REAL dotVV = v.squaredNorm();
if(t < dotVV)
{
t /= dotVV;
......@@ -299,8 +299,8 @@ namespace fcl {
else
t = 0;
nearest = from + v * t;
return diff.dot(diff);
nearest.noalias() = from + v * t;
return diff.squaredNorm();
}
/// @brief Whether a point's projection is in a triangle
......
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