diff --git a/include/hpp/fcl/BV/BV.h b/include/hpp/fcl/BV/BV.h
index c9f4f02522e867f7cc02dac93fc8e4233263984b..22f0f61baf3953ba297359a34728f9d97cd10fc7 100644
--- a/include/hpp/fcl/BV/BV.h
+++ b/include/hpp/fcl/BV/BV.h
@@ -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]);
}
};
diff --git a/include/hpp/fcl/BV/BV_node.h b/include/hpp/fcl/BV/BV_node.h
index 52a40baa4567c98e55354c3dc29731965ddff256..fc70c99a05c01330fcde059547a107568c8d3e8e 100644
--- a/include/hpp/fcl/BV/BV_node.h
+++ b/include/hpp/fcl/BV/BV_node.h
@@ -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;
}
diff --git a/include/hpp/fcl/BV/OBB.h b/include/hpp/fcl/BV/OBB.h
index 9159941cd567ecd993d6335780a7af90f8c67de5..6bb1127988aabe4985e9f72816fd36ef66effbce 100644
--- a/include/hpp/fcl/BV/OBB.h
+++ b/include/hpp/fcl/BV/OBB.h
@@ -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;
diff --git a/include/hpp/fcl/BV/RSS.h b/include/hpp/fcl/BV/RSS.h
index 7dc79c3c0fc0e0cf0e2f5bbe6bba55e678a8d6ad..2c5b80898ab301f890f1808135172849edf83ab0 100644
--- a/include/hpp/fcl/BV/RSS.h
+++ b/include/hpp/fcl/BV/RSS.h
@@ -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;
diff --git a/include/hpp/fcl/BVH/BVH_model.h b/include/hpp/fcl/BVH/BVH_model.h
index 5794850c94a6c89af55b49aa7a2cfbf3c879359d..761c6789ee8ecc96c6fac0bcd276d5e324a2e547 100644
--- a/include/hpp/fcl/BVH/BVH_model.h
+++ b/include/hpp/fcl/BVH/BVH_model.h
@@ -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
diff --git a/include/hpp/fcl/BVH/BVH_utility.h b/include/hpp/fcl/BVH/BVH_utility.h
index b5783ed785c06ccade80bec2fbc35acb09e6a302..dc9796e42d6cbe2269774f91e31edb8731e7bb71 100644
--- a/include/hpp/fcl/BVH/BVH_utility.h
+++ b/include/hpp/fcl/BVH/BVH_utility.h
@@ -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);
diff --git a/include/hpp/fcl/collision_object.h b/include/hpp/fcl/collision_object.h
index ec9ecdce999484cd39ff19f0c539d5b13dc407f1..fe1f816d9f405f15a9d0464d731b3441a9842573 100644
--- a/include/hpp/fcl/collision_object.h
+++ b/include/hpp/fcl/collision_object.h
@@ -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;
diff --git a/include/hpp/fcl/math/transform.h b/include/hpp/fcl/math/transform.h
index 5dfbe7c9772da41cd4bb0630e8a9ed268d081683..ebb19e16877928210a89eec346dca76e01a29f54 100644
--- a/include/hpp/fcl/math/transform.h
+++ b/include/hpp/fcl/math/transform.h
@@ -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);
diff --git a/include/hpp/fcl/traversal/traversal_node_bvhs.h b/include/hpp/fcl/traversal/traversal_node_bvhs.h
index 9b3f4efd8b509f2e9206216fb66344e7560447c2..515fd9ec2230d5a774ffb96ade2dea3d0563fc89 100644
--- a/include/hpp/fcl/traversal/traversal_node_bvhs.h
+++ b/include/hpp/fcl/traversal/traversal_node_bvhs.h
@@ -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);
diff --git a/src/BV/OBB.cpp b/src/BV/OBB.cpp
index bf045f803d1ca176e3bf95c6a923373401f7f96f..2628e9214fdc23a110ca59245401033849684afa 100644
--- a/src/BV/OBB.cpp
+++ b/src/BV/OBB.cpp
@@ -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);
diff --git a/src/BV/RSS.cpp b/src/BV/RSS.cpp
index 435a1b2afe91da051ce8defdf079aeab7c3717f7..240171d71173218016007cfb3efaa7f114da107d 100644
--- a/src/BV/RSS.cpp
+++ b/src/BV/RSS.cpp
@@ -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);
- d0_neg = axis[0] * (-r);
- d1_neg = axis[1] * (-r);
- d2_pos = axis[2] * r;
- d2_neg = axis[2] * (-r);
-
- v[8] = Tr + d0_pos + d1_pos + d2_pos;
- v[9] = Tr + d0_pos + d1_pos + d2_neg;
- v[10] = Tr + d0_pos + d1_neg + d2_pos;
- v[11] = Tr + d0_pos + d1_neg + d2_neg;
- v[12] = Tr + d0_neg + d1_pos + d2_pos;
- v[13] = Tr + d0_neg + d1_pos + d2_neg;
- v[14] = Tr + d0_neg + d1_neg + d2_pos;
- v[15] = Tr + d0_neg + d1_neg + d2_neg;
+ Vec3f d0_pos (other.axes.col(0) * (other.l[0] + other.r));
+ Vec3f d1_pos (other.axes.col(1) * (other.l[1] + other.r));
+ Vec3f d0_neg (other.axes.col(0) * (-other.r));
+ Vec3f d1_neg (other.axes.col(1) * (-other.r));
+ Vec3f d2_pos (other.axes.col(2) * other.r);
+ Vec3f d2_neg (other.axes.col(2) * (-other.r));
+
+ v[0].noalias() = other.Tr + d0_pos + d1_pos + d2_pos;
+ v[1].noalias() = other.Tr + d0_pos + d1_pos + d2_neg;
+ v[2].noalias() = other.Tr + d0_pos + d1_neg + d2_pos;
+ v[3].noalias() = other.Tr + d0_pos + d1_neg + d2_neg;
+ v[4].noalias() = other.Tr + d0_neg + d1_pos + d2_pos;
+ v[5].noalias() = other.Tr + d0_neg + d1_pos + d2_neg;
+ v[6].noalias() = other.Tr + d0_neg + d1_neg + d2_pos;
+ v[7].noalias() = other.Tr + d0_neg + d1_neg + d2_neg;
+
+ d0_pos.noalias() = axes.col(0) * (l[0] + r);
+ d1_pos.noalias() = axes.col(1) * (l[1] + r);
+ d0_neg.noalias() = axes.col(0) * (-r);
+ d1_neg.noalias() = axes.col(1) * (-r);
+ d2_pos.noalias() = axes.col(2) * r;
+ d2_neg.noalias() = axes.col(2) * (-r);
+
+ v[ 8].noalias() = Tr + d0_pos + d1_pos + d2_pos;
+ v[ 9].noalias() = Tr + d0_pos + d1_pos + d2_neg;
+ v[10].noalias() = Tr + d0_pos + d1_neg + d2_pos;
+ v[11].noalias() = Tr + d0_pos + d1_neg + d2_neg;
+ v[12].noalias() = Tr + d0_neg + d1_pos + d2_pos;
+ v[13].noalias() = Tr + d0_neg + d1_pos + d2_neg;
+ v[14].noalias() = Tr + d0_neg + d1_neg + d2_pos;
+ v[15].noalias() = Tr + d0_neg + d1_neg + d2_neg;
Matrix3f M; // row first matrix
@@ -1097,14 +1084,14 @@ RSS RSS::operator + (const RSS& other) const
else { mid = 2; }
// column first matrix, as the axis in RSS
- bv.axis[0] << E[0][max], E[1][max], E[2][max];
- bv.axis[1] << E[0][mid], E[1][mid], E[2][mid];
- bv.axis[2] << E[1][max]*E[2][mid] - E[1][mid]*E[2][max],
- E[0][mid]*E[2][max] - E[0][max]*E[2][mid],
- E[0][max]*E[1][mid] - E[0][mid]*E[1][max];
+ bv.axes.col(0) << E[0][max], E[1][max], E[2][max];
+ bv.axes.col(1) << E[0][mid], E[1][mid], E[2][mid];
+ bv.axes.col(2) << E[1][max]*E[2][mid] - E[1][mid]*E[2][max],
+ E[0][mid]*E[2][max] - E[0][max]*E[2][mid],
+ E[0][max]*E[1][mid] - E[0][mid]*E[1][max];
// set rss origin, rectangle size and radius
- getRadiusAndOriginAndRectangleSize(v, NULL, NULL, NULL, 16, bv.axis, bv.Tr, bv.l, bv.r);
+ getRadiusAndOriginAndRectangleSize(v, NULL, NULL, NULL, 16, bv.axes, bv.Tr, bv.l, bv.r);
return bv;
}
@@ -1114,12 +1101,8 @@ FCL_REAL RSS::distance(const RSS& other, Vec3f* P, Vec3f* Q) 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.Tr - Tr; // 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]);
+ Matrix3f R (axes.transpose() * other.axes);
+ Vec3f T (axes.transpose() * (other.Tr - Tr));
FCL_REAL dist = rectDistance(R, T, l, other.l, P, Q);
dist -= (r + other.r);
@@ -1128,19 +1111,10 @@ FCL_REAL RSS::distance(const RSS& other, Vec3f* P, Vec3f* Q) const
FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, Vec3f* P, Vec3f* Q)
{
- 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.Tr + T0 - b1.Tr;
+ Matrix3f R (b1.axes.transpose() * R0 * b2.axes);
+ 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 T(b1.axes.transpose() * Ttemp);
FCL_REAL dist = rectDistance(R, T, b1.l, b2.l, P, Q);
dist -= (b1.r + b2.r);
diff --git a/src/BV/kIOS.cpp b/src/BV/kIOS.cpp
index acc8150dedc78024144d6659c42833b713a4bb06..a482c4d0b150c7dc080bb49a1e25a38416f56fc8 100644
--- a/src/BV/kIOS.cpp
+++ b/src/BV/kIOS.cpp
@@ -183,9 +183,7 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2
b2_temp.obb.To = R0 * b2_temp.obb.To + T0;
- b2_temp.obb.axis[0] = R0 * b2_temp.obb.axis[0];
- b2_temp.obb.axis[1] = R0 * b2_temp.obb.axis[1];
- b2_temp.obb.axis[2] = R0 * b2_temp.obb.axis[2];
+ b2_temp.obb.axes = R0 * b2_temp.obb.axes;
return b1.overlap(b2_temp);
}
diff --git a/src/BVH/BVH_model.cpp b/src/BVH/BVH_model.cpp
index b283bfc4d9469f41ac6865f31f1f62ae349f02e8..ae4e903dac6352801adb3c10429a87d133b72a24 100644
--- a/src/BVH/BVH_model.cpp
+++ b/src/BVH/BVH_model.cpp
@@ -879,69 +879,60 @@ void BVHModel<BV>::computeLocalAABB()
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)
{
OBB& obb = bvs[bv_id].bv;
if(!bvs[bv_id].isLeaf())
{
- makeParentRelativeRecurse(bvs[bv_id].first_child, obb.axis, obb.To);
+ makeParentRelativeRecurse(bvs[bv_id].first_child, obb.axes, obb.To);
- makeParentRelativeRecurse(bvs[bv_id].first_child + 1, obb.axis, obb.To);
+ makeParentRelativeRecurse(bvs[bv_id].first_child + 1, obb.axes, obb.To);
}
// make self parent relative
- obb.axis[0] = Vec3f(parent_axis[0].dot(obb.axis[0]), parent_axis[1].dot(obb.axis[0]), parent_axis[2].dot(obb.axis[0]));
- obb.axis[1] = Vec3f(parent_axis[0].dot(obb.axis[1]), parent_axis[1].dot(obb.axis[1]), parent_axis[2].dot(obb.axis[1]));
- obb.axis[2] = Vec3f(parent_axis[0].dot(obb.axis[2]), parent_axis[1].dot(obb.axis[2]), parent_axis[2].dot(obb.axis[2]));
+ obb.axes = parent_axes.transpose() * obb.axes;
- Vec3f t = obb.To - parent_c;
- obb.To = Vec3f(parent_axis[0].dot(t), parent_axis[1].dot(t), parent_axis[2].dot(t));
+ Vec3f t (obb.To - parent_c);
+ obb.To.noalias() = parent_axes.transpose() * t;
}
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)
{
RSS& rss = bvs[bv_id].bv;
if(!bvs[bv_id].isLeaf())
{
- makeParentRelativeRecurse(bvs[bv_id].first_child, rss.axis, rss.Tr);
+ makeParentRelativeRecurse(bvs[bv_id].first_child, rss.axes, rss.Tr);
- makeParentRelativeRecurse(bvs[bv_id].first_child + 1, rss.axis, rss.Tr);
+ makeParentRelativeRecurse(bvs[bv_id].first_child + 1, rss.axes, rss.Tr);
}
// make self parent relative
- rss.axis[0] = Vec3f(parent_axis[0].dot(rss.axis[0]), parent_axis[1].dot(rss.axis[0]), parent_axis[2].dot(rss.axis[0]));
- rss.axis[1] = Vec3f(parent_axis[0].dot(rss.axis[1]), parent_axis[1].dot(rss.axis[1]), parent_axis[2].dot(rss.axis[1]));
- rss.axis[2] = Vec3f(parent_axis[0].dot(rss.axis[2]), parent_axis[1].dot(rss.axis[2]), parent_axis[2].dot(rss.axis[2]));
+ rss.axes = parent_axes.transpose() * rss.axes;
- Vec3f t = rss.Tr - parent_c;
- rss.Tr = Vec3f(parent_axis[0].dot(t), parent_axis[1].dot(t), parent_axis[2].dot(t));
+ Vec3f t (rss.Tr - parent_c);
+ rss.Tr.noalias() = parent_axes.transpose() * t;
}
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)
{
OBB& obb = bvs[bv_id].bv.obb;
RSS& rss = bvs[bv_id].bv.rss;
if(!bvs[bv_id].isLeaf())
{
- makeParentRelativeRecurse(bvs[bv_id].first_child, obb.axis, obb.To);
+ makeParentRelativeRecurse(bvs[bv_id].first_child, obb.axes, obb.To);
- makeParentRelativeRecurse(bvs[bv_id].first_child + 1, obb.axis, obb.To);
+ makeParentRelativeRecurse(bvs[bv_id].first_child + 1, obb.axes, obb.To);
}
// make self parent relative
- obb.axis[0] = Vec3f(parent_axis[0].dot(obb.axis[0]), parent_axis[1].dot(obb.axis[0]), parent_axis[2].dot(obb.axis[0]));
- obb.axis[1] = Vec3f(parent_axis[0].dot(obb.axis[1]), parent_axis[1].dot(obb.axis[1]), parent_axis[2].dot(obb.axis[1]));
- obb.axis[2] = Vec3f(parent_axis[0].dot(obb.axis[2]), parent_axis[1].dot(obb.axis[2]), parent_axis[2].dot(obb.axis[2]));
+ rss.axes.noalias() = parent_axes.transpose() * obb.axes;
+ obb.axes.noalias() = rss.axes;
- rss.axis[0] = obb.axis[0];
- rss.axis[1] = obb.axis[1];
- rss.axis[2] = obb.axis[2];
-
- Vec3f t = obb.To - parent_c;
- obb.To = Vec3f(parent_axis[0].dot(t), parent_axis[1].dot(t), parent_axis[2].dot(t));
- rss.Tr = obb.To;
+ Vec3f t (obb.To - parent_c);
+ obb.To.noalias() = parent_axes.transpose() *t;
+ rss.Tr.noalias() = obb.To;
}
diff --git a/src/BVH/BVH_utility.cpp b/src/BVH/BVH_utility.cpp
index e06b1a6e68f97debcdb1b22698554ea96afa6908..7316b412e2390557bf2b00b635d1029c5be0e829 100644
--- a/src/BVH/BVH_utility.cpp
+++ b/src/BVH/BVH_utility.cpp
@@ -192,7 +192,7 @@ void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, i
/** \brief Compute the RSS bounding volume parameters: radius, rectangle size and the origin.
* The bounding volume axes are known.
*/
-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)
{
bool indirect_index = true;
if(!indices) indirect_index = false;
@@ -215,9 +215,9 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns
int point_id = t[j];
const Vec3f& p = ps[point_id];
Vec3f v(p[0], p[1], p[2]);
- P[P_id][0] = axis[0].dot(v);
- P[P_id][1] = axis[1].dot(v);
- P[P_id][2] = axis[2].dot(v);
+ P[P_id][0] = axes.col(0).dot(v);
+ P[P_id][1] = axes.col(1).dot(v);
+ P[P_id][2] = axes.col(2).dot(v);
P_id++;
}
@@ -227,10 +227,11 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns
{
int point_id = t[j];
const Vec3f& p = ps2[point_id];
+ // FIXME Is this right ?????
Vec3f v(p[0], p[1], p[2]);
- P[P_id][0] = axis[0].dot(v);
- P[P_id][1] = axis[0].dot(v);
- P[P_id][2] = axis[1].dot(v);
+ P[P_id][0] = axes.col(0).dot(v);
+ P[P_id][1] = axes.col(0).dot(v);
+ P[P_id][2] = axes.col(1).dot(v);
P_id++;
}
}
@@ -244,17 +245,17 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns
const Vec3f& p = ps[index];
Vec3f v(p[0], p[1], p[2]);
- P[P_id][0] = axis[0].dot(v);
- P[P_id][1] = axis[1].dot(v);
- P[P_id][2] = axis[2].dot(v);
+ P[P_id][0] = axes.col(0).dot(v);
+ P[P_id][1] = axes.col(1).dot(v);
+ P[P_id][2] = axes.col(2).dot(v);
P_id++;
if(ps2)
{
const Vec3f& v = ps2[index];
- P[P_id][0] = axis[0].dot(v);
- P[P_id][1] = axis[1].dot(v);
- P[P_id][2] = axis[2].dot(v);
+ P[P_id][0] = axes.col(0).dot(v);
+ P[P_id][1] = axes.col(1).dot(v);
+ P[P_id][2] = axes.col(2).dot(v);
P_id++;
}
}
@@ -457,7 +458,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns
}
}
- origin = axis[0] * minx + axis[1] * miny + axis[2] * cz;
+ origin.noalias() = axes * Vec3f(minx, miny, cz);
l[0] = maxx - minx;
if(l[0] < 0) l[0] = 0;
@@ -472,7 +473,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns
/** \brief Compute the bounding volume extent and center for a set or subset of points.
* The bounding volume axes are known.
*/
-static inline void getExtentAndCenter_pointcloud(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, Vec3f axis[3], Vec3f& center, Vec3f& extent)
+static inline void getExtentAndCenter_pointcloud(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, Matrix3f& axes, Vec3f& center, Vec3f& extent)
{
bool indirect_index = true;
if(!indices) indirect_index = false;
@@ -488,10 +489,7 @@ static inline void getExtentAndCenter_pointcloud(Vec3f* ps, Vec3f* ps2, unsigned
const Vec3f& p = ps[index];
Vec3f v(p[0], p[1], p[2]);
- FCL_REAL proj[3];
- proj[0] = axis[0].dot(v);
- proj[1] = axis[1].dot(v);
- proj[2] = axis[2].dot(v);
+ Vec3f proj (axes.transpose() * v);
for(int j = 0; j < 3; ++j)
{
@@ -502,9 +500,7 @@ static inline void getExtentAndCenter_pointcloud(Vec3f* ps, Vec3f* ps2, unsigned
if(ps2)
{
const Vec3f& v = ps2[index];
- proj[0] = axis[0].dot(v);
- proj[1] = axis[1].dot(v);
- proj[2] = axis[2].dot(v);
+ proj.noalias() = axes.transpose() * v;
for(int j = 0; j < 3; ++j)
{
@@ -518,7 +514,7 @@ static inline void getExtentAndCenter_pointcloud(Vec3f* ps, Vec3f* ps2, unsigned
(max_coord[1] + min_coord[1]) / 2,
(max_coord[2] + min_coord[2]) / 2);
- center = axis[0] * o[0] + axis[1] * o[1] + axis[2] * o[2];
+ center.noalias() = axes * o;
extent << (max_coord[0] - min_coord[0]) / 2,
(max_coord[1] - min_coord[1]) / 2,
@@ -530,7 +526,7 @@ static inline void getExtentAndCenter_pointcloud(Vec3f* ps, Vec3f* ps2, unsigned
/** \brief Compute the bounding volume extent and center for a set or subset of points.
* The bounding volume axes are known.
*/
-static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& center, Vec3f& extent)
+static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Matrix3f& axes, Vec3f& center, Vec3f& extent)
{
bool indirect_index = true;
if(!indices) indirect_index = false;
@@ -550,10 +546,7 @@ static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts,
int point_id = t[j];
const Vec3f& p = ps[point_id];
Vec3f v(p[0], p[1], p[2]);
- FCL_REAL proj[3];
- proj[0] = axis[0].dot(v);
- proj[1] = axis[1].dot(v);
- proj[2] = axis[2].dot(v);
+ Vec3f proj (axes.transpose() * v);
for(int k = 0; k < 3; ++k)
{
@@ -569,10 +562,7 @@ static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts,
int point_id = t[j];
const Vec3f& p = ps2[point_id];
Vec3f v(p[0], p[1], p[2]);
- FCL_REAL proj[3];
- proj[0] = axis[0].dot(v);
- proj[1] = axis[1].dot(v);
- proj[2] = axis[2].dot(v);
+ Vec3f proj (axes.transpose() * v);
for(int k = 0; k < 3; ++k)
{
@@ -587,7 +577,7 @@ static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts,
(max_coord[1] + min_coord[1]) / 2,
(max_coord[2] + min_coord[2]) / 2);
- center = axis[0] * o[0] + axis[1] * o[1] + axis[2] * o[2];
+ center.noalias() = axes * o;
extent << (max_coord[0] - min_coord[0]) / 2,
(max_coord[1] - min_coord[1]) / 2,
@@ -595,12 +585,12 @@ static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts,
}
-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)
{
if(ts)
- getExtentAndCenter_mesh(ps, ps2, ts, indices, n, axis, center, extent);
+ getExtentAndCenter_mesh(ps, ps2, ts, indices, n, axes, center, extent);
else
- getExtentAndCenter_pointcloud(ps, ps2, indices, n, axis, center, extent);
+ getExtentAndCenter_pointcloud(ps, ps2, indices, n, axes, center, extent);
}
void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, Vec3f& center, FCL_REAL& radius)
diff --git a/src/BVH/BV_fitter.cpp b/src/BVH/BV_fitter.cpp
index 548bc4f4b0b4053238038a6736b46b5bec9cc0e8..a303d8a5ba9a93aba8ede5ebda50065032f9d57b 100644
--- a/src/BVH/BV_fitter.cpp
+++ b/src/BVH/BV_fitter.cpp
@@ -49,7 +49,7 @@ static const double invCosA = 2.0 / sqrt(3.0);
static const double sinA = 0.5;
static const double cosA = sqrt(3.0) / 2.0;
-static inline void axisFromEigen(Vec3f eigenV[3], Matrix3f::Scalar eigenS[3], Vec3f axis[3])
+static inline void axisFromEigen(Vec3f eigenV[3], Matrix3f::Scalar eigenS[3], Matrix3f& axes)
{
int min, mid, max;
if(eigenS[0] > eigenS[1]) { max = 0; min = 1; }
@@ -58,11 +58,11 @@ static inline void axisFromEigen(Vec3f eigenV[3], Matrix3f::Scalar eigenS[3], Ve
else if(eigenS[2] > eigenS[max]) { mid = max; max = 2; }
else { mid = 2; }
- axis[0] << eigenV[0][max], eigenV[1][max], eigenV[2][max];
- axis[1] << eigenV[0][mid], eigenV[1][mid], eigenV[2][mid];
- axis[2] << eigenV[1][max]*eigenV[2][mid] - eigenV[1][mid]*eigenV[2][max],
- eigenV[0][mid]*eigenV[2][max] - eigenV[0][max]*eigenV[2][mid],
- eigenV[0][max]*eigenV[1][mid] - eigenV[0][mid]*eigenV[1][max];
+ axes.col(0) << eigenV[0][max], eigenV[1][max], eigenV[2][max];
+ axes.col(1) << eigenV[0][mid], eigenV[1][mid], eigenV[2][mid];
+ axes.col(2) << eigenV[1][max]*eigenV[2][mid] - eigenV[1][mid]*eigenV[2][max],
+ eigenV[0][mid]*eigenV[2][max] - eigenV[0][max]*eigenV[2][mid],
+ eigenV[0][max]*eigenV[1][mid] - eigenV[0][mid]*eigenV[1][max];
}
namespace OBB_fit_functions
@@ -70,10 +70,8 @@ namespace OBB_fit_functions
void fit1(Vec3f* ps, OBB& bv)
{
- bv.To = ps[0];
- bv.axis[0] << 1, 0, 0;
- bv.axis[1] << 0, 1, 0;
- bv.axis[2] << 0, 0, 1;
+ bv.To.noalias() = ps[0];
+ bv.axes.setIdentity();
bv.extent.setZero();
}
@@ -85,13 +83,11 @@ void fit2(Vec3f* ps, OBB& bv)
FCL_REAL len_p1p2 = p1p2.norm();
p1p2.normalize();
- bv.axis[0] = p1p2;
- generateCoordinateSystem(bv.axis[0], bv.axis[1], bv.axis[2]);
+ bv.axes.col(0).noalias() = p1p2;
+ generateCoordinateSystem(bv.axes.col(0), bv.axes.col(1), bv.axes.col(2));
bv.extent << len_p1p2 * 0.5, 0, 0;
- bv.To << 0.5 * (p1[0] + p2[0]),
- 0.5 * (p1[1] + p2[1]),
- 0.5 * (p1[2] + p2[2]);
+ bv.To.noalias() = (p1 + p2) / 2;
}
void fit3(Vec3f* ps, OBB& bv)
@@ -112,17 +108,11 @@ void fit3(Vec3f* ps, OBB& bv)
if(len[1] > len[0]) imax = 1;
if(len[2] > len[imax]) imax = 2;
- Vec3f& u = bv.axis[0];
- Vec3f& v = bv.axis[1];
- Vec3f& w = bv.axis[2];
+ bv.axes.col(2).noalias() = e[0].cross(e[1]).normalized();
+ bv.axes.col(0).noalias() = e[imax].normalized();
+ bv.axes.col(1).noalias() = bv.axes.col(2).cross(bv.axes.col(0));
- w = e[0].cross(e[1]);
- w.normalize();
- u = e[imax];
- u.normalize();
- v = w.cross(u);
-
- getExtentAndCenter(ps, NULL, NULL, NULL, 3, bv.axis, bv.To, bv.extent);
+ getExtentAndCenter(ps, NULL, NULL, NULL, 3, bv.axes, bv.To, bv.extent);
}
void fit6(Vec3f* ps, OBB& bv)
@@ -142,10 +132,10 @@ void fitn(Vec3f* ps, int n, OBB& bv)
getCovariance(ps, NULL, NULL, NULL, n, M);
eigen(M, s, E);
- axisFromEigen(E, s, bv.axis);
+ axisFromEigen(E, s, bv.axes);
// set obb centers and extensions
- getExtentAndCenter(ps, NULL, NULL, NULL, n, bv.axis, bv.To, bv.extent);
+ getExtentAndCenter(ps, NULL, NULL, NULL, n, bv.axes, bv.To, bv.extent);
}
}
@@ -155,10 +145,8 @@ namespace RSS_fit_functions
{
void fit1(Vec3f* ps, RSS& bv)
{
- bv.Tr = ps[0];
- bv.axis[0] << 1, 0, 0;
- bv.axis[1] << 0, 1, 0;
- bv.axis[2] << 0, 0, 1;
+ bv.Tr.noalias() = ps[0];
+ bv.axes.setIdentity();
bv.l[0] = 0;
bv.l[1] = 0;
bv.r = 0;
@@ -172,8 +160,8 @@ void fit2(Vec3f* ps, RSS& bv)
FCL_REAL len_p1p2 = p1p2.norm();
p1p2.normalize();
- bv.axis[0] = p1p2;
- generateCoordinateSystem(bv.axis[0], bv.axis[1], bv.axis[2]);
+ bv.axes.col(0) = p1p2;
+ generateCoordinateSystem(bv.axes.col(0), bv.axes.col(1), bv.axes.col(2));
bv.l[0] = len_p1p2;
bv.l[1] = 0;
@@ -199,17 +187,11 @@ void fit3(Vec3f* ps, RSS& bv)
if(len[1] > len[0]) imax = 1;
if(len[2] > len[imax]) imax = 2;
- Vec3f& u = bv.axis[0];
- Vec3f& v = bv.axis[1];
- Vec3f& w = bv.axis[2];
-
- w = e[0].cross(e[1]);
- w.normalize();
- u = e[imax];
- u.normalize();
- v = w.cross(u);
+ bv.axes.col(2).noalias() = e[0].cross(e[1]).normalized();
+ bv.axes.col(0).noalias() = e[imax].normalized();
+ bv.axes.col(1).noalias() = bv.axes.col(2).cross(bv.axes.col(0));
- getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, 3, bv.axis, bv.Tr, bv.l, bv.r);
+ getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, 3, bv.axes, bv.Tr, bv.l, bv.r);
}
void fit6(Vec3f* ps, RSS& bv)
@@ -228,10 +210,10 @@ void fitn(Vec3f* ps, int n, RSS& bv)
getCovariance(ps, NULL, NULL, NULL, n, M);
eigen(M, s, E);
- axisFromEigen(E, s, bv.axis);
+ axisFromEigen(E, s, bv.axes);
// set rss origin, rectangle size and radius
- getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, n, bv.axis, bv.Tr, bv.l, bv.r);
+ getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, n, bv.axes, bv.Tr, bv.l, bv.r);
}
}
@@ -242,14 +224,12 @@ namespace kIOS_fit_functions
void fit1(Vec3f* ps, kIOS& bv)
{
bv.num_spheres = 1;
- bv.spheres[0].o = ps[0];
+ bv.spheres[0].o.noalias() = ps[0];
bv.spheres[0].r = 0;
- bv.obb.axis[0] << 1, 0, 0;
- bv.obb.axis[1] << 0, 1, 0;
- bv.obb.axis[2] << 0, 0, 1;
+ bv.obb.axes.setIdentity();
bv.obb.extent.setZero();
- bv.obb.To = ps[0];
+ bv.obb.To.noalias() = ps[0];
}
void fit2(Vec3f* ps, kIOS& bv)
@@ -262,9 +242,9 @@ void fit2(Vec3f* ps, kIOS& bv)
FCL_REAL len_p1p2 = p1p2.norm();
p1p2.normalize();
- Vec3f* axis = bv.obb.axis;
- axis[0] = p1p2;
- generateCoordinateSystem(axis[0], axis[1], axis[2]);
+ Matrix3f& axes = bv.obb.axes;
+ axes.col(0).noalias() = p1p2;
+ generateCoordinateSystem(axes.col(0), axes.col(1), axes.col(2));
FCL_REAL r0 = len_p1p2 * 0.5;
bv.obb.extent << r0, 0, 0;
@@ -277,13 +257,13 @@ void fit2(Vec3f* ps, kIOS& bv)
FCL_REAL r1cosA = r1 * cosA;
bv.spheres[1].r = r1;
bv.spheres[2].r = r1;
- Vec3f delta = axis[1] * r1cosA;
+ Vec3f delta = axes.col(1) * r1cosA;
bv.spheres[1].o = bv.spheres[0].o - delta;
bv.spheres[2].o = bv.spheres[0].o + delta;
bv.spheres[3].r = r1;
bv.spheres[4].r = r1;
- delta = axis[2] * r1cosA;
+ delta = axes.col(2) * r1cosA;
bv.spheres[3].o = bv.spheres[0].o - delta;
bv.spheres[4].o = bv.spheres[0].o + delta;
}
@@ -308,17 +288,11 @@ void fit3(Vec3f* ps, kIOS& bv)
if(len[1] > len[0]) imax = 1;
if(len[2] > len[imax]) imax = 2;
- Vec3f& u = bv.obb.axis[0];
- Vec3f& v = bv.obb.axis[1];
- Vec3f& w = bv.obb.axis[2];
-
- w = e[0].cross(e[1]);
- w.normalize();
- u = e[imax];
- u.normalize();
- v = w.cross(u);
+ bv.obb.axes.col(2).noalias() = e[0].cross(e[1]).normalized();
+ bv.obb.axes.col(0).noalias() = e[imax].normalized();
+ bv.obb.axes.col(1).noalias() = bv.obb.axes.col(2).cross(bv.obb.axes.col(0));
- getExtentAndCenter(ps, NULL, NULL, NULL, 3, bv.obb.axis, bv.obb.To, bv.obb.extent);
+ getExtentAndCenter(ps, NULL, NULL, NULL, 3, bv.obb.axes, bv.obb.To, bv.obb.extent);
// compute radius and center
FCL_REAL r0;
@@ -329,7 +303,7 @@ void fit3(Vec3f* ps, kIOS& bv)
bv.spheres[0].r = r0;
FCL_REAL r1 = r0 * invSinA;
- Vec3f delta = bv.obb.axis[2] * (r1 * cosA);
+ Vec3f delta = bv.obb.axes.col(2) * (r1 * cosA);
bv.spheres[1].r = r1;
bv.spheres[1].o = center - delta;
@@ -346,10 +320,10 @@ void fitn(Vec3f* ps, int n, kIOS& bv)
getCovariance(ps, NULL, NULL, NULL, n, M);
eigen(M, s, E);
- Vec3f* axis = bv.obb.axis;
- axisFromEigen(E, s, axis);
+ Matrix3f& axes = bv.obb.axes;
+ axisFromEigen(E, s, axes);
- getExtentAndCenter(ps, NULL, NULL, NULL, n, axis, bv.obb.To, bv.obb.extent);
+ getExtentAndCenter(ps, NULL, NULL, NULL, n, axes, bv.obb.To, bv.obb.extent);
// get center and extension
const Vec3f& center = bv.obb.To;
@@ -371,15 +345,15 @@ void fitn(Vec3f* ps, int n, kIOS& bv)
if(bv.num_spheres >= 3)
{
FCL_REAL r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA;
- Vec3f delta = axis[2] * (r10 * cosA - extent[2]);
+ Vec3f delta = axes.col(2) * (r10 * cosA - extent[2]);
bv.spheres[1].o = center - delta;
bv.spheres[2].o = center + delta;
FCL_REAL r11 = 0, r12 = 0;
r11 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[1].o);
r12 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[2].o);
- bv.spheres[1].o += axis[2] * (-r10 + r11);
- bv.spheres[2].o += axis[2] * (r10 - r12);
+ bv.spheres[1].o += axes.col(2) * (-r10 + r11);
+ bv.spheres[2].o += axes.col(2) * (r10 - r12);
bv.spheres[1].r = r10;
bv.spheres[2].r = r10;
@@ -388,7 +362,7 @@ void fitn(Vec3f* ps, int n, kIOS& bv)
if(bv.num_spheres >= 5)
{
FCL_REAL r10 = bv.spheres[1].r;
- Vec3f delta = axis[1] * (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - extent[1]);
+ Vec3f delta = axes.col(1) * (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - extent[1]);
bv.spheres[3].o = bv.spheres[0].o - delta;
bv.spheres[4].o = bv.spheres[0].o + delta;
@@ -396,8 +370,8 @@ void fitn(Vec3f* ps, int n, kIOS& bv)
r21 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[3].o);
r22 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[4].o);
- bv.spheres[3].o += axis[1] * (-r10 + r21);
- bv.spheres[4].o += axis[1] * (r10 - r22);
+ bv.spheres[3].o += axes.col(1) * (-r10 + r21);
+ bv.spheres[4].o += axes.col(1) * (r10 - r22);
bv.spheres[3].r = r10;
bv.spheres[4].r = r10;
@@ -528,10 +502,10 @@ OBB BVFitter<OBB>::fit(unsigned int* primitive_indices, int num_primitives)
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
eigen(M, s, E);
- axisFromEigen(E, s, bv.axis);
+ axisFromEigen(E, s, bv.axes);
// set obb centers and extensions
- getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.axis, bv.To, bv.extent);
+ getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.axes, bv.To, bv.extent);
return bv;
}
@@ -546,17 +520,15 @@ OBBRSS BVFitter<OBBRSS>::fit(unsigned int* primitive_indices, int num_primitives
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
eigen(M, s, E);
- axisFromEigen(E, s, bv.obb.axis);
- bv.rss.axis[0] = bv.obb.axis[0];
- bv.rss.axis[1] = bv.obb.axis[1];
- bv.rss.axis[2] = bv.obb.axis[2];
+ axisFromEigen(E, s, bv.obb.axes);
+ bv.rss.axes.noalias() = bv.obb.axes;
- getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.obb.axis, bv.obb.To, bv.obb.extent);
+ getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.obb.axes, bv.obb.To, bv.obb.extent);
Vec3f origin;
FCL_REAL l[2];
FCL_REAL r;
- getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.rss.axis, origin, l, r);
+ getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.rss.axes, origin, l, r);
bv.rss.Tr = origin;
bv.rss.l[0] = l[0];
@@ -575,14 +547,14 @@ RSS BVFitter<RSS>::fit(unsigned int* primitive_indices, int num_primitives)
Matrix3f::Scalar s[3]; // three eigen values
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
eigen(M, s, E);
- axisFromEigen(E, s, bv.axis);
+ axisFromEigen(E, s, bv.axes);
// set rss origin, rectangle size and radius
Vec3f origin;
FCL_REAL l[2];
FCL_REAL r;
- getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.axis, origin, l, r);
+ getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.axes, origin, l, r);
bv.Tr = origin;
bv.l[0] = l[0];
@@ -605,11 +577,11 @@ kIOS BVFitter<kIOS>::fit(unsigned int* primitive_indices, int num_primitives)
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
eigen(M, s, E);
- Vec3f* axis = bv.obb.axis;
- axisFromEigen(E, s, axis);
+ Matrix3f& axes = bv.obb.axes;
+ axisFromEigen(E, s, axes);
// get centers and extensions
- getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, axis, bv.obb.To, bv.obb.extent);
+ getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, axes, bv.obb.To, bv.obb.extent);
const Vec3f& center = bv.obb.To;
const Vec3f& extent = bv.obb.extent;
@@ -629,15 +601,15 @@ kIOS BVFitter<kIOS>::fit(unsigned int* primitive_indices, int num_primitives)
if(bv.num_spheres >= 3)
{
FCL_REAL r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA;
- Vec3f delta = axis[2] * (r10 * cosA - extent[2]);
+ Vec3f delta = axes.col(2) * (r10 * cosA - extent[2]);
bv.spheres[1].o = center - delta;
bv.spheres[2].o = center + delta;
FCL_REAL r11 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[1].o);
FCL_REAL r12 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[2].o);
- bv.spheres[1].o += axis[2] * (-r10 + r11);
- bv.spheres[2].o += axis[2] * (r10 - r12);
+ bv.spheres[1].o += axes.col(2) * (-r10 + r11);
+ bv.spheres[2].o += axes.col(2) * (r10 - r12);
bv.spheres[1].r = r10;
bv.spheres[2].r = r10;
@@ -646,7 +618,7 @@ kIOS BVFitter<kIOS>::fit(unsigned int* primitive_indices, int num_primitives)
if(bv.num_spheres >= 5)
{
FCL_REAL r10 = bv.spheres[1].r;
- Vec3f delta = axis[1] * (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - extent[1]);
+ Vec3f delta = axes.col(1) * (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - extent[1]);
bv.spheres[3].o = bv.spheres[0].o - delta;
bv.spheres[4].o = bv.spheres[0].o + delta;
@@ -654,8 +626,8 @@ kIOS BVFitter<kIOS>::fit(unsigned int* primitive_indices, int num_primitives)
r21 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[3].o);
r22 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[4].o);
- bv.spheres[3].o += axis[1] * (-r10 + r21);
- bv.spheres[4].o += axis[1] * (r10 - r22);
+ bv.spheres[3].o += axes.col(1) * (-r10 + r21);
+ bv.spheres[4].o += axes.col(1) * (r10 - r22);
bv.spheres[3].r = r10;
bv.spheres[4].r = r10;
diff --git a/src/BVH/BV_splitter.cpp b/src/BVH/BV_splitter.cpp
index a7f53f48d9c1ba567aa9575134d633578e722b37..1cce58f1ac10896ffa10858ba8e79568db920106 100644
--- a/src/BVH/BV_splitter.cpp
+++ b/src/BVH/BV_splitter.cpp
@@ -44,7 +44,7 @@ namespace fcl
template<typename BV>
void computeSplitVector(const BV& bv, Vec3f& split_vector)
{
- split_vector = bv.axis[0];
+ split_vector.noalias() = bv.axes.col(0);
}
template<>
@@ -78,13 +78,13 @@ void computeSplitVector<kIOS>(const kIOS& bv, Vec3f& split_vector)
;
}
*/
- split_vector = bv.obb.axis[0];
+ split_vector.noalias() = bv.obb.axes.col(0);
}
template<>
void computeSplitVector<OBBRSS>(const OBBRSS& bv, Vec3f& split_vector)
{
- split_vector = bv.obb.axis[0];
+ split_vector.noalias() = bv.obb.axes.col(0);
}
template<typename BV>
diff --git a/src/broadphase/broadphase_SSaP.cpp b/src/broadphase/broadphase_SSaP.cpp
index 4a7cee1497818907623386f466f5f6578e59dba9..5595bde03f195d3cd1431abba66e8445c09158ce 100644
--- a/src/broadphase/broadphase_SSaP.cpp
+++ b/src/broadphase/broadphase_SSaP.cpp
@@ -354,7 +354,7 @@ bool SSaPCollisionManager::distance_(CollisionObject* obj, void* cdata, Distance
else // need more loop
{
if(isEqual(dummy_vector, obj->getAABB().max_))
- dummy_vector = dummy_vector + delta;
+ dummy_vector.noalias() += delta;
else
dummy_vector = dummy_vector * 2 - obj->getAABB().max_;
}
diff --git a/src/ccd/motion.cpp b/src/ccd/motion.cpp
index 58d8792c57886ee368d9ace41fc1298074102c5a..554131ae6b2d8e5fefce95db18b7af2c7d1e667c 100644
--- a/src/ccd/motion.cpp
+++ b/src/ccd/motion.cpp
@@ -48,9 +48,9 @@ FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const SplineMotion& motion) const
FCL_REAL tf_t = motion.getCurrentTime();
Vec3f c1 = bv.Tr;
- Vec3f c2 = bv.Tr + bv.axis[0] * bv.l[0];
- Vec3f c3 = bv.Tr + bv.axis[1] * bv.l[1];
- Vec3f c4 = bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1];
+ Vec3f c2 = bv.Tr + bv.axes.col(0) * bv.l[0];
+ Vec3f c3 = bv.Tr + bv.axes.col(1) * bv.l[1];
+ Vec3f c4 = bv.Tr + bv.axes.col(0) * bv.l[0] + bv.axes.col(1) * bv.l[1];
FCL_REAL tmp;
// max_i |c_i * n|
@@ -327,11 +327,11 @@ FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const ScrewMotion& motion) const
FCL_REAL c_proj_max = ((tf.getQuatRotation().transform(bv.Tr)).cross(axis)).squaredNorm();
FCL_REAL tmp;
- tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0])).cross(axis)).squaredNorm();
+ tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axes.col(0) * bv.l[0])).cross(axis)).squaredNorm();
if(tmp > c_proj_max) c_proj_max = tmp;
- tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[1] * bv.l[1])).cross(axis)).squaredNorm();
+ tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axes.col(1) * bv.l[1])).cross(axis)).squaredNorm();
if(tmp > c_proj_max) c_proj_max = tmp;
- tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1])).cross(axis)).squaredNorm();
+ tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axes.col(0) * bv.l[0] + bv.axes.col(1) * bv.l[1])).cross(axis)).squaredNorm();
if(tmp > c_proj_max) c_proj_max = tmp;
c_proj_max = sqrt(c_proj_max);
@@ -392,11 +392,11 @@ FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const InterpMotion& motion) const
FCL_REAL c_proj_max = ((tf.getQuatRotation().transform(bv.Tr - reference_p)).cross(angular_axis)).squaredNorm();
FCL_REAL tmp;
- tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] - reference_p)).cross(angular_axis)).squaredNorm();
+ tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axes.col(0) * bv.l[0] - reference_p)).cross(angular_axis)).squaredNorm();
if(tmp > c_proj_max) c_proj_max = tmp;
- tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[1] * bv.l[1] - reference_p)).cross(angular_axis)).squaredNorm();
+ tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axes.col(1) * bv.l[1] - reference_p)).cross(angular_axis)).squaredNorm();
if(tmp > c_proj_max) c_proj_max = tmp;
- tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1] - reference_p)).cross(angular_axis)).squaredNorm();
+ tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axes.col(0) * bv.l[0] + bv.axes.col(1) * bv.l[1] - reference_p)).cross(angular_axis)).squaredNorm();
if(tmp > c_proj_max) c_proj_max = tmp;
c_proj_max = std::sqrt(c_proj_max);
diff --git a/src/math/transform.cpp b/src/math/transform.cpp
index c5f406d2a8a01229e62e5750814bdb02ffd2c205..279e0c50ec75928d59eaf1d9acfd0469eceace43 100644
--- a/src/math/transform.cpp
+++ b/src/math/transform.cpp
@@ -108,15 +108,14 @@ void Quaternion3f::toRotation(Matrix3f& R) const
twoXZ - twoWY, twoYZ + twoWX, 1.0 - (twoXX + twoYY);
}
-
-void Quaternion3f::fromAxes(const Vec3f axis[3])
+void Quaternion3f::fromAxes(const Matrix3f& axes)
{
// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
// article "Quaternion Calculus and Fast Animation".
const int next[3] = {1, 2, 0};
- FCL_REAL trace = axis[0][0] + axis[1][1] + axis[2][2];
+ FCL_REAL trace = axes.trace();
FCL_REAL root;
if(trace > 0.0)
@@ -125,36 +124,36 @@ void Quaternion3f::fromAxes(const Vec3f axis[3])
root = sqrt(trace + 1.0); // 2w
data[0] = 0.5 * root;
root = 0.5 / root; // 1/(4w)
- data[1] = (axis[1][2] - axis[2][1])*root;
- data[2] = (axis[2][0] - axis[0][2])*root;
- data[3] = (axis[0][1] - axis[1][0])*root;
+ data[1] = (axes(1,2) - axes(2,1))*root;
+ data[2] = (axes(2,0) - axes(0,2))*root;
+ data[3] = (axes(0,1) - axes(1,0))*root;
}
else
{
// |w| <= 1/2
int i = 0;
- if(axis[1][1] > axis[0][0])
+ if(axes(1,1) > axes(0,0))
{
i = 1;
}
- if(axis[2][2] > axis[i][i])
+ if(axes(2,2) > axes(i,i))
{
i = 2;
}
int j = next[i];
int k = next[j];
- root = sqrt(axis[i][i] - axis[j][j] - axis[k][k] + 1.0);
+ root = sqrt(axes(i,i) - axes(j,j) - axes(k,k) + 1.0);
FCL_REAL* quat[3] = { &data[1], &data[2], &data[3] };
*quat[i] = 0.5 * root;
root = 0.5 / root;
- data[0] = (axis[j][k] - axis[k][j]) * root;
- *quat[j] = (axis[i][j] + axis[j][i]) * root;
- *quat[k] = (axis[i][k] + axis[k][i]) * root;
+ data[0] = (axes(j,k) - axes(k,j)) * root;
+ *quat[j] = (axes(i,j) + axes(j,i)) * root;
+ *quat[k] = (axes(i,k) + axes(k,i)) * root;
}
}
-void Quaternion3f::toAxes(Vec3f axis[3]) const
+void Quaternion3f::toAxes(Matrix3f& axes) const
{
FCL_REAL twoX = 2.0*data[1];
FCL_REAL twoY = 2.0*data[2];
@@ -169,9 +168,9 @@ void Quaternion3f::toAxes(Vec3f axis[3]) const
FCL_REAL twoYZ = twoZ*data[2];
FCL_REAL twoZZ = twoZ*data[3];
- axis[0] << 1.0 - (twoYY + twoZZ), twoXY + twoWZ, twoXZ - twoWY;
- axis[1] << twoXY - twoWZ, 1.0 - (twoXX + twoZZ), twoYZ + twoWX;
- axis[2] << twoXZ + twoWY, twoYZ - twoWX, 1.0 - (twoXX + twoYY);
+ axes << 1.0 - (twoYY + twoZZ), twoXY + twoWZ, twoXZ - twoWY,
+ twoXY - twoWZ, 1.0 - (twoXX + twoZZ), twoYZ + twoWX,
+ twoXZ + twoWY, twoYZ - twoWX, 1.0 - (twoXX + twoYY);
}
diff --git a/src/shape/geometric_shapes_utility.cpp b/src/shape/geometric_shapes_utility.cpp
index 058a59088c3f1834e9ffa4acb4c444f6b407e642..97d0a8d5c02a3e4dcff8b8e2f45b1c21166bb5ab 100644
--- a/src/shape/geometric_shapes_utility.cpp
+++ b/src/shape/geometric_shapes_utility.cpp
@@ -412,9 +412,7 @@ void computeBV<OBB, Box>(const Box& s, const Transform3f& tf, OBB& bv)
const Vec3f& T = tf.getTranslation();
bv.To = T;
- bv.axis[0] = R.col(0);
- bv.axis[1] = R.col(1);
- bv.axis[2] = R.col(2);
+ bv.axes.noalias() = R;
bv.extent = s.side * (FCL_REAL)0.5;
}
@@ -424,9 +422,7 @@ void computeBV<OBB, Sphere>(const Sphere& s, const Transform3f& tf, OBB& bv)
const Vec3f& T = tf.getTranslation();
bv.To = T;
- bv.axis[0] << 1, 0, 0;
- bv.axis[1] << 0, 1, 0;
- bv.axis[2] << 0, 0, 1;
+ bv.axes.setIdentity();
bv.extent.setConstant(s.radius);
}
@@ -437,9 +433,7 @@ void computeBV<OBB, Capsule>(const Capsule& s, const Transform3f& tf, OBB& bv)
const Vec3f& T = tf.getTranslation();
bv.To = T;
- bv.axis[0] = R.col(0);
- bv.axis[1] = R.col(1);
- bv.axis[2] = R.col(2);
+ bv.axes.noalias() = R;
bv.extent << s.radius, s.radius, s.lz / 2 + s.radius;
}
@@ -450,9 +444,7 @@ void computeBV<OBB, Cone>(const Cone& s, const Transform3f& tf, OBB& bv)
const Vec3f& T = tf.getTranslation();
bv.To = T;
- bv.axis[0] = R.col(0);
- bv.axis[1] = R.col(1);
- bv.axis[2] = R.col(2);
+ bv.axes.noalias() = R;
bv.extent << s.radius, s.radius, s.lz / 2;
}
@@ -463,9 +455,7 @@ void computeBV<OBB, Cylinder>(const Cylinder& s, const Transform3f& tf, OBB& bv)
const Vec3f& T = tf.getTranslation();
bv.To = T;
- bv.axis[0] = R.col(0);
- bv.axis[1] = R.col(1);
- bv.axis[2] = R.col(2);
+ bv.axes.noalias() = R;
bv.extent << s.radius, s.radius, s.lz / 2;
}
@@ -477,9 +467,7 @@ void computeBV<OBB, Convex>(const Convex& s, const Transform3f& tf, OBB& bv)
fit(s.points, s.num_points, bv);
- bv.axis[0] = R * bv.axis[0];
- bv.axis[1] = R * bv.axis[1];
- bv.axis[2] = R * bv.axis[2];
+ bv.axes = R * bv.axes;
bv.To = R * bv.To + T;
}
@@ -488,10 +476,8 @@ template<>
void computeBV<OBB, Halfspace>(const Halfspace& s, const Transform3f& tf, OBB& bv)
{
/// Half space can only have very rough OBB
- bv.axis[0] = Vec3f(1, 0, 0);
- bv.axis[1] = Vec3f(0, 1, 0);
- bv.axis[2] = Vec3f(0, 0, 1);
- bv.To = Vec3f(0, 0, 0);
+ bv.axes.setIdentity();
+ bv.To.setZero();
bv.extent.setConstant(std::numeric_limits<FCL_REAL>::max());
}
@@ -499,10 +485,8 @@ template<>
void computeBV<RSS, Halfspace>(const Halfspace& s, const Transform3f& tf, RSS& bv)
{
/// Half space can only have very rough RSS
- bv.axis[0] = Vec3f(1, 0, 0);
- bv.axis[1] = Vec3f(0, 1, 0);
- bv.axis[2] = Vec3f(0, 0, 1);
- bv.Tr = Vec3f(0, 0, 0);
+ bv.axes.setIdentity();
+ bv.Tr.setZero();
bv.l[0] = bv.l[1] = bv.r = std::numeric_limits<FCL_REAL>::max();
}
@@ -720,8 +704,8 @@ template<>
void computeBV<OBB, Plane>(const Plane& s, const Transform3f& tf, OBB& bv)
{
Vec3f n = tf.getQuatRotation().transform(s.n);
- generateCoordinateSystem(n, bv.axis[1], bv.axis[2]);
- bv.axis[0] = n;
+ generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2));
+ bv.axes.col(0).noalias() = n;
bv.extent << 0, std::numeric_limits<FCL_REAL>::max(), std::numeric_limits<FCL_REAL>::max();
@@ -734,8 +718,8 @@ void computeBV<RSS, Plane>(const Plane& s, const Transform3f& tf, RSS& bv)
{
Vec3f n = tf.getQuatRotation().transform(s.n);
- generateCoordinateSystem(n, bv.axis[1], bv.axis[2]);
- bv.axis[0] = n;
+ generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2));
+ bv.axes.col(0).noalias() = n;
bv.l[0] = std::numeric_limits<FCL_REAL>::max();
bv.l[1] = std::numeric_limits<FCL_REAL>::max();
@@ -945,33 +929,25 @@ void constructBox(const AABB& bv, Box& box, Transform3f& tf)
void constructBox(const OBB& bv, Box& box, Transform3f& tf)
{
box = Box(bv.extent * 2);
- tf = Transform3f((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(), bv.To);
+ tf = Transform3f(bv.axes, bv.To);
}
void constructBox(const OBBRSS& bv, Box& box, Transform3f& tf)
{
box = Box(bv.obb.extent * 2);
- tf = Transform3f((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(), bv.obb.To);
+ tf = Transform3f(bv.obb.axes, bv.obb.To);
}
void constructBox(const kIOS& bv, Box& box, Transform3f& tf)
{
box = Box(bv.obb.extent * 2);
- tf = Transform3f((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(), bv.obb.To);
+ tf = Transform3f(bv.obb.axes, bv.obb.To);
}
void constructBox(const RSS& bv, Box& box, Transform3f& tf)
{
box = Box(bv.width(), bv.height(), bv.depth());
- tf = Transform3f((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(), bv.Tr);
+ tf = Transform3f(bv.axes, bv.Tr);
}
void constructBox(const KDOP<16>& bv, Box& box, Transform3f& tf)
@@ -1003,33 +979,25 @@ void constructBox(const AABB& bv, const Transform3f& tf_bv, Box& box, Transform3
void constructBox(const OBB& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
{
box = Box(bv.extent * 2);
- tf = tf_bv *Transform3f((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(), bv.To);
+ tf = tf_bv * Transform3f(bv.axes, bv.To);
}
void constructBox(const OBBRSS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
{
box = Box(bv.obb.extent * 2);
- tf = tf_bv * Transform3f((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(), bv.obb.To);
+ tf = tf_bv * Transform3f(bv.obb.axes, bv.obb.To);
}
void constructBox(const kIOS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
{
box = Box(bv.obb.extent * 2);
- tf = tf_bv * Transform3f((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(), bv.obb.To);
+ tf = tf_bv * Transform3f(bv.obb.axes, bv.obb.To);
}
void constructBox(const RSS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
{
box = Box(bv.width(), bv.height(), bv.depth());
- tf = tf_bv * Transform3f((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(), bv.Tr);
+ tf = tf_bv * Transform3f(bv.axes, bv.Tr);
}
void constructBox(const KDOP<16>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
diff --git a/src/traversal/traversal_node_bvhs.cpp b/src/traversal/traversal_node_bvhs.cpp
index edd316d3ebdddeb46c1e7b82c5884aaa7e334c83..5682606c403d0049063ec12259a344ba0d064474 100644
--- a/src/traversal/traversal_node_bvhs.cpp
+++ b/src/traversal/traversal_node_bvhs.cpp
@@ -488,10 +488,8 @@ bool meshConservativeAdvancementOrientedNodeCanStop(FCL_REAL c,
assert(c == d);
// n is in local frame of c1, so we need to turn n into the global frame
- Vec3f n_transformed =
- getBVAxis(model1->getBV(c1).bv, 0) * n[0] +
- getBVAxis(model1->getBV(c1).bv, 1) * n[2] +
- getBVAxis(model1->getBV(c1).bv, 2) * n[2];
+ // TODO: Erase me. There was a bug here
+ Vec3f n_transformed (getBVAxes(model1->getBV(c1).bv) * n);
Quaternion3f R0;
motion1->getCurrentRotation(R0);
n_transformed = R0.transform(n_transformed);
diff --git a/test/test_fcl_collision.cpp b/test/test_fcl_collision.cpp
index 7ce96904b55f35711932d93b94f4cec7919611ad..24386b909377f53fc0ba46553651f7750c1ff12c 100644
--- a/test/test_fcl_collision.cpp
+++ b/test/test_fcl_collision.cpp
@@ -220,8 +220,8 @@ BOOST_AUTO_TEST_CASE(OBB_AABB_test)
{
std::cout << aabb1.min_ << " " << aabb1.max_ << std::endl;
std::cout << aabb2.min_ << " " << aabb2.max_ << std::endl;
- std::cout << obb1.To << " " << obb1.extent << " " << obb1.axis[0] << " " << obb1.axis[1] << " " << obb1.axis[2] << std::endl;
- std::cout << obb2.To << " " << obb2.extent << " " << obb2.axis[0] << " " << obb2.axis[1] << " " << obb2.axis[2] << std::endl;
+ std::cout << obb1.To << " " << obb1.extent << " " << obb1.axes << std::endl;
+ std::cout << obb2.To << " " << obb2.extent << " " << obb2.axes << std::endl;
}
BOOST_CHECK(overlap_aabb == overlap_obb);