Skip to content
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

Added Conditions for Slerp vs Squad interpolation #577

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

Added Conditions for Slerp vs Squad interpolation #577

Changes from all commits
Commits
Show all changes
6 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
mjcarroll 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
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 @@ -16,10 +16,12 @@
*/
#include "gz/math/Quaternion.hh"
#include "gz/math/RotationSpline.hh"
#include "cmath"
Comment on lines 16 to +19
Copy link
Contributor

Choose a reason for hiding this comment

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

Suggested change
*/
#include "gz/math/Quaternion.hh"
#include "gz/math/RotationSpline.hh"
#include "cmath"
*/
#include <cmath>
#include "gz/math/Quaternion.hh"
#include "gz/math/RotationSpline.hh"


using namespace gz;
using namespace math;

using namespace std;
Copy link
Contributor

Choose a reason for hiding this comment

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

remove this

/// \internal
/// \brief Private data for RotationSpline
class RotationSpline::Implementation
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))
{
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