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

Perf-optimized WrapCylinder::_make_spiral_path #3164

Closed
wants to merge 5 commits into from
Closed

Perf-optimized WrapCylinder::_make_spiral_path #3164

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
9a708e0
Refactored WrapCylinder::_calc_spiral_wrap_point for const-correctnes…
Mar 16, 2022
4db52a0
Reimplemented WrapCylinder::_make_spiral_path for perf
Mar 16, 2022
f628028
Cleaned up wrap angle calc
Mar 19, 2022
637b0b8
Cleaned up _make_spiral_path comment
Mar 19, 2022
c51eb7f
Added extremely appropriate TODOs into the wrapping implementation
Mar 19, 2022
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
223 changes: 99 additions & 124 deletions OpenSim/Simulation/Wrap/WrapCylinder.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,11 +44,28 @@ using SimTK::Vec3;
using SimTK::UnitVec3;

static const char* wrapTypeName = "cylinder";
static Vec3 p0(0.0, 0.0, -1.0);
static Vec3 dn(0.0, 0.0, 1.0);
static const Vec3 g_CylinderBottom(0.0, 0.0, -1.0);
static const UnitVec3 g_CylinderDirection(0.0, 0.0, 1.0);
#define MAX_ITERATIONS 100
#define TANGENCY_THRESHOLD (0.1 * SimTK_DEGREE_TO_RADIAN) // find tangency to within 1 degree

// helper function for calling the wrapmath version
static Vec3 GetClosestPointOnLineToPoint(const Vec3& pt,
const Vec3& linePt,
const UnitVec3& lineDir)
{
Vec3 rv;
double t;
WrapMath::GetClosestPointOnLineToPoint(pt, linePt, lineDir, rv, t);
return rv;
}

static double CalcWrapAngle(const UnitVec3& a, const UnitVec3& b, bool reverse)
{
double ang = acos(SimTK::dot(a, b));
return !reverse ? ang : -(2.0*SimTK_PI - ang);
Copy link
Member

Choose a reason for hiding this comment

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

I think I see the logic but return reverse ? (ang - 2.0*SimTK_PI) : ang; might be easier to read. Also not sure what reverse is used for… if it comes from the old line 644

if (far_side_wrap)
    theta = 2.0 * SimTK_PI - theta;

then why is it now ang - 2.0*SimTK_PI rather than 2.0*SimTK_PI - ang?

Copy link
Contributor Author

Choose a reason for hiding this comment

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

For this one, it's because the original code was using a variable called sense to flip the magnitude of the angle so, in effect, it was being double-flipped by the time it reached the rotation step.

The reason I reversed the if was a silly premature optimization: most compilers have a concept of a "happy" path on branching code (assumed to be the true branch if the compiler can't guess the weighting). The happy branch usually does the comparison and then just keeps moving through the code, the unhappy branch requires a jump (this is why there's the concept of "unlikely" annotations in compilers).

It's not an applicable optimization here because this isn't a short method where skipping a jump and immediately returning would have advantages. I'll reverse it :)

Copy link
Member

Choose a reason for hiding this comment

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

Thanks for the explanation. I wasn't aware of the likely/unlikely attributes.

}

//=============================================================================
// CONSTRUCTOR(S) AND DESTRUCTOR
//=============================================================================
Expand Down Expand Up @@ -208,8 +225,8 @@ int WrapCylinder::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::V
aWrapResult.wrap_pts.setSize(0);

// abort if aPoint1 or aPoint2 is inside the cylinder.
if (WrapMath::CalcDistanceSquaredPointToLine(aPoint1, p0, dn) < r_squared ||
WrapMath::CalcDistanceSquaredPointToLine(aPoint2, p0, dn) < r_squared)
if (WrapMath::CalcDistanceSquaredPointToLine(aPoint1, g_CylinderBottom, g_CylinderDirection) < r_squared ||
WrapMath::CalcDistanceSquaredPointToLine(aPoint2, g_CylinderBottom, g_CylinderDirection) < r_squared)
{
return insideRadius;
}
Expand Down Expand Up @@ -242,8 +259,8 @@ int WrapCylinder::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::V
}

