1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
|
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
#include "main.h"
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/QR>
template<typename LineType> void parametrizedline(const LineType& _line)
{
/* this test covers the following files:
ParametrizedLine.h
*/
using std::abs;
const Index dim = _line.dim();
typedef typename LineType::Scalar Scalar;
typedef typename NumTraits<Scalar>::Real RealScalar;
typedef Matrix<Scalar, LineType::AmbientDimAtCompileTime, 1> VectorType;
typedef Hyperplane<Scalar,LineType::AmbientDimAtCompileTime> HyperplaneType;
typedef Matrix<Scalar, HyperplaneType::AmbientDimAtCompileTime,
HyperplaneType::AmbientDimAtCompileTime> MatrixType;
VectorType p0 = VectorType::Random(dim);
VectorType p1 = VectorType::Random(dim);
VectorType d0 = VectorType::Random(dim).normalized();
LineType l0(p0, d0);
Scalar s0 = internal::random<Scalar>();
Scalar s1 = abs(internal::random<Scalar>());
VERIFY_IS_MUCH_SMALLER_THAN( l0.distance(p0), RealScalar(1) );
VERIFY_IS_MUCH_SMALLER_THAN( l0.distance(p0+s0*d0), RealScalar(1) );
VERIFY_IS_APPROX( (l0.projection(p1)-p1).norm(), l0.distance(p1) );
VERIFY_IS_MUCH_SMALLER_THAN( l0.distance(l0.projection(p1)), RealScalar(1) );
VERIFY_IS_APPROX( Scalar(l0.distance((p0+s0*d0) + d0.unitOrthogonal() * s1)), s1 );
// casting
const int Dim = LineType::AmbientDimAtCompileTime;
typedef typename GetDifferentType<Scalar>::type OtherScalar;
ParametrizedLine<OtherScalar,Dim> hp1f = l0.template cast<OtherScalar>();
VERIFY_IS_APPROX(hp1f.template cast<Scalar>(),l0);
ParametrizedLine<Scalar,Dim> hp1d = l0.template cast<Scalar>();
VERIFY_IS_APPROX(hp1d.template cast<Scalar>(),l0);
// intersections
VectorType p2 = VectorType::Random(dim);
VectorType n2 = VectorType::Random(dim).normalized();
HyperplaneType hp(p2,n2);
Scalar t = l0.intersectionParameter(hp);
VectorType pi = l0.pointAt(t);
VERIFY_IS_MUCH_SMALLER_THAN(hp.signedDistance(pi), RealScalar(1));
VERIFY_IS_MUCH_SMALLER_THAN(l0.distance(pi), RealScalar(1));
VERIFY_IS_APPROX(l0.intersectionPoint(hp), pi);
// transform
if (!NumTraits<Scalar>::IsComplex)
{
MatrixType rot = MatrixType::Random(dim,dim).householderQr().householderQ();
DiagonalMatrix<Scalar,LineType::AmbientDimAtCompileTime> scaling(VectorType::Random());
Translation<Scalar,LineType::AmbientDimAtCompileTime> translation(VectorType::Random());
while(scaling.diagonal().cwiseAbs().minCoeff()<RealScalar(1e-4)) scaling.diagonal() = VectorType::Random();
LineType l1 = l0;
VectorType p3 = l0.pointAt(Scalar(1));
VERIFY_IS_MUCH_SMALLER_THAN( l1.transform(rot).distance(rot * p3), Scalar(1) );
l1 = l0;
VERIFY_IS_MUCH_SMALLER_THAN( l1.transform(rot,Isometry).distance(rot * p3), Scalar(1) );
l1 = l0;
VERIFY_IS_MUCH_SMALLER_THAN( l1.transform(rot*scaling).distance((rot*scaling) * p3), Scalar(1) );
l1 = l0;
VERIFY_IS_MUCH_SMALLER_THAN( l1.transform(rot*scaling*translation)
.distance((rot*scaling*translation) * p3), Scalar(1) );
l1 = l0;
VERIFY_IS_MUCH_SMALLER_THAN( l1.transform(rot*translation,Isometry)
.distance((rot*translation) * p3), Scalar(1) );
}
}
template<typename Scalar> void parametrizedline_alignment()
{
typedef ParametrizedLine<Scalar,4,AutoAlign> Line4a;
typedef ParametrizedLine<Scalar,4,DontAlign> Line4u;
EIGEN_ALIGN_MAX Scalar array1[16];
EIGEN_ALIGN_MAX Scalar array2[16];
EIGEN_ALIGN_MAX Scalar array3[16+1];
Scalar* array3u = array3+1;
Line4a *p1 = ::new(reinterpret_cast<void*>(array1)) Line4a;
Line4u *p2 = ::new(reinterpret_cast<void*>(array2)) Line4u;
Line4u *p3 = ::new(reinterpret_cast<void*>(array3u)) Line4u;
p1->origin().setRandom();
p1->direction().setRandom();
*p2 = *p1;
*p3 = *p1;
VERIFY_IS_APPROX(p1->origin(), p2->origin());
VERIFY_IS_APPROX(p1->origin(), p3->origin());
VERIFY_IS_APPROX(p1->direction(), p2->direction());
VERIFY_IS_APPROX(p1->direction(), p3->direction());
#if defined(EIGEN_VECTORIZE) && EIGEN_MAX_STATIC_ALIGN_BYTES>0
if(internal::packet_traits<Scalar>::Vectorizable && internal::packet_traits<Scalar>::size<=4)
VERIFY_RAISES_ASSERT((::new(reinterpret_cast<void*>(array3u)) Line4a));
#endif
}
EIGEN_DECLARE_TEST(geo_parametrizedline)
{
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1( parametrizedline(ParametrizedLine<float,2>()) );
CALL_SUBTEST_2( parametrizedline(ParametrizedLine<float,3>()) );
CALL_SUBTEST_2( parametrizedline_alignment<float>() );
CALL_SUBTEST_3( parametrizedline(ParametrizedLine<double,4>()) );
CALL_SUBTEST_3( parametrizedline_alignment<double>() );
CALL_SUBTEST_4( parametrizedline(ParametrizedLine<std::complex<double>,5>()) );
}
}
|
