bool testRawDataAcces() {
bool passed = true;
Eigen::Matrix<Scalar, 4, 1> raw = {0, 1, 0, 0};
Eigen::Map<RxSO3Type const> map_of_const_rxso3(raw.data());
SOPHUS_TEST_APPROX(passed, map_of_const_rxso3.quaternion().coeffs().eval(),
raw, Constants<Scalar>::epsilon());
SOPHUS_TEST_EQUAL(passed, map_of_const_rxso3.quaternion().coeffs().data(),
raw.data());
Eigen::Map<RxSO3Type const> const_shallow_copy = map_of_const_rxso3;
SOPHUS_TEST_EQUAL(passed, const_shallow_copy.quaternion().coeffs().eval(),
map_of_const_rxso3.quaternion().coeffs().eval());
Eigen::Matrix<Scalar, 4, 1> raw2 = {1, 0, 0, 0};
Eigen::Map<RxSO3Type> map_of_rxso3(raw.data());
Eigen::Quaternion<Scalar> quat;
quat.coeffs() = raw2;
map_of_rxso3.setQuaternion(quat);
SOPHUS_TEST_APPROX(passed, map_of_rxso3.quaternion().coeffs().eval(), raw2,
Constants<Scalar>::epsilon());
SOPHUS_TEST_EQUAL(passed, map_of_rxso3.quaternion().coeffs().data(),
raw.data());
SOPHUS_TEST_NEQ(passed, map_of_rxso3.quaternion().coeffs().data(),
quat.coeffs().data());
Eigen::Map<RxSO3Type> shallow_copy = map_of_rxso3;
SOPHUS_TEST_EQUAL(passed, shallow_copy.quaternion().coeffs().eval(),
map_of_rxso3.quaternion().coeffs().eval());
RxSO3Type const const_so3(quat);
for (int i = 0; i < 4; ++i) {
SOPHUS_TEST_EQUAL(passed, const_so3.data()[i], raw2.data()[i]);
}
RxSO3Type so3(quat);
for (int i = 0; i < 4; ++i) {
so3.data()[i] = raw[i];
}
for (int i = 0; i < 4; ++i) {
SOPHUS_TEST_EQUAL(passed, so3.data()[i], raw.data()[i]);
}
for (int i = 0; i < 10; ++i) {
Matrix3<Scalar> M = Matrix3<Scalar>::Random();
for (Scalar scale : {Scalar(0.01), Scalar(0.99), Scalar(1), Scalar(10)}) {
Matrix3<Scalar> R = makeRotationMatrix(M);
Matrix3<Scalar> sR = scale * R;
SOPHUS_TEST(passed, isScaledOrthogonalAndPositive(sR),
"isScaledOrthogonalAndPositive(sR): % *\n%", scale, R);
Matrix3<Scalar> sR_cols_swapped;
sR_cols_swapped << sR.col(1), sR.col(0), sR.col(2);
SOPHUS_TEST(passed, !isScaledOrthogonalAndPositive(sR_cols_swapped),
"isScaledOrthogonalAndPositive(-sR): % *\n%", scale, R);
}
}
return passed;
}
void PointMesher<T>::Mesher::createFaceAttributes(const Matrix vList, const Matrix fList, Matrix& fAttrList, Labels& fAttrLabels) const
{
// Initialization
Matrix3 vp;
Vector3 vc;
Vector3 vn;
for (int i = 0; i < fList.cols(); i++)
{
// Get triangle
vp.col(0) = vList.col(int(fList(0, i)));
vp.col(1) = vList.col(int(fList(1, i)));
vp.col(2) = vList.col(int(fList(2, i)));
// Compute triangle centroid
vc = this->computeCentroid(vp);
// Compute normal at centroid
vn = this->computeNormal(vp);
// Generate data structure
fAttrList.block(0, i, 3, 1) = vc;
fAttrList.block(3, i, 3, 1) = vn;
}
fAttrLabels[0].text = "FaceCentroids";
fAttrLabels[0].span = 3;
fAttrLabels[1].text = "FaceNormals";
fAttrLabels[1].span = 3;
}
typename PointMesher<T>::Vector3 PointMesher<T>::Mesher::computeNormal(Matrix3 matrixIn) const
{
Vector3 v1 = matrixIn.col(1) - matrixIn.col(0);
Vector3 v2 = matrixIn.col(2) - matrixIn.col(0);
Vector3 vn = v1.cross(v2);
return vn.normalized();
}
template<typename Scalar> void orthomethods_3()
{
typedef typename NumTraits<Scalar>::Real RealScalar;
typedef Matrix<Scalar,3,3> Matrix3;
typedef Matrix<Scalar,3,1> Vector3;
typedef Matrix<Scalar,4,1> Vector4;
Vector3 v0 = Vector3::Random(),
v1 = Vector3::Random(),
v2 = Vector3::Random();
// cross product
VERIFY_IS_MUCH_SMALLER_THAN(v1.cross(v2).dot(v1), Scalar(1));
VERIFY_IS_MUCH_SMALLER_THAN(v1.dot(v1.cross(v2)), Scalar(1));
VERIFY_IS_MUCH_SMALLER_THAN(v1.cross(v2).dot(v2), Scalar(1));
VERIFY_IS_MUCH_SMALLER_THAN(v2.dot(v1.cross(v2)), Scalar(1));
VERIFY_IS_MUCH_SMALLER_THAN(v1.cross(Vector3::Random()).dot(v1), Scalar(1));
Matrix3 mat3;
mat3 << v0.normalized(),
(v0.cross(v1)).normalized(),
(v0.cross(v1).cross(v0)).normalized();
VERIFY(mat3.isUnitary());
mat3.setRandom();
VERIFY_IS_APPROX(v0.cross(mat3*v1), -(mat3*v1).cross(v0));
VERIFY_IS_APPROX(v0.cross(mat3.lazyProduct(v1)), -(mat3.lazyProduct(v1)).cross(v0));
// colwise/rowwise cross product
mat3.setRandom();
Vector3 vec3 = Vector3::Random();
Matrix3 mcross;
int i = internal::random<int>(0,2);
mcross = mat3.colwise().cross(vec3);
VERIFY_IS_APPROX(mcross.col(i), mat3.col(i).cross(vec3));
VERIFY_IS_MUCH_SMALLER_THAN((mat3.adjoint() * mat3.colwise().cross(vec3)).diagonal().cwiseAbs().sum(), Scalar(1));
VERIFY_IS_MUCH_SMALLER_THAN((mat3.adjoint() * mat3.colwise().cross(Vector3::Random())).diagonal().cwiseAbs().sum(), Scalar(1));
VERIFY_IS_MUCH_SMALLER_THAN((vec3.adjoint() * mat3.colwise().cross(vec3)).cwiseAbs().sum(), Scalar(1));
VERIFY_IS_MUCH_SMALLER_THAN((vec3.adjoint() * Matrix3::Random().colwise().cross(vec3)).cwiseAbs().sum(), Scalar(1));
mcross = mat3.rowwise().cross(vec3);
VERIFY_IS_APPROX(mcross.row(i), mat3.row(i).cross(vec3));
// cross3
Vector4 v40 = Vector4::Random(),
v41 = Vector4::Random(),
v42 = Vector4::Random();
v40.w() = v41.w() = v42.w() = 0;
v42.template head<3>() = v40.template head<3>().cross(v41.template head<3>());
VERIFY_IS_APPROX(v40.cross3(v41), v42);
VERIFY_IS_MUCH_SMALLER_THAN(v40.cross3(Vector4::Random()).dot(v40), Scalar(1));
// check mixed product
typedef Matrix<RealScalar, 3, 1> RealVector3;
RealVector3 rv1 = RealVector3::Random();
VERIFY_IS_APPROX(v1.cross(rv1.template cast<Scalar>()), v1.cross(rv1));
VERIFY_IS_APPROX(rv1.template cast<Scalar>().cross(v1), rv1.cross(v1));
}
void Vert::calc_curvature() {
Edge *e = edge;
Matrix3 m = Matrix3();
double wij_sum = 0.0;
do {
Vector tij = ((Matrix3::identity() - Matrix3(normal(), normal())) * e->pair->vect()).unit();
double kij = 2 * normal().dot(e->pair->vect()) / pow(e->pair->vect().length(),2);
double wij = 0.0;
if (e->face != NULL) { wij += e->face->area(); }
if (e->pair->face != NULL) { wij += e->pair->face->area(); }
wij_sum += wij;
m = m + wij * kij * Matrix3(tij, tij);
e = e->pair->prev;
} while (e != edge);
m = m / wij_sum;
Vector e1 = Vector(1, 0, 0);
Vector wvi;
if ((e1 - normal()).length() > (e1 + normal()).length()) {
wvi = (e1 - normal()).unit();
} else {
wvi = (e1 + normal()).unit();
}
Matrix3 qvi = Matrix3::identity() - 2 * Matrix3(wvi,wvi);
Matrix3 m2 = qvi.transpose() * m * qvi;
double m11 = m2.values[4],
m12 = m2.values[7],
m22 = m2.values[8],
beta = (m22 - m11) / 2 / m12,
t = sign(beta) / (abs(beta) + sqrt(pow(beta, 2) + 1)),
c = 1.0 / sqrt(pow(t,2) + 1),
s = c * t,
m11_p = pow(c,2) * m11 + pow(s,2) * m22 - 2 * c * s * m12,
m22_p = pow(s,2) * m11 + pow(c,2) * m22 + 2 * c * s * m12,
phi = atan(t),
k1_p = 3 * m11_p - m22_p,
k2_p = 3 * m22_p - m11_p;
if (k1_p < k2_p) {
mem_max_curvature_mag = k1_p;
mem_max_curvature_dir = cos(phi) * qvi.col(1) - sin(phi) * qvi.col(2);
mem_min_curvature_mag = k2_p;
mem_min_curvature_dir = sin(phi) * qvi.col(1) + cos(phi) * qvi.col(2);
} else {
mem_max_curvature_mag = k2_p;
mem_max_curvature_dir = sin(phi) * qvi.col(1) + cos(phi) * qvi.col(2);
mem_min_curvature_mag = k1_p;
mem_min_curvature_dir = cos(phi) * qvi.col(1) - sin(phi) * qvi.col(2);
}
}
template<typename Scalar> void geometry(void)
{
/* this test covers the following files:
Cross.h Quaternion.h, Transform.cpp
*/
typedef Matrix<Scalar,2,2> Matrix2;
typedef Matrix<Scalar,3,3> Matrix3;
typedef Matrix<Scalar,4,4> Matrix4;
typedef Matrix<Scalar,2,1> Vector2;
typedef Matrix<Scalar,3,1> Vector3;
typedef Matrix<Scalar,4,1> Vector4;
typedef Quaternion<Scalar> Quaternionx;
typedef AngleAxis<Scalar> AngleAxisx;
typedef Transform<Scalar,2> Transform2;
typedef Transform<Scalar,3> Transform3;
typedef Scaling<Scalar,2> Scaling2;
typedef Scaling<Scalar,3> Scaling3;
typedef Translation<Scalar,2> Translation2;
typedef Translation<Scalar,3> Translation3;
Scalar largeEps = test_precision<Scalar>();
if (ei_is_same_type<Scalar,float>::ret)
largeEps = 1e-2f;
Vector3 v0 = Vector3::Random(),
v1 = Vector3::Random(),
v2 = Vector3::Random();
Vector2 u0 = Vector2::Random();
Matrix3 matrot1;
Scalar a = ei_random<Scalar>(-Scalar(M_PI), Scalar(M_PI));
// cross product
VERIFY_IS_MUCH_SMALLER_THAN(v1.cross(v2).eigen2_dot(v1), Scalar(1));
Matrix3 m;
m << v0.normalized(),
(v0.cross(v1)).normalized(),
(v0.cross(v1).cross(v0)).normalized();
VERIFY(m.isUnitary());
// Quaternion: Identity(), setIdentity();
Quaternionx q1, q2;
q2.setIdentity();
VERIFY_IS_APPROX(Quaternionx(Quaternionx::Identity()).coeffs(), q2.coeffs());
q1.coeffs().setRandom();
VERIFY_IS_APPROX(q1.coeffs(), (q1*q2).coeffs());
// unitOrthogonal
VERIFY_IS_MUCH_SMALLER_THAN(u0.unitOrthogonal().eigen2_dot(u0), Scalar(1));
VERIFY_IS_MUCH_SMALLER_THAN(v0.unitOrthogonal().eigen2_dot(v0), Scalar(1));
VERIFY_IS_APPROX(u0.unitOrthogonal().norm(), Scalar(1));
VERIFY_IS_APPROX(v0.unitOrthogonal().norm(), Scalar(1));
VERIFY_IS_APPROX(v0, AngleAxisx(a, v0.normalized()) * v0);
VERIFY_IS_APPROX(-v0, AngleAxisx(Scalar(M_PI), v0.unitOrthogonal()) * v0);
VERIFY_IS_APPROX(ei_cos(a)*v0.squaredNorm(), v0.eigen2_dot(AngleAxisx(a, v0.unitOrthogonal()) * v0));
m = AngleAxisx(a, v0.normalized()).toRotationMatrix().adjoint();
VERIFY_IS_APPROX(Matrix3::Identity(), m * AngleAxisx(a, v0.normalized()));
VERIFY_IS_APPROX(Matrix3::Identity(), AngleAxisx(a, v0.normalized()) * m);
q1 = AngleAxisx(a, v0.normalized());
q2 = AngleAxisx(a, v1.normalized());
// angular distance
Scalar refangle = ei_abs(AngleAxisx(q1.inverse()*q2).angle());
if (refangle>Scalar(M_PI))
refangle = Scalar(2)*Scalar(M_PI) - refangle;
if((q1.coeffs()-q2.coeffs()).norm() > 10*largeEps)
{
VERIFY(ei_isApprox(q1.angularDistance(q2), refangle, largeEps));
}
// rotation matrix conversion
VERIFY_IS_APPROX(q1 * v2, q1.toRotationMatrix() * v2);
VERIFY_IS_APPROX(q1 * q2 * v2,
q1.toRotationMatrix() * q2.toRotationMatrix() * v2);
VERIFY( (q2*q1).isApprox(q1*q2, largeEps) || !(q2 * q1 * v2).isApprox(
q1.toRotationMatrix() * q2.toRotationMatrix() * v2));
q2 = q1.toRotationMatrix();
VERIFY_IS_APPROX(q1*v1,q2*v1);
matrot1 = AngleAxisx(Scalar(0.1), Vector3::UnitX())
* AngleAxisx(Scalar(0.2), Vector3::UnitY())
* AngleAxisx(Scalar(0.3), Vector3::UnitZ());
VERIFY_IS_APPROX(matrot1 * v1,
AngleAxisx(Scalar(0.1), Vector3(1,0,0)).toRotationMatrix()
* (AngleAxisx(Scalar(0.2), Vector3(0,1,0)).toRotationMatrix()
* (AngleAxisx(Scalar(0.3), Vector3(0,0,1)).toRotationMatrix() * v1)));
// angle-axis conversion
AngleAxisx aa = q1;
VERIFY_IS_APPROX(q1 * v1, Quaternionx(aa) * v1);
VERIFY_IS_NOT_APPROX(q1 * v1, Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1);
// from two vector creation
VERIFY_IS_APPROX(v2.normalized(),(q2.setFromTwoVectors(v1,v2)*v1).normalized());
VERIFY_IS_APPROX(v2.normalized(),(q2.setFromTwoVectors(v1,v2)*v1).normalized());
// inverse and conjugate
VERIFY_IS_APPROX(q1 * (q1.inverse() * v1), v1);
VERIFY_IS_APPROX(q1 * (q1.conjugate() * v1), v1);
// AngleAxis
VERIFY_IS_APPROX(AngleAxisx(a,v1.normalized()).toRotationMatrix(),
Quaternionx(AngleAxisx(a,v1.normalized())).toRotationMatrix());
AngleAxisx aa1;
m = q1.toRotationMatrix();
aa1 = m;
VERIFY_IS_APPROX(AngleAxisx(m).toRotationMatrix(),
Quaternionx(m).toRotationMatrix());
// Transform
// TODO complete the tests !
a = 0;
while (ei_abs(a)<Scalar(0.1))
a = ei_random<Scalar>(-Scalar(0.4)*Scalar(M_PI), Scalar(0.4)*Scalar(M_PI));
q1 = AngleAxisx(a, v0.normalized());
Transform3 t0, t1, t2;
// first test setIdentity() and Identity()
t0.setIdentity();
VERIFY_IS_APPROX(t0.matrix(), Transform3::MatrixType::Identity());
t0.matrix().setZero();
t0 = Transform3::Identity();
VERIFY_IS_APPROX(t0.matrix(), Transform3::MatrixType::Identity());
t0.linear() = q1.toRotationMatrix();
t1.setIdentity();
t1.linear() = q1.toRotationMatrix();
v0 << 50, 2, 1;//= ei_random_matrix<Vector3>().cwiseProduct(Vector3(10,2,0.5));
t0.scale(v0);
t1.prescale(v0);
VERIFY_IS_APPROX( (t0 * Vector3(1,0,0)).norm(), v0.x());
//VERIFY(!ei_isApprox((t1 * Vector3(1,0,0)).norm(), v0.x()));
t0.setIdentity();
t1.setIdentity();
v1 << 1, 2, 3;
t0.linear() = q1.toRotationMatrix();
t0.pretranslate(v0);
t0.scale(v1);
t1.linear() = q1.conjugate().toRotationMatrix();
t1.prescale(v1.cwise().inverse());
t1.translate(-v0);
VERIFY((t0.matrix() * t1.matrix()).isIdentity(test_precision<Scalar>()));
t1.fromPositionOrientationScale(v0, q1, v1);
VERIFY_IS_APPROX(t1.matrix(), t0.matrix());
VERIFY_IS_APPROX(t1*v1, t0*v1);
t0.setIdentity(); t0.scale(v0).rotate(q1.toRotationMatrix());
t1.setIdentity(); t1.scale(v0).rotate(q1);
VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
t0.setIdentity(); t0.scale(v0).rotate(AngleAxisx(q1));
VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
VERIFY_IS_APPROX(t0.scale(a).matrix(), t1.scale(Vector3::Constant(a)).matrix());
VERIFY_IS_APPROX(t0.prescale(a).matrix(), t1.prescale(Vector3::Constant(a)).matrix());
// More transform constructors, operator=, operator*=
Matrix3 mat3 = Matrix3::Random();
Matrix4 mat4;
mat4 << mat3 , Vector3::Zero() , Vector4::Zero().transpose();
Transform3 tmat3(mat3), tmat4(mat4);
tmat4.matrix()(3,3) = Scalar(1);
VERIFY_IS_APPROX(tmat3.matrix(), tmat4.matrix());
Scalar a3 = ei_random<Scalar>(-Scalar(M_PI), Scalar(M_PI));
Vector3 v3 = Vector3::Random().normalized();
AngleAxisx aa3(a3, v3);
Transform3 t3(aa3);
Transform3 t4;
t4 = aa3;
VERIFY_IS_APPROX(t3.matrix(), t4.matrix());
t4.rotate(AngleAxisx(-a3,v3));
VERIFY_IS_APPROX(t4.matrix(), Matrix4::Identity());
t4 *= aa3;
VERIFY_IS_APPROX(t3.matrix(), t4.matrix());
v3 = Vector3::Random();
Translation3 tv3(v3);
Transform3 t5(tv3);
t4 = tv3;
VERIFY_IS_APPROX(t5.matrix(), t4.matrix());
t4.translate(-v3);
VERIFY_IS_APPROX(t4.matrix(), Matrix4::Identity());
t4 *= tv3;
VERIFY_IS_APPROX(t5.matrix(), t4.matrix());
Scaling3 sv3(v3);
Transform3 t6(sv3);
t4 = sv3;
VERIFY_IS_APPROX(t6.matrix(), t4.matrix());
t4.scale(v3.cwise().inverse());
VERIFY_IS_APPROX(t4.matrix(), Matrix4::Identity());
t4 *= sv3;
VERIFY_IS_APPROX(t6.matrix(), t4.matrix());
// matrix * transform
VERIFY_IS_APPROX(Transform3(t3.matrix()*t4).matrix(), Transform3(t3*t4).matrix());
// chained Transform product
VERIFY_IS_APPROX(((t3*t4)*t5).matrix(), (t3*(t4*t5)).matrix());
// check that Transform product doesn't have aliasing problems
t5 = t4;
t5 = t5*t5;
VERIFY_IS_APPROX(t5, t4*t4);
// 2D transformation
Transform2 t20, t21;
Vector2 v20 = Vector2::Random();
Vector2 v21 = Vector2::Random();
for (int k=0; k<2; ++k)
if (ei_abs(v21[k])<Scalar(1e-3)) v21[k] = Scalar(1e-3);
t21.setIdentity();
t21.linear() = Rotation2D<Scalar>(a).toRotationMatrix();
VERIFY_IS_APPROX(t20.fromPositionOrientationScale(v20,a,v21).matrix(),
t21.pretranslate(v20).scale(v21).matrix());
t21.setIdentity();
t21.linear() = Rotation2D<Scalar>(-a).toRotationMatrix();
VERIFY( (t20.fromPositionOrientationScale(v20,a,v21)
* (t21.prescale(v21.cwise().inverse()).translate(-v20))).matrix().isIdentity(test_precision<Scalar>()) );
// Transform - new API
// 3D
t0.setIdentity();
t0.rotate(q1).scale(v0).translate(v0);
// mat * scaling and mat * translation
t1 = (Matrix3(q1) * Scaling3(v0)) * Translation3(v0);
VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
// mat * transformation and scaling * translation
t1 = Matrix3(q1) * (Scaling3(v0) * Translation3(v0));
VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
t0.setIdentity();
t0.prerotate(q1).prescale(v0).pretranslate(v0);
// translation * scaling and transformation * mat
t1 = (Translation3(v0) * Scaling3(v0)) * Matrix3(q1);
VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
// scaling * mat and translation * mat
t1 = Translation3(v0) * (Scaling3(v0) * Matrix3(q1));
VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
t0.setIdentity();
t0.scale(v0).translate(v0).rotate(q1);
// translation * mat and scaling * transformation
t1 = Scaling3(v0) * (Translation3(v0) * Matrix3(q1));
VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
// transformation * scaling
t0.scale(v0);
t1 = t1 * Scaling3(v0);
VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
// transformation * translation
t0.translate(v0);
t1 = t1 * Translation3(v0);
VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
// translation * transformation
t0.pretranslate(v0);
t1 = Translation3(v0) * t1;
VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
// transform * quaternion
t0.rotate(q1);
t1 = t1 * q1;
VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
// translation * quaternion
t0.translate(v1).rotate(q1);
t1 = t1 * (Translation3(v1) * q1);
VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
// scaling * quaternion
t0.scale(v1).rotate(q1);
t1 = t1 * (Scaling3(v1) * q1);
VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
// quaternion * transform
t0.prerotate(q1);
t1 = q1 * t1;
VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
// quaternion * translation
t0.rotate(q1).translate(v1);
t1 = t1 * (q1 * Translation3(v1));
VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
// quaternion * scaling
t0.rotate(q1).scale(v1);
t1 = t1 * (q1 * Scaling3(v1));
VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
// translation * vector
t0.setIdentity();
t0.translate(v0);
VERIFY_IS_APPROX(t0 * v1, Translation3(v0) * v1);
// scaling * vector
t0.setIdentity();
t0.scale(v0);
VERIFY_IS_APPROX(t0 * v1, Scaling3(v0) * v1);
// test transform inversion
t0.setIdentity();
t0.translate(v0);
t0.linear().setRandom();
VERIFY_IS_APPROX(t0.inverse(Affine), t0.matrix().inverse());
t0.setIdentity();
t0.translate(v0).rotate(q1);
VERIFY_IS_APPROX(t0.inverse(Isometry), t0.matrix().inverse());
// test extract rotation and scaling
t0.setIdentity();
t0.translate(v0).rotate(q1).scale(v1);
VERIFY_IS_APPROX(t0.rotation() * v1, Matrix3(q1) * v1);
Matrix3 mat_rotation, mat_scaling;
t0.setIdentity();
t0.translate(v0).rotate(q1).scale(v1);
t0.computeRotationScaling(&mat_rotation, &mat_scaling);
VERIFY_IS_APPROX(t0.linear(), mat_rotation * mat_scaling);
VERIFY_IS_APPROX(mat_rotation*mat_rotation.adjoint(), Matrix3::Identity());
VERIFY_IS_APPROX(mat_rotation.determinant(), Scalar(1));
t0.computeScalingRotation(&mat_scaling, &mat_rotation);
VERIFY_IS_APPROX(t0.linear(), mat_scaling * mat_rotation);
VERIFY_IS_APPROX(mat_rotation*mat_rotation.adjoint(), Matrix3::Identity());
VERIFY_IS_APPROX(mat_rotation.determinant(), Scalar(1));
// test casting
Transform<float,3> t1f = t1.template cast<float>();
VERIFY_IS_APPROX(t1f.template cast<Scalar>(),t1);
Transform<double,3> t1d = t1.template cast<double>();
VERIFY_IS_APPROX(t1d.template cast<Scalar>(),t1);
Translation3 tr1(v0);
Translation<float,3> tr1f = tr1.template cast<float>();
VERIFY_IS_APPROX(tr1f.template cast<Scalar>(),tr1);
Translation<double,3> tr1d = tr1.template cast<double>();
VERIFY_IS_APPROX(tr1d.template cast<Scalar>(),tr1);
Scaling3 sc1(v0);
Scaling<float,3> sc1f = sc1.template cast<float>();
VERIFY_IS_APPROX(sc1f.template cast<Scalar>(),sc1);
Scaling<double,3> sc1d = sc1.template cast<double>();
VERIFY_IS_APPROX(sc1d.template cast<Scalar>(),sc1);
Quaternion<float> q1f = q1.template cast<float>();
VERIFY_IS_APPROX(q1f.template cast<Scalar>(),q1);
Quaternion<double> q1d = q1.template cast<double>();
VERIFY_IS_APPROX(q1d.template cast<Scalar>(),q1);
AngleAxis<float> aa1f = aa1.template cast<float>();
VERIFY_IS_APPROX(aa1f.template cast<Scalar>(),aa1);
AngleAxis<double> aa1d = aa1.template cast<double>();
VERIFY_IS_APPROX(aa1d.template cast<Scalar>(),aa1);
Rotation2D<Scalar> r2d1(ei_random<Scalar>());
Rotation2D<float> r2d1f = r2d1.template cast<float>();
VERIFY_IS_APPROX(r2d1f.template cast<Scalar>(),r2d1);
Rotation2D<double> r2d1d = r2d1.template cast<double>();
VERIFY_IS_APPROX(r2d1d.template cast<Scalar>(),r2d1);
m = q1;
// m.col(1) = Vector3(0,ei_random<Scalar>(),ei_random<Scalar>()).normalized();
// m.col(0) = Vector3(-1,0,0).normalized();
// m.col(2) = m.col(0).cross(m.col(1));
#define VERIFY_EULER(I,J,K, X,Y,Z) { \
Vector3 ea = m.eulerAngles(I,J,K); \
Matrix3 m1 = Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z())); \
VERIFY_IS_APPROX(m, m1); \
VERIFY_IS_APPROX(m, Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z()))); \
}
VERIFY_EULER(0,1,2, X,Y,Z);
VERIFY_EULER(0,1,0, X,Y,X);
VERIFY_EULER(0,2,1, X,Z,Y);
VERIFY_EULER(0,2,0, X,Z,X);
VERIFY_EULER(1,2,0, Y,Z,X);
VERIFY_EULER(1,2,1, Y,Z,Y);
VERIFY_EULER(1,0,2, Y,X,Z);
VERIFY_EULER(1,0,1, Y,X,Y);
VERIFY_EULER(2,0,1, Z,X,Y);
VERIFY_EULER(2,0,2, Z,X,Z);
VERIFY_EULER(2,1,0, Z,Y,X);
VERIFY_EULER(2,1,2, Z,Y,Z);
// colwise/rowwise cross product
mat3.setRandom();
Vector3 vec3 = Vector3::Random();
Matrix3 mcross;
int i = ei_random<int>(0,2);
mcross = mat3.colwise().cross(vec3);
VERIFY_IS_APPROX(mcross.col(i), mat3.col(i).cross(vec3));
mcross = mat3.rowwise().cross(vec3);
VERIFY_IS_APPROX(mcross.row(i), mat3.row(i).cross(vec3));
}
// Original comment:
// Triangulation happens in 2d. We could inverse transform the polygon around the normal direction, or we just use the two
// most signficant axes. Here we find the two longest axes and use them to triangulate. Inverse transforming them would
// introduce more doubling point error and isn't worth it.
//
// SC says:
// This doesn't work: the vertices can be collinear when projected onto the plane of the two longest axes of the bounding box.
// Example (from real data):
//
// v[0] = (-13.7199, 4.45725, -8.00059)
// v[1] = (-0.115787, 12.3116, -4.96109)
// v[2] = (0.88992, 12.8922, -3.80342)
// v[3] = (-0.115787, 12.3116, -2.64576)
// v[4] = (-13.7199, 4.45725, 0.393742)
// v[5] = (-13.7199, 4.45725, -0.856258)
// v[6] = (-12.5335, 5.14221, -3.80342)
// v[7] = (-13.7199, 4.45725, -6.75059)
//
// Instead, we will project onto the plane of the polygon.
long
Polygon3::triangulate(Array<long> & tri_indices, Real epsilon) const
{
if (epsilon < 0)
epsilon = Math::eps<Real>();
if (vertices.size() < 3)
{
tri_indices.clear();
}
else if (vertices.size() == 3)
{
tri_indices.resize(3);
tri_indices[0] = vertices[0].index;
tri_indices[1] = vertices[1].index;
tri_indices[2] = vertices[2].index;
}
else if (vertices.size() > 3)
{
tri_indices.clear();
size_t n = vertices.size();
proj_vertices.resize(n);
Vector3 normal = computeNormal();
Matrix3 basis = Math::orthonormalBasis(normal);
Vector3 axis0 = basis.col(0);
Vector3 axis1 = basis.col(1);
Vector3 v0 = vertices[0].position; // a reference point for the plane of the polygon
for (size_t i = 0; i < n; ++i)
{
Vector3 v = vertices[i].position - v0;
proj_vertices[i] = Vector2(v.dot(axis0), v.dot(axis1));
}
Array<size_t> indices(n);
bool flipped = false;
if (projArea() > 0)
{
for (size_t v = 0; v < n; ++v)
indices[v] = v;
}
else
{
for (size_t v = 0; v < n; ++v)
indices[v] = (n - 1) - v;
flipped = true;
}
size_t nv = n;
size_t count = 2 * nv;
for (size_t v = nv - 1; nv > 2; )
{
if ((count--) <= 0)
break;
size_t u = v;
if (nv <= u) u = 0;
v = u + 1;
if (nv <= v) v = 0;
size_t w = v + 1;
if (nv <= w) w = 0;
if (snip(u, v, w, nv, indices, epsilon))
{
size_t a = indices[u];
size_t b = indices[v];
size_t c = indices[w];
if (flipped)
{
tri_indices.push_back(vertices[c].index);
tri_indices.push_back(vertices[b].index);
tri_indices.push_back(vertices[a].index);
}
else
{
tri_indices.push_back(vertices[a].index);
tri_indices.push_back(vertices[b].index);
tri_indices.push_back(vertices[c].index);
}
size_t s = v, t = v + 1;
for ( ; t < nv; ++s, ++t)
indices[s] = indices[t];
nv--;
count = 2 * nv;
}
}
}
return (long)tri_indices.size() / 3;
}