// find points p11 & p22 on the cylinder axis closest aPoint1 & aPoint2
WrapMath::GetClosestPointOnLineToPoint(aPoint1, p0, dn, p11, t);
WrapMath::GetClosestPointOnLineToPoint(aPoint2, p0, dn, p22, t);
WrapMath::GetClosestPointOnLineToPoint(aPoint1, g_CylinderBottom, g_CylinderDirection, p11, t);
WrapMath::GetClosestPointOnLineToPoint(aPoint2, g_CylinderBottom, g_CylinderDirection, p22, t);

// find preliminary tangent point candidates r1a & r1b
vv = aPoint1 - p11;
Expand All @@ -259,7 +276,7 @@ int WrapCylinder::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::V

dist = sqrt(r_squared - dist * dist);

uu = dn % vv;
uu = g_CylinderDirection % vv;

for (i = 0; i < 3; i++)
{
Expand All @@ -281,7 +298,7 @@ int WrapCylinder::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::V

dist = sqrt(r_squared - dist * dist);

uu = dn % vv;
uu = g_CylinderDirection % vv;

for (i = 0; i < 3; i++)
{
Expand Down Expand Up @@ -475,7 +492,7 @@ int WrapCylinder::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::V
mpt[i] = aPoint1[i] + t * (aPoint2[i] - aPoint1[i]);

// find closest point on cylinder axis to mpt
WrapMath::GetClosestPointOnLineToPoint(mpt, p0, dn, axispt, t);
WrapMath::GetClosestPointOnLineToPoint(mpt, g_CylinderBottom, g_CylinderDirection, axispt, t);

// find normal of plane through aPoint1, aPoint2, axispt
l1 = aPoint1 - axispt;
Expand All @@ -490,9 +507,9 @@ int WrapCylinder::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::V
// cross plane normal and cylinder axis (each way) to
// get vectors pointing from axispt towards mpt and
// away from mpt (you can't tell which is which yet).
vert1 = dn % plane_normal;
vert1 = g_CylinderDirection % plane_normal;
WrapMath::NormalizeOrZero(vert1, vert1);
vert2 = plane_normal % dn;
vert2 = plane_normal % g_CylinderDirection;
WrapMath::NormalizeOrZero(vert2, vert2);

// now find two potential apex points, one along each vector
Expand Down Expand Up @@ -548,11 +565,11 @@ int WrapCylinder::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::V

for (i = 0; i < 3; i++)
{
r1a[i] = aWrapResult.r1[i] - 10.0 * dn[i];
r2a[i] = aWrapResult.r2[i] - 10.0 * dn[i];
r1a[i] = aWrapResult.r1[i] - 10.0 * g_CylinderDirection[i];
r2a[i] = aWrapResult.r2[i] - 10.0 * g_CylinderDirection[i];

r1b[i] = aWrapResult.r1[i] + 10.0 * dn[i];
r2b[i] = aWrapResult.r2[i] + 10.0 * dn[i];
r1b[i] = aWrapResult.r1[i] + 10.0 * g_CylinderDirection[i];
r2b[i] = aWrapResult.r2[i] + 10.0 * g_CylinderDirection[i];
}

r1_inter = WrapMath::IntersectLineSegPlane(r1a, r1b, plane_normal, d, r1p);
Expand Down Expand Up @@ -594,6 +611,7 @@ int WrapCylinder::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::V
return return_code;
}


//_____________________________________________________________________________
/**
* Calculate the wrapping points along a spiral path on the cylinder from aPoint1
Expand All @@ -605,149 +623,106 @@ int WrapCylinder::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::V
* @param far_side_wrap Boolean indicating if the wrap is the long way around
* @param aWrapResult The result of the wrapping (tangent points, etc.)
*/
void WrapCylinder::_make_spiral_path(SimTK::Vec3& aPoint1,
SimTK::Vec3& aPoint2,
bool far_side_wrap,
WrapResult& aWrapResult) const
void WrapCylinder::_make_spiral_path(const SimTK::Vec3& p1,
const SimTK::Vec3& p2,
bool doFarSideWrap,
Copy link
Member

Choose a reason for hiding this comment

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

Should update comment on line 623 to match change of variable name. (Doxygen-style comments do not belong in the cpp file but that's another story 📘.)

WrapResult& out) const
{
double x, y, t, axial_dist, theta;
Vec3 r1a, r2a, uu, vv, ax, axial_vec, wrap_pt;
double sense = far_side_wrap ? -1.0 : 1.0;
int i, iterations = 0;
const double _radius = get_radius();
const double radius = get_radius();

int iterations = 0;
restart_spiral_wrap:

aWrapResult.wrap_pts.setSize(0);

// determine the axial vector
const Vec3 r1SurfacePos = out.r1;
const Vec3 r1AxialPos = GetClosestPointOnLineToPoint(r1SurfacePos, g_CylinderBottom, g_CylinderDirection);
const Vec3 r2SurfacePos = out.r2;
const Vec3 r2AxialPos = GetClosestPointOnLineToPoint(r2SurfacePos, g_CylinderBottom, g_CylinderDirection);

WrapMath::GetClosestPointOnLineToPoint(aWrapResult.r1, p0, dn, r1a, t);
WrapMath::GetClosestPointOnLineToPoint(aWrapResult.r2, p0, dn, r2a, t);
const Vec3 r1AxisToSurface = r1SurfacePos - r1AxialPos;
const UnitVec3 r1AxisToSurfaceDir(r1AxisToSurface);
const UnitVec3 r2AxisToSurfaceDir(r2SurfacePos - r2AxialPos);

axial_vec = r2a - r1a;
const Vec3 r1AxisToR2Axis = r2AxialPos - r1AxialPos;

axial_dist = axial_vec.norm();
const double theta = CalcWrapAngle(
r1AxisToSurfaceDir,
r2AxisToSurfaceDir,
doFarSideWrap);

// determine the radial angle
uu = aWrapResult.r1 - r1a;
vv = aWrapResult.r2 - r2a;

for (i = 0; i < 3; i++)
// use Pythagoras to calculate the length of the spiral path (imagine
// a right angle where one edge is a line around the circumference and
// the other edge is a line along the cylinder axis at a right angle)
{
uu[i] /= _radius;
vv[i] /= _radius;
const double x = radius * theta; // circumference
const double y = r1AxisToR2Axis.norm();
out.wrap_path_length = sqrt(x*x + y*y);
}

theta = acos((~uu * vv) / (uu.norm() * vv.norm()));

if (far_side_wrap)
theta = 2.0 * SimTK_PI - theta;

// use Pythagoras to calculate the length of the spiral path (imaging
// a right triangle wrapping around the surface of a cylinder)
x = _radius * theta;
y = axial_dist;
const UnitVec3 ax{SimTK::cross(r1AxisToSurfaceDir, r2AxisToSurfaceDir)};

aWrapResult.wrap_path_length = sqrt(x * x + y * y);

// build path segments
ax = uu % vv;
WrapMath::NormalizeOrZero(ax, ax);

vv = ax % uu;
auto CalcWrapPoint = [&](double t) // t = 0.0 to 1.0
{
SimTK::Mat33 R = SimTK::Rotation{t * theta, ax};
return r1AxialPos + R*r1AxisToSurface + t*r1AxisToR2Axis;
};

SimTK::Rotation m;
m.set(0, 0, ax[0]); m.set(0, 1, ax[1]); m.set(0, 2, ax[2]);
m.set(1, 0, uu[0]); m.set(1, 1, uu[1]); m.set(1, 2, uu[2]);
m.set(2, 0, vv[0]); m.set(2, 1, vv[1]); m.set(2, 2, vv[2]);
// WrapTorus creates a WrapCyl with no connected model, avoid this hack
if (!_model.empty() && !getModel().getDisplayHints().isVisualizationEnabled() &&
aWrapResult.singleWrap) {
if (!_model.empty() &&
!getModel().getDisplayHints().isVisualizationEnabled() &&
out.singleWrap) {

// Use one WrapSegment/cord instead of finely sampled list of wrap_pt(s)
_calc_spiral_wrap_point(
r1a, axial_vec, m, ax, sense, 0, theta, wrap_pt);
aWrapResult.wrap_pts.append(wrap_pt);
_calc_spiral_wrap_point(
r1a, axial_vec, m, ax, sense, 1.0, theta, wrap_pt);
aWrapResult.wrap_pts.append(wrap_pt);
out.wrap_pts.setSize(2);
out.wrap_pts[0] = CalcWrapPoint(0);
out.wrap_pts[1] = CalcWrapPoint(1.0);
}
else {

// Each muscle segment on the surface of the cylinder should be
// 0.002 meters long. This assumes the model is in meters, of course.
int numWrapSegments = (int)(aWrapResult.wrap_path_length / 0.002);
if (numWrapSegments < 1) numWrapSegments = 1;
int numWrapSegments = static_cast<int>(out.wrap_path_length / 0.002);
Copy link
Member

Choose a reason for hiding this comment

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

Suggest marking this hard-coded 0.002 "TODO"

if (numWrapSegments < 1) {
numWrapSegments = 1;
}

out.wrap_pts.setSize(0);

for (i = 0; i < numWrapSegments; i++) {
double t = (double)i / numWrapSegments;
for (int i = 0; i < numWrapSegments; i++) {
double t = static_cast<double>(i)/static_cast<double>(numWrapSegments);

_calc_spiral_wrap_point(
r1a, axial_vec, m, ax, sense, t, theta, wrap_pt);
Vec3 wrap_pt = CalcWrapPoint(t);

// adjust r1/r2 tangent points if necessary to achieve tangency with
// the spiral path:
if (i == 1 && iterations < MAX_ITERATIONS &&
!aWrapResult.singleWrap) {
bool did_adjust_r2 = false;
bool did_adjust_r1 = _adjust_tangent_point(
aPoint1, dn, aWrapResult.r1, wrap_pt);
if (i == 1
&& iterations < MAX_ITERATIONS
&& !out.singleWrap) {

SimTK::Vec3 temp_wrap_pt;
bool did_adjust_r1 = _adjust_tangent_point(
p1,
g_CylinderDirection,
out.r1,
wrap_pt);

_calc_spiral_wrap_point(r1a, axial_vec, m, ax, sense, 1.0 - t,
theta, temp_wrap_pt);
Vec3 temp_wrap_pt = CalcWrapPoint(1.0 - t);

did_adjust_r2 = _adjust_tangent_point(
aPoint2, dn, aWrapResult.r2, temp_wrap_pt);
bool did_adjust_r2 = _adjust_tangent_point(
p2,
g_CylinderDirection,
out.r2,
temp_wrap_pt);
Copy link
Member

Choose a reason for hiding this comment

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

https://imgs.xkcd.com/comics/goto.png

Copy link
Contributor Author

Choose a reason for hiding this comment

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

First time in a long time I've seen a go-to :D

Although on my local branch (which is even more refactored, for a ~15pct boost, by doing stuff like optimized quaternion rotations) I tried to rewrite the code in an iteration loop, which worked fine but (imho) is actually kind of uglier (I hate to admit this) because the original algorithim only logically performs an adjustment if n>=1. It then has to perform two method calls (point adjustment), and this only happens if it's not the final iteration of the loop. Also, the loop block contains the necessary state to finalize the algorithm. This means that you end up with something like a continue (jump) on the adjustment step (the goto) and a break at the bottom of the loop (the fall through), which also felt gross :(


if (did_adjust_r1 || did_adjust_r2) {
iterations++;
goto restart_spiral_wrap;
}
}

aWrapResult.wrap_pts.append(wrap_pt);
out.wrap_pts.append(wrap_pt);
}
}
}

//_____________________________________________________________________________
/**
* Calculate a new point along a spiral wrapping path.
*
* @param r1a An existing point on the spiral path
* @param axial_vec Vector from r1a parallel to cylinder axis
* @param m A transform matrix used for the radial component
* @param axis Axis of the cylinder
* @param sense The direction of the spiral
* @param t Parameterized amount of angle for this point
* @param theta The total angle of the spiral on the cylinder
* @param wrap_pt The new point on the spiral path
*/
void WrapCylinder::_calc_spiral_wrap_point(const SimTK::Vec3& r1a,
const SimTK::Vec3& axial_vec,
const SimTK::Rotation& m,
const SimTK::Vec3& axis,
double sense,
double t,
double theta,
SimTK::Vec3& wrap_pt) const
{
SimTK::Rotation R;
double angle = sense * t * theta;
R.setRotationFromAngleAboutNonUnitVector(angle, axis);

SimTK::Mat33 n = m * ~R;

for (int i = 0; i < 3; i++)
{
double radial_component = get_radius() * n[1][i];
double axial_component = t * axial_vec[i];

wrap_pt[i] = r1a[i] + radial_component + axial_component;
}
}

//_____________________________________________________________________________
/**
* Determine whether the specified tangent point 'r1'
Expand All @@ -762,10 +737,10 @@ void WrapCylinder::_calc_spiral_wrap_point(const SimTK::Vec3& r1a,
* @param w1 A wrapping point (?)
* @return Whether or not the point was adjusted
*/
bool WrapCylinder::_adjust_tangent_point(SimTK::Vec3& pt1,
SimTK::Vec3& dn,
SimTK::Vec3& r1,
SimTK::Vec3& w1) const
bool WrapCylinder::_adjust_tangent_point(const SimTK::Vec3& pt1,
const SimTK::UnitVec3& dn,
SimTK::Vec3& r1,
const SimTK::Vec3& w1) const
{
SimTK::Vec3 pr_vec = r1 - pt1;
SimTK::Vec3 rw_vec = w1 - r1;
Expand Down
25 changes: 9 additions & 16 deletions OpenSim/Simulation/Wrap/WrapCylinder.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -78,22 +78,15 @@ OpenSim_DECLARE_CONCRETE_OBJECT(WrapCylinder, WrapObject);
private:
void constructProperties();

void _make_spiral_path(SimTK::Vec3& aPoint1, SimTK::Vec3& aPoint2,
bool far_side_wrap,WrapResult& aWrapResult) const;
void _calc_spiral_wrap_point(const SimTK::Vec3& r1a,
const SimTK::Vec3& axial_vec,
const SimTK::Rotation& rotation,
const SimTK::Vec3& axis,
double sense,
double t,
double theta,
SimTK::Vec3& wrap_pt) const;


bool _adjust_tangent_point(SimTK::Vec3& pt1,
SimTK::Vec3& dn,
SimTK::Vec3& r1,
SimTK::Vec3& w1) const;
void _make_spiral_path(const SimTK::Vec3& aPoint1,
const SimTK::Vec3& aPoint2,
bool farSideWrap,
WrapResult& aWrapResult) const;

bool _adjust_tangent_point(const SimTK::Vec3& pt1,
const SimTK::UnitVec3& dn,
SimTK::Vec3& r1,
const SimTK::Vec3& w1) const;

//=============================================================================
}; // END of class WrapCylinder
Expand Down
Loading