Skip to content

Added Conditions for Slerp vs Squad interpolation #577

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
sdhar04 wants to merge 14 commits into
base: gz-math7
Choose a base branch
from
Open

Added Conditions for Slerp vs Squad interpolation #577

Show file tree
Hide file tree
Changes from 12 commits
Commits
Show all changes
14 commits
Select commit Hold shift + click to select a range
b6d7673
Added Conditions for Slerp vs Squad interpolation
sdhar04 Feb 7, 2024
9d504f3
Removed bash script
sdhar04 Mar 10, 2024
c9e01c2
Merge branch 'gz-math7' into gz-math7
Mar 28, 2024
039a64a
Addressed lint issues
sdhar04 Mar 28, 2024
7dfe977
Merge branch 'gz-math7' of https://github.com/sdhar04/gz-math into gz…
sdhar04 Mar 28, 2024
cf8b74c
Reduced scope of variables required only for squad interpolation
sdhar04 Mar 28, 2024
76c57b4
Update RotationSpline.cc
sdhar04 Jan 10, 2025
2638fe0
Merge branch 'gz-math7' into gz-math7
sdhar04 Jul 30, 2025
79fda43
Update RotationSpline.cc
sdhar04 Jul 30, 2025
cc558a5
Update RotationSpline_TEST.cc
sdhar04 Jul 30, 2025
1399fd2
Update RotationSpline_TEST.cc
sdhar04 Jul 30, 2025
d854a85
Update RotationSpline_TEST.cc
sdhar04 Jul 31, 2025
28b4321
Changed according to review
sdhar04 Aug 23, 2025
0b0685f
Changes for review
sdhar04 Aug 23, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
22 changes: 19 additions & 3 deletions src/RotationSpline.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,8 @@
* limitations under the License.
*
*/
#include <cmath>

#include "gz/math/Quaternion.hh"
#include "gz/math/RotationSpline.hh"

Expand Down Expand Up @@ -88,11 +90,25 @@ Quaterniond RotationSpline::Interpolate(const unsigned int _fromIndex,
// Use squad using tangents we've already set up
Quaterniond &p = this->dataPtr->points[_fromIndex];
Quaterniond &q = this->dataPtr->points[_fromIndex+1];
Quaterniond &a = this->dataPtr->tangents[_fromIndex];
Quaterniond &b = this->dataPtr->tangents[_fromIndex+1];

Vector3d peu(p.Euler());
Vector3d qeu(q.Euler());

double diffX = abs(peu.X()-qeu.X());
double diffY = abs(peu.Y()-qeu.Y());
double diffZ = abs(peu.Z()-qeu.Z());

// NB interpolate to nearest rotation
return Quaterniond::Squad(_t, p, a, b, q, _useShortestPath);
if ((diffX < 0.16) || (diffY < 0.16) || (diffZ < 0.16))

@azeey azeey Aug 8, 2025

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Comparing euler angles like this doesn't seem like a safe thing to do. I would suggest calculating the auto diffQ = p.Inverse()*q and then doing const double diff = 2*acos(diffQ.W())) and check the threshold against that.

{
return Quaterniond::Slerp(_t, p, q, _useShortestPath);
}
else
{
Quaterniond &a = this->dataPtr->tangents[_fromIndex];
Quaterniond &b = this->dataPtr->tangents[_fromIndex+1];
return Quaterniond::Squad(_t, p, a, b, q, _useShortestPath);
}
}

/////////////////////////////////////////////////
Expand Down
12 changes: 9 additions & 3 deletions src/RotationSpline_TEST.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,8 +51,8 @@ TEST(RotationSplineTest, RotationSpline)
s.AutoCalculate(true);

// ::Interpolate
EXPECT_TRUE(s.Interpolate(0.5) ==
math::Quaterniond(0.998089, 0.0315333, 0.0427683, 0.0315333));
math::Quaterniond expected(0.999181, 0.0184352, 0.030948, 0.0184352);
EXPECT_TRUE(s.Interpolate(0.5).Equal(expected, 1e-6));

// ::Interpolate
s.AddPoint(math::Quaterniond(.4, .4, .4));
Expand All @@ -62,9 +62,15 @@ TEST(RotationSplineTest, RotationSpline)
EXPECT_EQ(s.Interpolate(s.PointCount()-1, 0.2),
s.Point(s.PointCount()-1));
EXPECT_TRUE(s.Interpolate(1, 0.2) ==
math::Quaterniond(0.978787, 0.107618, 0.137159, 0.107618));
math::Quaterniond(0.980579, 0.101786, 0.133208, 0.101786));
EXPECT_EQ(s.Interpolate(1, 0.0), s.Point(1));
EXPECT_EQ(s.Interpolate(1, 1.0), s.Point(2));

// ::Interpolate
s.Clear();
s.AddPoint(math::Quaterniond(0.1, 0, 0));
s.AddPoint(math::Quaterniond(0.2, 0, 0));
EXPECT_EQ(s.Interpolate(0.5), math::Quaterniond(0.15, 0, 0));
}

/////////////////////////////////////////////////
Expand Down