Commit fdd9b54d authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Replace setValue by operator<< and remove normalize(bool)

parent 48ba15ac
......@@ -268,7 +268,7 @@ public:
ScrewMotion() : MotionBase()
{
// Default angular velocity is zero
axis.setValue(1, 0, 0);
axis << 1, 0, 0;
angular_vel = 0;
// Default reference point is local zero point
......
......@@ -236,7 +236,7 @@ public:
T yx, T yy, T yz,
T zx, T zy, T zz) : Base()
{
setValue(xx, xy, xz, yx, yy, yz, zx, zy, zz);
*this << xx, xy, xz, yx, yy, yz, zx, zy, zz;
}
template <typename Vector>
......@@ -303,42 +303,12 @@ public:
return *this;
}
inline FclMatrix& normalize(bool* signal)
{
T sqr_length = this->squaredNorm();
if(sqr_length > 0)
{
this->operator/= ((T)std::sqrt(sqr_length));
*signal = true;
}
else
*signal = false;
return *this;
}
inline FclMatrix& abs()
{
*this = this->cwiseAbs ();
return *this;
}
inline void setValue(T x, T y, T z) {
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(FclMatrix, 3);
this->m_storage.data()[0] = x;
this->m_storage.data()[1] = y;
this->m_storage.data()[2] = z;
}
inline void setValue(T xx, T xy, T xz,
T yx, T yy, T yz,
T zx, T zy, T zz)
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(FclMatrix, 3, 3);
this->operator()(0,0) = xx; this->operator()(0,1) = xy; this->operator()(0,2) = xz;
this->operator()(1,0) = yx; this->operator()(1,1) = yy; this->operator()(1,2) = yz;
this->operator()(2,0) = zx; this->operator()(2,1) = zy; this->operator()(2,2) = zz;
}
inline void setValue(T x) { this->setConstant (x); }
// inline void setZero () {data.setValue (0); }
inline bool equal(const FclMatrix& other, T epsilon = std::numeric_limits<T>::epsilon() * 100) const
{
return ((*this - other).cwiseAbs().array () < epsilon).all();
......@@ -440,9 +410,9 @@ public:
Scalar sc = si * ch;
Scalar ss = si * sh;
setValue(cj * ch, sj * sc - cs, sj * cc + ss,
*this << cj * ch, sj * sc - cs, sj * cc + ss,
cj * sh, sj * ss + cc, sj * cs - sc,
-sj, cj * si, cj * ci);
-sj, cj * si, cj * ci;
}
......@@ -672,7 +642,7 @@ void generateCoordinateSystem(
template<typename Matrix, typename Vector>
void hat(Matrix& mat, const Vector& vec)
{
mat.setValue(0, -vec[2], vec[1], vec[2], 0, -vec[0], -vec[1], vec[0], 0);
mat << 0, -vec[2], vec[1], vec[2], 0, -vec[0], -vec[1], vec[0], 0;
}
template<typename Matrix, typename Vector>
......@@ -715,9 +685,9 @@ void eigen(const FclType<Matrix>& m, typename Matrix::Scalar dout[3], Vector* vo
sm += std::abs(R(ip, iq));
if(sm == 0.0)
{
vout[0].setValue(v[0][0], v[0][1], v[0][2]);
vout[1].setValue(v[1][0], v[1][1], v[1][2]);
vout[2].setValue(v[2][0], v[2][1], v[2][2]);
vout[0] << v[0][0], v[0][1], v[0][2];
vout[1] << v[1][0], v[1][1], v[1][2];
vout[2] << v[2][0], v[2][1], v[2][2];
dout[0] = d[0]; dout[1] = d[1]; dout[2] = d[2];
return;
}
......
......@@ -362,7 +362,7 @@ public:
inline void setIdentity()
{
R.setIdentity();
T.setValue(0);
T.setZero();
q = Quaternion3f();
matrix_set = true;
}
......
......@@ -55,14 +55,14 @@ void generateBVHModel(BVHModel<BV>& model, const Box& shape, const Transform3f&
double c = shape.side[2];
std::vector<Vec3f> points(8);
std::vector<Triangle> tri_indices(12);
points[0].setValue(0.5 * a, -0.5 * b, 0.5 * c);
points[1].setValue(0.5 * a, 0.5 * b, 0.5 * c);
points[2].setValue(-0.5 * a, 0.5 * b, 0.5 * c);
points[3].setValue(-0.5 * a, -0.5 * b, 0.5 * c);
points[4].setValue(0.5 * a, -0.5 * b, -0.5 * c);
points[5].setValue(0.5 * a, 0.5 * b, -0.5 * c);
points[6].setValue(-0.5 * a, 0.5 * b, -0.5 * c);
points[7].setValue(-0.5 * a, -0.5 * b, -0.5 * c);
points[0] << 0.5 * a, -0.5 * b, 0.5 * c;
points[1] << 0.5 * a, 0.5 * b, 0.5 * c;
points[2] << -0.5 * a, 0.5 * b, 0.5 * c;
points[3] << -0.5 * a, -0.5 * b, 0.5 * c;
points[4] << 0.5 * a, -0.5 * b, -0.5 * c;
points[5] << 0.5 * a, 0.5 * b, -0.5 * c;
points[6] << -0.5 * a, 0.5 * b, -0.5 * c;
points[7] << -0.5 * a, -0.5 * b, -0.5 * c;
tri_indices[0].set(0, 4, 1);
tri_indices[1].set(1, 4, 5);
......
......@@ -100,8 +100,8 @@ inline OBB merge_largedist(const OBB& b1, const OBB& b2)
else { mid = 2; }
R1.setValue(E[0][max], E[1][max], E[2][max]);
R2.setValue(E[0][mid], E[1][mid], E[2][mid]);
R1 << E[0][max], E[1][max], E[2][max];
R2 << E[0][mid], E[1][mid], E[2][mid];
// set obb centers and extensions
Vec3f center, extent;
......@@ -588,7 +588,7 @@ OBB& OBB::operator += (const Vec3f& p)
bvp.axis[0] = axis[0];
bvp.axis[1] = axis[1];
bvp.axis[2] = axis[2];
bvp.extent.setValue(0);
bvp.extent.setZero();
*this += bvp;
return *this;
......
......@@ -208,7 +208,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if(P && Q)
{
P->setValue(a[0], t, 0);
*P << a[0], t, 0;
*Q = S + (*P);
}
......@@ -237,7 +237,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if(P && Q)
{
P->setValue(a[0], t, 0);
*P << a[0], t, 0;
*Q = S + (*P);
}
......@@ -265,7 +265,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if(P && Q)
{
P->setValue(0, t, 0);
*P << 0, t, 0;
*Q = S + (*P);
}
......@@ -293,7 +293,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if(P && Q)
{
P->setValue(0, t, 0);
*P << 0, t, 0;
*Q = S + (*P);
}
......@@ -361,7 +361,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if(P && Q)
{
P->setValue(a[0], t, 0);
*P << a[0], t, 0;
*Q = S + (*P);
}
......@@ -389,7 +389,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if(P && Q)
{
P->setValue(a[0], t, 0);
*P << a[0], t, 0;
*Q = S + (*P);
}
......@@ -418,7 +418,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if(P && Q)
{
P->setValue(0, t, 0);
*P << 0, t, 0;
*Q = S + (*P);
}
......@@ -447,7 +447,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if(P&& Q)
{
P->setValue(0, t, 0);
*P << 0, t, 0;
*Q = S + (*P);
}
......@@ -515,7 +515,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if(P && Q)
{
P->setValue(t, a[1], 0);
*P << t, a[1], 0;
*Q = S + (*P);
}
......@@ -543,7 +543,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if(P && Q)
{
P->setValue(t, a[1], 0);
*P << t, a[1], 0;
*Q = S + (*P);
}
......@@ -571,7 +571,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if(P && Q)
{
P->setValue(t, 0, 0);
*P << t, 0, 0;
*Q = S + (*P);
}
......@@ -599,7 +599,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if(P && Q)
{
P->setValue(t, 0, 0);
*P << t, 0, 0;
*Q = S + (*P);
}
......@@ -660,7 +660,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if(P && Q)
{
P->setValue(t, a[1], 0);
*P << t, a[1], 0;
*Q = S + (*P);
}
......@@ -688,7 +688,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if(P && Q)
{
P->setValue(t, a[1], 0);
*P << t, a[1], 0;
*Q = S + (*P);
}
......@@ -717,7 +717,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if(P && Q)
{
P->setValue(t, 0, 0);
*P << t, 0, 0;
*Q = S + (*P);
}
......@@ -745,7 +745,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if(P && Q)
{
P->setValue(t, 0, 0);
*P << t, 0, 0;
*Q = S + (*P);
}
......@@ -786,14 +786,14 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if(sep1 >= sep2 && sep1 >= 0)
{
if(Tab[2] > 0)
S.setValue(0, 0, sep1);
S << 0, 0, sep1;
else
S.setValue(0, 0, -sep1);
S << 0, 0, -sep1;
if(P && Q)
{
*Q = S;
P->setValue(0);
P->setZero();
}
}
......@@ -1094,11 +1094,11 @@ RSS RSS::operator + (const RSS& other) const
else { mid = 2; }
// column first matrix, as the axis in RSS
bv.axis[0].setValue(E[0][max], E[1][max], E[2][max]);
bv.axis[1].setValue(E[0][mid], E[1][mid], E[2][mid]);
bv.axis[2].setValue(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.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];
// set rss origin, rectangle size and radius
getRadiusAndOriginAndRectangleSize(v, NULL, NULL, NULL, 16, bv.axis, bv.Tr, bv.l, bv.r);
......
......@@ -712,7 +712,7 @@ int BVHModel<BV>::recursiveBuildTree(int bv_id, int first_primitive, int num_pri
FCL_REAL x = (p1[0] + p2[0] + p3[0]) / 3.0;
FCL_REAL y = (p1[1] + p2[1] + p3[1]) / 3.0;
FCL_REAL z = (p1[2] + p2[2] + p3[2]) / 3.0;
p.setValue(x, y, z);
p << x, y, z;
}
else
{
......
......@@ -520,9 +520,9 @@ static inline void getExtentAndCenter_pointcloud(Vec3f* ps, Vec3f* ps2, unsigned
center = axis[0] * o[0] + axis[1] * o[1] + axis[2] * o[2];
extent.setValue((max_coord[0] - min_coord[0]) / 2,
(max_coord[1] - min_coord[1]) / 2,
(max_coord[2] - min_coord[2]) / 2);
extent << (max_coord[0] - min_coord[0]) / 2,
(max_coord[1] - min_coord[1]) / 2,
(max_coord[2] - min_coord[2]) / 2;
}
......@@ -589,9 +589,9 @@ static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts,
center = axis[0] * o[0] + axis[1] * o[1] + axis[2] * o[2];
extent.setValue((max_coord[0] - min_coord[0]) / 2,
(max_coord[1] - min_coord[1]) / 2,
(max_coord[2] - min_coord[2]) / 2);
extent << (max_coord[0] - min_coord[0]) / 2,
(max_coord[1] - min_coord[1]) / 2,
(max_coord[2] - min_coord[2]) / 2;
}
......
......@@ -58,11 +58,11 @@ static inline void axisFromEigen(Vec3f eigenV[3], Matrix3f::U eigenS[3], Vec3f a
else if(eigenS[2] > eigenS[max]) { mid = max; max = 2; }
else { mid = 2; }
axis[0].setValue(eigenV[0][max], eigenV[1][max], eigenV[2][max]);
axis[1].setValue(eigenV[0][mid], eigenV[1][mid], eigenV[2][mid]);
axis[2].setValue(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]);
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];
}
namespace OBB_fit_functions
......@@ -71,10 +71,10 @@ namespace OBB_fit_functions
void fit1(Vec3f* ps, OBB& bv)
{
bv.To = ps[0];
bv.axis[0].setValue(1, 0, 0);
bv.axis[1].setValue(0, 1, 0);
bv.axis[2].setValue(0, 0, 1);
bv.extent.setValue(0);
bv.axis[0] << 1, 0, 0;
bv.axis[1] << 0, 1, 0;
bv.axis[2] << 0, 0, 1;
bv.extent.setZero();
}
void fit2(Vec3f* ps, OBB& bv)
......@@ -88,10 +88,10 @@ void fit2(Vec3f* ps, OBB& bv)
bv.axis[0] = p1p2;
generateCoordinateSystem(bv.axis[0], bv.axis[1], bv.axis[2]);
bv.extent.setValue(len_p1p2 * 0.5, 0, 0);
bv.To.setValue(0.5 * (p1[0] + p2[0]),
0.5 * (p1[1] + p2[1]),
0.5 * (p1[2] + p2[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]);
}
void fit3(Vec3f* ps, OBB& bv)
......@@ -156,9 +156,9 @@ namespace RSS_fit_functions
void fit1(Vec3f* ps, RSS& bv)
{
bv.Tr = ps[0];
bv.axis[0].setValue(1, 0, 0);
bv.axis[1].setValue(0, 1, 0);
bv.axis[2].setValue(0, 0, 1);
bv.axis[0] << 1, 0, 0;
bv.axis[1] << 0, 1, 0;
bv.axis[2] << 0, 0, 1;
bv.l[0] = 0;
bv.l[1] = 0;
bv.r = 0;
......@@ -245,10 +245,10 @@ void fit1(Vec3f* ps, kIOS& bv)
bv.spheres[0].o = ps[0];
bv.spheres[0].r = 0;
bv.obb.axis[0].setValue(1, 0, 0);
bv.obb.axis[1].setValue(0, 1, 0);
bv.obb.axis[2].setValue(0, 0, 1);
bv.obb.extent.setValue(0);
bv.obb.axis[0] << 1, 0, 0;
bv.obb.axis[1] << 0, 1, 0;
bv.obb.axis[2] << 0, 0, 1;
bv.obb.extent.setZero();
bv.obb.To = ps[0];
}
......@@ -267,7 +267,7 @@ void fit2(Vec3f* ps, kIOS& bv)
generateCoordinateSystem(axis[0], axis[1], axis[2]);
FCL_REAL r0 = len_p1p2 * 0.5;
bv.obb.extent.setValue(r0, 0, 0);
bv.obb.extent << r0, 0, 0;
bv.obb.To = (p1 + p2) * 0.5;
bv.spheres[0].o = bv.obb.To;
......
......@@ -441,7 +441,7 @@ FCL_REAL TriangleMotionBoundVisitor::visit(const InterpMotion& motion) const
InterpMotion::InterpMotion() : MotionBase()
{
// Default angular velocity is zero
angular_axis.setValue(1, 0, 0);
angular_axis << 1, 0, 0;
angular_vel = 0;
// Default reference point is local zero point
......
......@@ -1111,11 +1111,10 @@ FCL_REAL Intersect::distanceToPlane(const Vec3f& n, FCL_REAL t, const Vec3f& v)
bool Intersect::buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, FCL_REAL* t)
{
Vec3f n_ = (v2 - v1).cross(v3 - v1);
bool can_normalize = false;
n_.normalize(&can_normalize);
if(can_normalize)
FCL_REAL norm2 = n_.squaredNorm();
if (n > 0)
{
*n = n_;
*n = n_ / sqrt(norm2);
*t = n_.dot(v1);
return true;
}
......@@ -1126,11 +1125,10 @@ bool Intersect::buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f
bool Intersect::buildEdgePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& tn, Vec3f* n, FCL_REAL* t)
{
Vec3f n_ = (v2 - v1).cross(tn);
bool can_normalize = false;
n_.normalize(&can_normalize);
if(can_normalize)
FCL_REAL norm2 = n_.squaredNorm();
if (n > 0)
{
*n = n_;
*n = n_ / sqrt(norm2);
*t = n_.dot(v1);
return true;
}
......
......@@ -103,9 +103,9 @@ void Quaternion3f::toRotation(Matrix3f& R) const
FCL_REAL twoYZ = twoZ*data[2];
FCL_REAL twoZZ = twoZ*data[3];
R.setValue(1.0 - (twoYY + twoZZ), twoXY - twoWZ, twoXZ + twoWY,
twoXY + twoWZ, 1.0 - (twoXX + twoZZ), twoYZ - twoWX,
twoXZ - twoWY, twoYZ + twoWX, 1.0 - (twoXX + twoYY));
R << 1.0 - (twoYY + twoZZ), twoXY - twoWZ, twoXZ + twoWY,
twoXY + twoWZ, 1.0 - (twoXX + twoZZ), twoYZ - twoWX,
twoXZ - twoWY, twoYZ + twoWX, 1.0 - (twoXX + twoYY);
}
......@@ -169,9 +169,9 @@ void Quaternion3f::toAxes(Vec3f axis[3]) const
FCL_REAL twoYZ = twoZ*data[2];
FCL_REAL twoZZ = twoZ*data[3];
axis[0].setValue(1.0 - (twoYY + twoZZ), twoXY + twoWZ, twoXZ - twoWY);
axis[1].setValue(twoXY - twoWZ, 1.0 - (twoXX + twoZZ), twoYZ + twoWX);
axis[2].setValue(twoXZ + twoWY, twoYZ - twoWX, 1.0 - (twoXX + twoYY));
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);
}
......
......@@ -780,9 +780,9 @@ bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
res = ccdMPRPenetration(obj1, obj2, &ccd, &depth, &dir, &pos);
if(res == 0)
{
contact_points->setValue(ccdVec3X(&pos), ccdVec3Y(&pos), ccdVec3Z(&pos));
*contact_points << ccdVec3X(&pos), ccdVec3Y(&pos), ccdVec3Z(&pos);
*penetration_depth = depth;
normal->setValue(ccdVec3X(&dir), ccdVec3Y(&dir), ccdVec3Z(&dir));
*normal << ccdVec3X(&dir), ccdVec3Y(&dir), ccdVec3Z(&dir);
return true;
}
......@@ -808,8 +808,8 @@ bool GJKDistance(void* obj1, ccd_support_fn supp1,
ccd_vec3_t p1_, p2_;
dist = libccd_extension::ccdGJKDist2(obj1, obj2, &ccd, &p1_, &p2_);
if(p1) p1->setValue(ccdVec3X(&p1_), ccdVec3Y(&p1_), ccdVec3Z(&p1_));
if(p2) p2->setValue(ccdVec3X(&p2_), ccdVec3Y(&p2_), ccdVec3Z(&p2_));
if(p1) *p1 << ccdVec3X(&p1_), ccdVec3Y(&p1_), ccdVec3Z(&p1_);
if(p2) *p2 << ccdVec3X(&p2_), ccdVec3Y(&p2_), ccdVec3Z(&p2_);
if(res) *res = dist;
if(dist < 0) return false;
else return true;
......
......@@ -109,7 +109,7 @@ void Halfspace::unitNormalTest()
}
else
{
n.setValue(1, 0, 0);
n << 1, 0, 0;
d = 0;
}
}
......@@ -125,7 +125,7 @@ void Plane::unitNormalTest()
}
else
{
n.setValue(1, 0, 0);
n << 1, 0, 0;
d = 0;
}
}
......
......@@ -424,10 +424,10 @@ void computeBV<OBB, Sphere>(const Sphere& s, const Transform3f& tf, OBB& bv)
const Vec3f& T = tf.getTranslation();
bv.To = T;
bv.axis[0].setValue(1, 0, 0);
bv.axis[1].setValue(0, 1, 0);
bv.axis[2].setValue(0, 0, 1);
bv.extent.setValue(s.radius);
bv.axis[0] << 1, 0, 0;
bv.axis[1] << 0, 1, 0;
bv.axis[2] << 0, 0, 1;
bv.extent.setConstant(s.radius);
}
template<>
......@@ -440,7 +440,7 @@ void computeBV<OBB, Capsule>(const Capsule& s, const Transform3f& tf, OBB& bv)
bv.axis[0] = R.col(0);
bv.axis[1] = R.col(1);
bv.axis[2] = R.col(2);
bv.extent.setValue(s.radius, s.radius, s.lz / 2 + s.radius);
bv.extent << s.radius, s.radius, s.lz / 2 + s.radius;
}
template<>
......@@ -453,7 +453,7 @@ void computeBV<OBB, Cone>(const Cone& s, const Transform3f& tf, OBB& bv)
bv.axis[0] = R.col(0);
bv.axis[1] = R.col(1);
bv.axis[2] = R.col(2);