Skip to content
GitLab
Commit a42d0007 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Replave Vec3f[3] by Matrix3f

parent 7f3e1fd4
Hide whitespace changes
Inline Side-by-side
include/hpp/fcl/BV/BV.h
View file @ a42d0007
......@@ -125,12 +125,9 @@ public:
bv2.axis[2] = R.col(id[2]);
*/
bv2.To = tf1.transform(bv1.center());
bv2.extent = (bv1.max_ - bv1.min_) * 0.5;
const Matrix3f& R = tf1.getRotation();
bv2.axis[0] = R.col(0);
bv2.axis[1] = R.col(1);
bv2.axis[2] = R.col(2);
bv2.To.noalias() = tf1.transform(bv1.center());
bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5;
bv2.axes.noalias() = tf1.getRotation();
}
};
......@@ -140,11 +137,9 @@ class Converter<OBB, OBB>
public:
static void convert(const OBB& bv1, const Transform3f& tf1, OBB& bv2)
{
bv2.extent = bv1.extent;
bv2.To = tf1.transform(bv1.To);
bv2.axis[0] = tf1.getQuatRotation().transform(bv1.axis[0]);
bv2.axis[1] = tf1.getQuatRotation().transform(bv1.axis[1]);
bv2.axis[2] = tf1.getQuatRotation().transform(bv1.axis[2]);
bv2.extent.noalias() = bv1.extent;
bv2.To.noalias() = tf1.transform(bv1.To);
bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
}
};
......@@ -164,11 +159,9 @@ class Converter<RSS, OBB>
public:
static void convert(const RSS& bv1, const Transform3f& tf1, OBB& bv2)
{
bv2.extent = Vec3f(bv1.l[0] * 0.5 + bv1.r, bv1.l[1] * 0.5 + bv1.r, bv1.r);
bv2.To = tf1.transform(bv1.Tr);
bv2.axis[0] = tf1.getQuatRotation().transform(bv1.axis[0]);
bv2.axis[1] = tf1.getQuatRotation().transform(bv1.axis[1]);
bv2.axis[2] = tf1.getQuatRotation().transform(bv1.axis[2]);
bv2.extent.noalias() = Vec3f(bv1.l[0] * 0.5 + bv1.r, bv1.l[1] * 0.5 + bv1.r, bv1.r);
bv2.To.noalias() = tf1.transform(bv1.Tr);
bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
}
};
......@@ -207,9 +200,7 @@ public:
static void convert(const OBB& bv1, const Transform3f& tf1, RSS& bv2)
{
bv2.Tr = tf1.transform(bv1.To);
bv2.axis[0] = tf1.getQuatRotation().transform(bv1.axis[0]);
bv2.axis[1] = tf1.getQuatRotation().transform(bv1.axis[1]);
bv2.axis[2] = tf1.getQuatRotation().transform(bv1.axis[2]);
bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
bv2.r = bv1.extent[2];
bv2.l[0] = 2 * (bv1.extent[0] - bv2.r);
......@@ -223,10 +214,8 @@ class Converter<RSS, RSS>
public:
static void convert(const RSS& bv1, const Transform3f& tf1, RSS& bv2)
{
bv2.Tr = tf1.transform(bv1.Tr);
bv2.axis[0] = tf1.getQuatRotation().transform(bv1.axis[0]);
bv2.axis[1] = tf1.getQuatRotation().transform(bv1.axis[1]);
bv2.axis[2] = tf1.getQuatRotation().transform(bv1.axis[2]);
bv2.Tr.noalias() = tf1.transform(bv1.Tr);
bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
bv2.r = bv1.r;
bv2.l[0] = bv1.l[0];
......@@ -283,9 +272,10 @@ public:
const Matrix3f& R = tf1.getRotation();
bool left_hand = (id[0] == (id[1] + 1) % 3);
if (left_hand) bv2.axis[0] = -R.col(id[0]); else bv2.axis[0] = R.col(id[0]);
bv2.axis[1] = R.col(id[1]);
bv2.axis[2] = R.col(id[2]);
if (left_hand) bv2.axes.col(0).noalias() = -R.col(id[0]);
else bv2.axes.col(0).noalias() = R.col(id[0]);
bv2.axes.col(1).noalias() = R.col(id[1]);
bv2.axes.col(2).noalias() = R.col(id[2]);
}
};
......
include/hpp/fcl/BV/BV_node.h
View file @ a42d0007
......@@ -111,25 +111,19 @@ struct BVNode : public BVNodeBase
template<>
inline Matrix3f BVNode<OBB>::getOrientation() const
{
return (Matrix3f() << bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]).finished();
return bv.axes;
}
template<>
inline Matrix3f BVNode<RSS>::getOrientation() const
{
return (Matrix3f() << bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]).finished();
return bv.axes;
}
template<>
inline Matrix3f BVNode<OBBRSS>::getOrientation() const
{
return (Matrix3f() << bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]).finished();
return bv.obb.axes;
}
......
include/hpp/fcl/BV/OBB.h
View file @ a42d0007
......@@ -51,7 +51,7 @@ class OBB
public:
/// @brief Orientation of OBB. axis[i] is the ith column of the orientation matrix for the box; it is also the i-th principle direction of the box.
/// We assume that axis[0] corresponds to the axis with the longest box edge, axis[1] corresponds to the shorter one and axis[2] corresponds to the shortest one.
Vec3f axis[3];
Matrix3f axes;
/// @brief Center of OBB
Vec3f To;
......
include/hpp/fcl/BV/RSS.h
View file @ a42d0007
......@@ -52,7 +52,7 @@ class RSS
public:
/// @brief Orientation of RSS. axis[i] is the ith column of the orientation matrix for the RSS; it is also the i-th principle direction of the RSS.
/// We assume that axis[0] corresponds to the axis with the longest length, axis[1] corresponds to the shorter one and axis[2] corresponds to the shortest one.
Vec3f axis[3];
Matrix3f axes;
/// @brief Origin of the rectangle in RSS
Vec3f Tr;
......
include/hpp/fcl/BVH/BVH_model.h
View file @ a42d0007
......@@ -188,7 +188,7 @@ public:
/// BV node. When traversing the BVH, this can save one matrix transformation.
void makeParentRelative()
{
Vec3f I[3] = {Vec3f(1, 0, 0), Vec3f(0, 1, 0), Vec3f(0, 0, 1)};
Matrix3f I (Matrix3f::Identity());
makeParentRelativeRecurse(0, I, Vec3f());
}
......@@ -303,13 +303,13 @@ private:
/// @recursively compute each bv's transform related to its parent. For default BV, only the translation works.
/// For oriented BV (OBB, RSS, OBBRSS), special implementation is provided.
void makeParentRelativeRecurse(int bv_id, Vec3f parent_axis[], const Vec3f& parent_c)
void makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c)
{
if(!bvs[bv_id].isLeaf())
{
makeParentRelativeRecurse(bvs[bv_id].first_child, parent_axis, bvs[bv_id].getCenter());
makeParentRelativeRecurse(bvs[bv_id].first_child, parent_axes, bvs[bv_id].getCenter());
makeParentRelativeRecurse(bvs[bv_id].first_child + 1, parent_axis, bvs[bv_id].getCenter());
makeParentRelativeRecurse(bvs[bv_id].first_child + 1, parent_axes, bvs[bv_id].getCenter());
}
bvs[bv_id].bv = translate(bvs[bv_id].bv, -parent_c);
......@@ -318,13 +318,13 @@ private:
template<>
void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Vec3f parent_axis[], const Vec3f& parent_c);
void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c);
template<>
void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Vec3f parent_axis[], const Vec3f& parent_c);
void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c);
template<>
void BVHModel<OBBRSS>::makeParentRelativeRecurse(int bv_id, Vec3f parent_axis[], const Vec3f& parent_c);
void BVHModel<OBBRSS>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c);
/// @brief Specialization of getNodeType() for BVHModel with different BV types
......
include/hpp/fcl/BVH/BVH_utility.h
View file @ a42d0007
......@@ -81,10 +81,10 @@ void BVHExpand(BVHModel<RSS>& model, const Variance3f* ucs, FCL_REAL r);
void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Matrix3f& M);
/// @brief Compute the RSS bounding volume parameters: radius, rectangle size and the origin, given the BV axises.
void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& origin, FCL_REAL l[2], FCL_REAL& r);
void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, const Matrix3f& axes, Vec3f& origin, FCL_REAL l[2], FCL_REAL& r);
/// @brief Compute the bounding volume extent and center for a set or subset of points, given the BV axises.
void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& center, Vec3f& extent);
void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Matrix3f& axes, Vec3f& center, Vec3f& extent);
/// @brief Compute the center and radius for a triangle's circumcircle
void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, Vec3f& center, FCL_REAL& radius);
......
include/hpp/fcl/collision_object.h
View file @ a42d0007
......@@ -206,7 +206,7 @@ public:
}
else
{
Vec3f center = t.transform(cgeom->aabb_center);
Vec3f center (t.transform(cgeom->aabb_center));
Vec3f delta(Vec3f::Constant(cgeom->aabb_radius));
aabb.min_ = center - delta;
aabb.max_ = center + delta;
......
include/hpp/fcl/math/transform.h
View file @ a42d0007
......@@ -85,10 +85,10 @@ public:
void toEuler(FCL_REAL& a, FCL_REAL& b, FCL_REAL& c) const;
/// @brief Axes to quaternion
void fromAxes(const Vec3f axis[3]);
void fromAxes(const Matrix3f& axes);
/// @brief Axes to matrix
void toAxes(Vec3f axis[3]) const;
void toAxes(Matrix3f& axis) const;
/// @brief Axis and angle to quaternion
void fromAxisAngle(const Vec3f& axis, FCL_REAL angle);
......
include/hpp/fcl/traversal/traversal_node_bvhs.h
View file @ a42d0007
......@@ -860,15 +860,15 @@ namespace details
{
template<typename BV>
const Vec3f& getBVAxis(const BV& bv, int i)
inline const Matrix3f& getBVAxes(const BV& bv)
{
return bv.axis[i];
return bv.axes;
}
template<>
inline const Vec3f& getBVAxis<OBBRSS>(const OBBRSS& bv, int i)
inline const Matrix3f& getBVAxes<OBBRSS>(const OBBRSS& bv)
{
return bv.obb.axis[i];
return bv.obb.axes;
}
......@@ -906,10 +906,7 @@ bool meshConservativeAdvancementTraversalNodeCanStop(FCL_REAL c,
assert(c == d);
Vec3f n_transformed =
getBVAxis(model1->getBV(c1).bv, 0) * n[0] +
getBVAxis(model1->getBV(c1).bv, 1) * n[1] +
getBVAxis(model1->getBV(c1).bv, 2) * n[2];
Vec3f n_transformed (getBVAxes(model1->getBV(c1).bv) * n);
TBVMotionBoundVisitor<BV> mb_visitor1(model1->getBV(c1).bv, n_transformed), mb_visitor2(model2->getBV(c2).bv, n_transformed);
FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
......
src/BV/OBB.cpp
View file @ a42d0007
......@@ -48,22 +48,15 @@ namespace fcl
/// @brief Compute the 8 vertices of a OBB
inline void computeVertices(const OBB& b, Vec3f vertices[8])
{
const Vec3f* axis = b.axis;
const Vec3f& extent = b.extent;
const Vec3f& To = b.To;
Vec3f extAxis0 = axis[0] * extent[0];
Vec3f extAxis1 = axis[1] * extent[1];
Vec3f extAxis2 = axis[2] * extent[2];
vertices[0] = To - extAxis0 - extAxis1 - extAxis2;
vertices[1] = To + extAxis0 - extAxis1 - extAxis2;
vertices[2] = To + extAxis0 + extAxis1 - extAxis2;
vertices[3] = To - extAxis0 + extAxis1 - extAxis2;
vertices[4] = To - extAxis0 - extAxis1 + extAxis2;
vertices[5] = To + extAxis0 - extAxis1 + extAxis2;
vertices[6] = To + extAxis0 + extAxis1 + extAxis2;
vertices[7] = To - extAxis0 + extAxis1 + extAxis2;
Matrix3f extAxes (b.axes * b.extent.asDiagonal());
vertices[0].noalias() = b.To + extAxes * Vec3f(-1,-1,-1);
vertices[1].noalias() = b.To + extAxes * Vec3f( 1,-1,-1);
vertices[2].noalias() = b.To + extAxes * Vec3f( 1, 1,-1);
vertices[3].noalias() = b.To + extAxes * Vec3f(-1, 1,-1);
vertices[4].noalias() = b.To + extAxes * Vec3f(-1,-1, 1);
vertices[5].noalias() = b.To + extAxes * Vec3f( 1,-1, 1);
vertices[6].noalias() = b.To + extAxes * Vec3f( 1, 1, 1);
vertices[7].noalias() = b.To + extAxes * Vec3f(-1, 1, 1);
}
/// @brief OBB merge method when the centers of two smaller OBB are far away
......@@ -78,16 +71,11 @@ inline OBB merge_largedist(const OBB& b1, const OBB& b2)
Matrix3f::Scalar s[3] = {0, 0, 0};
// obb axes
Vec3f& R0 = b.axis[0];
Vec3f& R1 = b.axis[1];
Vec3f& R2 = b.axis[2];
R0 = b1.To - b2.To;
R0.normalize();
b.axes.col(0).noalias() = (b1.To - b2.To).normalized();
Vec3f vertex_proj[16];
for(int i = 0; i < 16; ++i)
vertex_proj[i] = vertex[i] - R0 * vertex[i].dot(R0);
vertex_proj[i] = vertex[i] - b.axes.col(0) * vertex[i].dot(b.axes.col(0));
getCovariance(vertex_proj, NULL, NULL, NULL, 16, M);
eigen(M, s, E);
......@@ -100,12 +88,12 @@ inline OBB merge_largedist(const OBB& b1, const OBB& b2)
else { mid = 2; }
R1 << E[0][max], E[1][max], E[2][max];
R2 << E[0][mid], E[1][mid], E[2][mid];
b.axes.col(1) << E[0][max], E[1][max], E[2][max];
b.axes.col(2) << E[0][mid], E[1][mid], E[2][mid];
// set obb centers and extensions
Vec3f center, extent;
getExtentAndCenter(vertex, NULL, NULL, NULL, 16, b.axis, center, extent);
getExtentAndCenter(vertex, NULL, NULL, NULL, 16, b.axes, center, extent);
b.To = center;
b.extent = extent;
......@@ -120,15 +108,15 @@ inline OBB merge_smalldist(const OBB& b1, const OBB& b2)
OBB b;
b.To = (b1.To + b2.To) * 0.5;
Quaternion3f q0, q1;
q0.fromAxes(b1.axis);
q1.fromAxes(b2.axis);
q0.fromAxes(b1.axes);
q1.fromAxes(b2.axes);
if(q0.dot(q1) < 0)
q1 = -q1;
Quaternion3f q = q0 + q1;
FCL_REAL inv_length = 1.0 / std::sqrt(q.dot(q));
q = q * inv_length;
q.toAxes(b.axis);
q.toAxes(b.axes);
Vec3f vertex[8], diff;
......@@ -142,7 +130,7 @@ inline OBB merge_smalldist(const OBB& b1, const OBB& b2)
diff = vertex[i] - b.To;
for(int j = 0; j < 3; ++j)
{
FCL_REAL dot = diff.dot(b.axis[j]);
FCL_REAL dot = diff.dot(b.axes.col(j));
if(dot > pmax[j])
pmax[j] = dot;
else if(dot < pmin[j])
......@@ -156,7 +144,7 @@ inline OBB merge_smalldist(const OBB& b1, const OBB& b2)
diff = vertex[i] - b.To;
for(int j = 0; j < 3; ++j)
{
FCL_REAL dot = diff.dot(b.axis[j]);
FCL_REAL dot = diff.dot(b.axes.col(j));
if(dot > pmax[j])
pmax[j] = dot;
else if(dot < pmin[j])
......@@ -166,7 +154,7 @@ inline OBB merge_smalldist(const OBB& b1, const OBB& b2)
for(int j = 0; j < 3; ++j)
{
b.To += (b.axis[j] * (0.5 * (pmax[j] + pmin[j])));
b.To.noalias() += (b.axes.col(j) * (0.5 * (pmax[j] + pmin[j])));
b.extent[j] = 0.5 * (pmax[j] - pmin[j]);
}
......@@ -545,12 +533,8 @@ bool OBB::overlap(const OBB& other) const
/// compute what transform [R,T] that takes us from cs1 to cs2.
/// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
/// First compute the rotation part, then translation part
Vec3f t = other.To - To; // T2 - T1
Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1)
Matrix3f R;
R << axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), axis[0].dot(other.axis[2]),
axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), axis[1].dot(other.axis[2]),
axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), axis[2].dot(other.axis[2]);
Vec3f T (axes.transpose() * (other.To - To));
Matrix3f R (axes.transpose() * other.axes);
return !obbDisjoint(R, T, extent, other.extent);
}
......@@ -560,15 +544,16 @@ bool OBB::overlap(const OBB& other) const
/// compute what transform [R,T] that takes us from cs1 to cs2.
/// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
/// First compute the rotation part, then translation part
Vec3f t = other.To - To; // T2 - T1
Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1)
Matrix3f R;
R << axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]),
axis[0].dot(other.axis[2]),
axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]),
axis[1].dot(other.axis[2]),
axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]),
axis[2].dot(other.axis[2]);
// Vec3f t = other.To - To; // T2 - T1
// Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1)
// Matrix3f R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]),
// axis[0].dot(other.axis[2]),
// axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]),
// axis[1].dot(other.axis[2]),
// axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]),
// axis[2].dot(other.axis[2]));
Vec3f T (axes.transpose() * (other.To - To));
Matrix3f R (axes.transpose() * other.axes);
return !obbDisjointAndLowerBoundDistance
(R, T, extent, other.extent, sqrDistLowerBound);
......@@ -578,15 +563,15 @@ bool OBB::overlap(const OBB& other) const
bool OBB::contain(const Vec3f& p) const
{
Vec3f local_p = p - To;
FCL_REAL proj = local_p.dot(axis[0]);
FCL_REAL proj = local_p.dot(axes.col(0));
if((proj > extent[0]) || (proj < -extent[0]))
return false;
proj = local_p.dot(axis[1]);
proj = local_p.dot(axes.col(1));
if((proj > extent[1]) || (proj < -extent[1]))
return false;
proj = local_p.dot(axis[2]);
proj = local_p.dot(axes.col(2));
if((proj > extent[2]) || (proj < -extent[2]))
return false;
......@@ -596,10 +581,8 @@ bool OBB::contain(const Vec3f& p) const
OBB& OBB::operator += (const Vec3f& p)
{
OBB bvp;
bvp.To = p;
bvp.axis[0] = axis[0];
bvp.axis[1] = axis[1];
bvp.axis[2] = axis[2];
bvp.To.noalias() = p;
bvp.axes.noalias() = axes;
bvp.extent.setZero();
*this += bvp;
......@@ -631,18 +614,9 @@ FCL_REAL OBB::distance(const OBB& /*other*/, Vec3f* /*P*/, Vec3f* /*Q*/) const
bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2)
{
Matrix3f R0b2;
R0b2 << R0.row(0).dot(b2.axis[0]), R0.row(0).dot(b2.axis[1]), R0.row(0).dot(b2.axis[2]),
R0.row(1).dot(b2.axis[0]), R0.row(1).dot(b2.axis[1]), R0.row(1).dot(b2.axis[2]),
R0.row(2).dot(b2.axis[0]), R0.row(2).dot(b2.axis[1]), R0.row(2).dot(b2.axis[2]);
Matrix3f R;
R << R0b2.col(0).dot(b1.axis[0]), R0b2.col(1).dot(b1.axis[0]), R0b2.col(2).dot(b1.axis[0]),
R0b2.col(0).dot(b1.axis[1]), R0b2.col(1).dot(b1.axis[1]), R0b2.col(2).dot(b1.axis[1]),
R0b2.col(0).dot(b1.axis[2]), R0b2.col(1).dot(b1.axis[2]), R0b2.col(2).dot(b1.axis[2]);
Vec3f Ttemp = R0 * b2.To + T0 - b1.To;
Vec3f T(Ttemp.dot(b1.axis[0]), Ttemp.dot(b1.axis[1]), Ttemp.dot(b1.axis[2]));
Vec3f Ttemp (R0 * b2.To + T0 - b1.To);
Vec3f T (b1.axes.transpose() * Ttemp);
Matrix3f R (b1.axes.transpose() * R0 * b2.axes);
return !obbDisjoint(R, T, b1.extent, b2.extent);
}
......@@ -650,18 +624,9 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2)
bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2,
FCL_REAL& sqrDistLowerBound)
{
Matrix3f R0b2;
R0b2 << R0.row(0).dot(b2.axis[0]), R0.row(0).dot(b2.axis[1]), R0.row(0).dot(b2.axis[2]),
R0.row(1).dot(b2.axis[0]), R0.row(1).dot(b2.axis[1]), R0.row(1).dot(b2.axis[2]),
R0.row(2).dot(b2.axis[0]), R0.row(2).dot(b2.axis[1]), R0.row(2).dot(b2.axis[2]);
Matrix3f R;
R << R0b2.col(0).dot(b1.axis[0]), R0b2.col(1).dot(b1.axis[0]), R0b2.col(2).dot(b1.axis[0]),
R0b2.col(0).dot(b1.axis[1]), R0b2.col(1).dot(b1.axis[1]), R0b2.col(2).dot(b1.axis[1]),
R0b2.col(0).dot(b1.axis[2]), R0b2.col(1).dot(b1.axis[2]), R0b2.col(2).dot(b1.axis[2]);
Vec3f Ttemp = R0 * b2.To + T0 - b1.To;
Vec3f T(Ttemp.dot(b1.axis[0]), Ttemp.dot(b1.axis[1]), Ttemp.dot(b1.axis[2]));
Vec3f Ttemp (R0 * b2.To + T0 - b1.To);
Vec3f T (b1.axes.transpose() * Ttemp);
Matrix3f R (b1.axes.transpose() * R0 * b2.axes);
return !obbDisjointAndLowerBoundDistance (R, T, b1.extent, b2.extent,
sqrDistLowerBound);
......
src/BV/RSS.cpp
View file @ a42d0007
......@@ -835,17 +835,11 @@ bool RSS::overlap(const RSS& other) const
/// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
/// First compute the rotation part, then translation part
/// First compute T2 - T1
Vec3f t = other.Tr - Tr;
/// Then compute R1'(T2 - T1)
Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2]));
Vec3f T (axes.transpose() * (other.Tr - Tr));
/// Now compute R1'R2
Matrix3f R;
R << axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), axis[0].dot(other.axis[2]),
axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), axis[1].dot(other.axis[2]),
axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), axis[2].dot(other.axis[2]);
Matrix3f R (axes.transpose() * other.axes);
FCL_REAL dist = rectDistance(R, T, l, other.l);
return (dist <= (r + other.r));
......@@ -855,20 +849,12 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2)
{
// ROb2 = R0 . b2
// where b2 = [ b2.axis [0] | b2.axis [1] | b2.axis [2] ]
Matrix3f R0b2;
R0b2 << R0.row(0).dot(b2.axis[0]), R0.row(0).dot(b2.axis[1]), R0.row(0).dot(b2.axis[2]),
R0.row(1).dot(b2.axis[0]), R0.row(1).dot(b2.axis[1]), R0.row(1).dot(b2.axis[2]),
R0.row(2).dot(b2.axis[0]), R0.row(2).dot(b2.axis[1]), R0.row(2).dot(b2.axis[2]);
// (1 0 0)^T R0b2^T axis [0] = (1 0 0)^T b2^T R0^T axis [0]
// R = b2^T RO^T b1
Matrix3f R;
R << R0b2.col(0).dot(b1.axis[0]), R0b2.col(1).dot(b1.axis[0]), R0b2.col(2).dot(b1.axis[0]),
R0b2.col(0).dot(b1.axis[1]), R0b2.col(1).dot(b1.axis[1]), R0b2.col(2).dot(b1.axis[1]),
R0b2.col(0).dot(b1.axis[2]), R0b2.col(1).dot(b1.axis[2]), R0b2.col(2).dot(b1.axis[2]);
Vec3f Ttemp = R0 * b2.Tr + T0 - b1.Tr;
Vec3f T(Ttemp.dot(b1.axis[0]), Ttemp.dot(b1.axis[1]), Ttemp.dot(b1.axis[2]));
Vec3f Ttemp (R0 * b2.Tr + T0 - b1.Tr);
Vec3f T(b1.axes.transpose() * Ttemp);
Matrix3f R(b2.axes.transpose() * R0.transpose() * b1.axes);
FCL_REAL dist = rectDistance(R, T, b1.l, b2.l);
return (dist <= (b1.r + b2.r));
......@@ -877,9 +863,10 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2)
bool RSS::contain(const Vec3f& p) const
{
Vec3f local_p = p - Tr;
FCL_REAL proj0 = local_p.dot(axis[0]);
FCL_REAL proj1 = local_p.dot(axis[1]);
FCL_REAL proj2 = local_p.dot(axis[2]);
// FIXME: Vec3f proj (axes.transpose() * local_p);
FCL_REAL proj0 = local_p.dot(axes.col(0));
FCL_REAL proj1 = local_p.dot(axes.col(1));
FCL_REAL proj2 = local_p.dot(axes.col(2));
FCL_REAL abs_proj2 = fabs(proj2);
Vec3f proj(proj0, proj1, proj2);
......@@ -912,9 +899,9 @@ bool RSS::contain(const Vec3f& p) const
RSS& RSS::operator += (const Vec3f& p)
{
Vec3f local_p = p - Tr;
FCL_REAL proj0 = local_p.dot(axis[0]);
FCL_REAL proj1 = local_p.dot(axis[1]);
FCL_REAL proj2 = local_p.dot(axis[2]);
FCL_REAL proj0 = local_p.dot(axes.col(0));
FCL_REAL proj1 = local_p.dot(axes.col(1));
FCL_REAL proj2 = local_p.dot(axes.col(2));
FCL_REAL abs_proj2 = fabs(proj2);
Vec3f proj(proj0, proj1, proj2);
......@@ -1049,37 +1036,37 @@ RSS RSS::operator + (const RSS& other) const
RSS bv;
Vec3f v[16];
Vec3f d0_pos = other.axis[0] * (other.l[0] + other.r);
Vec3f d1_pos = other.axis[1] * (other.l[1] + other.r);
Vec3f d0_neg = other.axis[0] * (-other.r);
Vec3f d1_neg = other.axis[1] * (-other.r);
Vec3f d2_pos = other.axis[2] * other.r;
Vec3f d2_neg = other.axis[2] * (-other.r);
v[0] = other.Tr + d0_pos + d1_pos + d2_pos;
v[1] = other.Tr + d0_pos + d1_pos + d2_neg;
v[2] = other.Tr + d0_pos + d1_neg + d2_pos;
v[3] = other.Tr + d0_pos + d1_neg + d2_neg;
v[4] = other.Tr + d0_neg + d1_pos + d2_pos;
v[5] = other.Tr + d0_neg + d1_pos + d2_neg;
v[6] = other.Tr + d0_neg + d1_neg + d2_pos;
v[7] = other.Tr + d0_neg + d1_neg + d2_neg;
d0_pos = axis[0] * (l[0] + r);
d1_pos = axis[1] * (l[1] + r);