1 /*
2  * Copyright (C) 2012 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  *     http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 
18 #include <gtest/gtest.h>
19 
20 #include "ignition/math/Vector3.hh"
21 #include "ignition/math/Spline.hh"
22 
23 using namespace ignition;
24 
25 /////////////////////////////////////////////////
TEST(SplineTest,Spline)26 TEST(SplineTest, Spline)
27 {
28   math::Spline s;
29 
30   s.AddPoint(math::Vector3d(0, 0, 0));
31   EXPECT_EQ(static_cast<unsigned int>(1), s.PointCount());
32 
33   s.Clear();
34   EXPECT_EQ(static_cast<unsigned int>(0), s.PointCount());
35 
36   s.AddPoint(math::Vector3d(0, 0, 0));
37   EXPECT_TRUE(s.Point(0) == math::Vector3d(0, 0, 0));
38   s.AddPoint(math::Vector3d(1, 1, 1));
39   EXPECT_TRUE(s.Point(1) == math::Vector3d(1, 1, 1));
40 
41   // ::UpdatePoint
42   EXPECT_FALSE(s.UpdatePoint(2, math::Vector3d(2, 2, 2)));
43 
44   EXPECT_TRUE(s.UpdatePoint(1, math::Vector3d(2, 2, 2)));
45   s.AutoCalculate(false);
46   EXPECT_TRUE(s.UpdatePoint(0, math::Vector3d(-1, -1, -1)));
47   s.AutoCalculate(true);
48 
49   // ::Interpolate
50   EXPECT_EQ(s.Interpolate(0.0), math::Vector3d(-1, -1, -1));
51   EXPECT_EQ(s.Interpolate(0.5), math::Vector3d(0.5, 0.5, 0.5));
52   EXPECT_EQ(s.Interpolate(1.0), math::Vector3d(2, 2, 2));
53 
54   // ::Interpolate
55   s.AddPoint(math::Vector3d(4, 4, 4));
56   EXPECT_EQ(s.Interpolate(1, 0.2), math::Vector3d(2.496, 2.496, 2.496));
57 }
58 
59 /////////////////////////////////////////////////
TEST(SplineTest,FixedTangentSpline)60 TEST(SplineTest, FixedTangentSpline)
61 {
62   math::Spline s;
63 
64   // ::AddPoint
65   s.AutoCalculate(false);
66   s.AddPoint(math::Vector3d(0, 0, 0));
67   s.AddPoint(math::Vector3d(0, 0.5, 0), math::Vector3d(0, 1, 0));
68   s.AddPoint(math::Vector3d(0.5, 1, 0), math::Vector3d(1, 0, 0));
69   s.AddPoint(math::Vector3d(1, 1, 0), math::Vector3d(1, 0, 0));
70 
71   // ::UpdatePoint
72   s.UpdatePoint(0, math::Vector3d(0, 0, 0), math::Vector3d(0, 1, 0));
73 
74   s.AutoCalculate(true);
75 
76   s.RecalcTangents();
77 
78   // ::Interpolate
79   EXPECT_EQ(s.Interpolate(0, 0.5), math::Vector3d(0, 0.25, 0));
80   EXPECT_EQ(s.InterpolateTangent(0, 0.5), math::Vector3d(0, 0.25, 0));
81   EXPECT_EQ(s.Interpolate(1, 0.5), math::Vector3d(0.125, 0.875, 0));
82   EXPECT_EQ(s.Interpolate(2, 0.5), math::Vector3d(0.75, 1, 0));
83   EXPECT_EQ(s.InterpolateTangent(2, 0.5), math::Vector3d(0.25, 0, 0));
84 }
85 
86 /////////////////////////////////////////////////
TEST(SplineTest,ArcLength)87 TEST(SplineTest, ArcLength)
88 {
89   math::Spline s;
90   EXPECT_FALSE(std::isfinite(s.ArcLength()));
91   s.AddPoint(math::Vector3d(1, 1, 1), math::Vector3d(1, 1, 1));
92   EXPECT_FALSE(std::isfinite(s.ArcLength(0)));
93   s.AddPoint(math::Vector3d(3, 3, 3), math::Vector3d(1, 1, 1));
94   s.AddPoint(math::Vector3d(4, 4, 4), math::Vector3d(1, 1, 1));
95   EXPECT_NEAR(s.ArcLength(0, 1.0), 3.46410161513775, 1e-14);
96   EXPECT_NEAR(s.ArcLength(), 5.19615242270663, 1e-14);
97   EXPECT_DOUBLE_EQ(s.ArcLength(), s.ArcLength(1.0));
98   EXPECT_FALSE(std::isfinite(s.ArcLength(-1.0)));
99   EXPECT_FALSE(std::isfinite(s.ArcLength(4, 0.0)));
100 }
101 
102 /////////////////////////////////////////////////
TEST(SplineTest,Tension)103 TEST(SplineTest, Tension)
104 {
105   math::Spline s;
106   s.Tension(0.1);
107 
108   EXPECT_DOUBLE_EQ(s.Tension(), 0.1);
109 }
110 
111 /////////////////////////////////////////////////
TEST(SplineTest,Interpolate)112 TEST(SplineTest, Interpolate)
113 {
114   math::Spline s;
115   EXPECT_NO_THROW(s.Interpolate(0, 0.1));
116   EXPECT_FALSE(s.Interpolate(0, 0.1).IsFinite());
117 
118   s.AddPoint(math::Vector3d(0, 0, 0));
119   EXPECT_EQ(s.Interpolate(0, 0.1), math::Vector3d(0, 0, 0));
120   EXPECT_FALSE(s.InterpolateTangent(0.1).IsFinite());
121 
122   s.AddPoint(math::Vector3d(1, 2, 3));
123   EXPECT_EQ(s.Interpolate(0, 0.0), s.Point(0));
124   EXPECT_FALSE(s.Interpolate(0, -0.1).IsFinite());
125   EXPECT_EQ(s.InterpolateTangent(0, 0.0), s.Tangent(0));
126 
127   // Fast and slow call variations
128   EXPECT_EQ(s.Interpolate(0, 0.5), math::Vector3d(0.5, 1.0, 1.5));
129   EXPECT_EQ(s.Interpolate(0, 1.0), s.Point(1));
130   EXPECT_EQ(s.InterpolateTangent(0, 0.5), math::Vector3d(1.25, 2.5, 3.75));
131   EXPECT_EQ(s.InterpolateTangent(0, 1.0), s.Tangent(1));
132   EXPECT_EQ(s.InterpolateMthDerivative(2, 0.5), math::Vector3d(0, 0, 0));
133   EXPECT_EQ(s.InterpolateMthDerivative(2, 1.0), math::Vector3d(-3, -6, -9));
134   EXPECT_EQ(s.InterpolateMthDerivative(3, 0.5), math::Vector3d(-6, -12, -18));
135   EXPECT_EQ(s.InterpolateMthDerivative(3, 1.0), math::Vector3d(-6, -12, -18));
136   EXPECT_EQ(s.InterpolateMthDerivative(4, 0.5), math::Vector3d(0, 0, 0));
137   EXPECT_EQ(s.InterpolateMthDerivative(4, 1.0), math::Vector3d(0, 0, 0));
138 }
139 
140 /////////////////////////////////////////////////
TEST(SplineTest,Point)141 TEST(SplineTest, Point)
142 {
143   math::Spline s;
144   EXPECT_NO_THROW(s.Point(0));
145   EXPECT_FALSE(s.Point(0).IsFinite());
146 }
147 
148 /////////////////////////////////////////////////
TEST(SplineTest,Tangent)149 TEST(SplineTest, Tangent)
150 {
151   math::Spline s;
152   EXPECT_NO_THROW(s.Tangent(0));
153   EXPECT_FALSE(s.Tangent(0).IsFinite());
154 
155   s.AddPoint(math::Vector3d(0, 0, 0));
156   EXPECT_NO_THROW(s.Tangent(0));
157   EXPECT_FALSE(s.Tangent(0).IsFinite());
158 
159   s.AddPoint(math::Vector3d(1, 0, 0));
160   EXPECT_EQ(s.Tangent(0), math::Vector3d(0.5, 0, 0));
161 
162   s.AddPoint(math::Vector3d(1, 1, 0), math::Vector3d(-1, 1, 0));
163   EXPECT_EQ(s.Tangent(1), math::Vector3d(0.5, 0.5, 0));
164   EXPECT_EQ(s.Tangent(2), math::Vector3d(-1, 1, 0));
165 }
166 
167 /////////////////////////////////////////////////
TEST(SplineTest,RecalcTangents)168 TEST(SplineTest, RecalcTangents)
169 {
170   math::Spline s;
171   s.AddPoint(math::Vector3d(0, 0, 0));
172   s.AddPoint(math::Vector3d(.4, .4, .4));
173   s.AddPoint(math::Vector3d(0, 0, 0));
174 
175   s.RecalcTangents();
176 
177   EXPECT_EQ(s.Interpolate(0, 0.5), math::Vector3d(0.2, 0.2, 0.2));
178   EXPECT_EQ(s.Interpolate(1, 0.5), math::Vector3d(0.2, 0.2, 0.2));
179 }
180