void OpenSimContext::setEndPoint(PathWrap& mw, int newEndPt) {
mw.setEndPoint(*_configState, newEndPt );
_configState->invalidateAll(SimTK::Stage::Position);
_model->getMultibodySystem().realize(*_configState, SimTK::Stage::Position);
mw.getPath()->updateGeometry(*_configState);
return;
}
/*
* Create a new wrap instance and add it to the set.
*
* @param aWrapObject The wrap object to use in the new wrap instance.
*/
void GeometryPath::addPathWrap(WrapObject& aWrapObject)
{
PathWrap* newWrap = new PathWrap();
newWrap->setWrapObject(aWrapObject);
newWrap->setMethod(PathWrap::hybrid);
upd_PathWrapSet().adoptAndAppend(newWrap);
finalizeFromProperties();
}
/**
* Calculate the wrapping of one line segment over the ellipsoid.
*
* @param aPoint1 One end of the line segment
* @param aPoint2 The other end of the line segment
* @param aPathWrap An object holding the parameters for this line/ellipsoid pairing
* @param aWrapResult The result of the wrapping (tangent points, etc.)
* @param aFlag A flag for indicating errors, etc.
* @return The status, as a WrapAction enum
*/
int WrapEllipsoid::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::Vec3& aPoint2,
const PathWrap& aPathWrap, WrapResult& aWrapResult, bool& aFlag) const
{
int i, j, bestMu;
SimTK::Vec3 p1, p2, m, a, p1p2, p1m, p2m, f1, f2, p1c1, r1r2, vs, t, mu;
double ppm, aa, bb, cc, disc, l1, l2,
p1e, p2e, vs4, dist, fanWeight = -SimTK::Infinity;
double t_sv[3][3], t_c1[3][3];
bool far_side_wrap = false;
static SimTK::Vec3 origin(0,0,0);
// In case you need any variables from the previous wrap, copy them from
// the PathWrap into the WrapResult, re-normalizing the ones that were
// un-normalized at the end of the previous wrap calculation.
const WrapResult& previousWrap = aPathWrap.getPreviousWrap();
aWrapResult.factor = previousWrap.factor;
for (i = 0; i < 3; i++)
{
aWrapResult.r1[i] = previousWrap.r1[i] * previousWrap.factor;
aWrapResult.r2[i] = previousWrap.r2[i] * previousWrap.factor;
aWrapResult.c1[i] = previousWrap.c1[i];
aWrapResult.sv[i] = previousWrap.sv[i];
}
aFlag = true;
aWrapResult.wrap_pts.setSize(0);
// This algorithm works best if the coordinates (aPoint1, aPoint2,
// origin, _dimensions) are all somewhat close to 1.0. So use
// the ellipsoid dimensions to calculate a multiplication factor that
// will be applied to all of the coordinates. You want to use just
// the ellipsoid dimensions because they do not change from one call to the
// next. You don't want the factor to change because the algorithm uses
// some vectors (r1, r2, c1) from the previous call.
aWrapResult.factor = 3.0 / (_dimensions[0] + _dimensions[1] + _dimensions[2]);
for (i = 0; i < 3; i++)
{
p1[i] = aPoint1[i] * aWrapResult.factor;
p2[i] = aPoint2[i] * aWrapResult.factor;
m[i] = origin[i] * aWrapResult.factor;
a[i] = _dimensions[i] * aWrapResult.factor;
}
p1e = -1.0;
p2e = -1.0;
for (i = 0; i < 3; i++)
{
p1e += SQR((p1[i] - m[i]) / a[i]);
p2e += SQR((p2[i] - m[i]) / a[i]);
}
// check if p1 and p2 are inside the ellipsoid
if (p1e < -0.0001 || p2e < -0.0001)
{
// p1 or p2 is inside the ellipsoid
aFlag = false;
aWrapResult.wrap_path_length = 0.0;
// transform back to starting coordinate system
for (i = 0; i < 3; i++)
{
aWrapResult.r1[i] /= aWrapResult.factor;
aWrapResult.r2[i] /= aWrapResult.factor;
}
return insideRadius;
}
MAKE_3DVECTOR21(p1, p2, p1p2);
MAKE_3DVECTOR21(p1, m, p1m);
Mtx::Normalize(3, p1m, p1m);
MAKE_3DVECTOR21(p2, m, p2m);
Mtx::Normalize(3, p2m, p2m);
ppm = Mtx::DotProduct(3, p1m, p2m) - 1.0; // angle between p1->m and p2->m: -2.0 to 0.0
if (fabs(ppm) < 0.0001)
{
// vector p1m and p2m are collinear
aFlag = false;
aWrapResult.wrap_path_length = 0.0;
// transform back to starting coordinate system
for (i = 0; i < 3; i++)
{
aWrapResult.r1[i] /= aWrapResult.factor;
aWrapResult.r2[i] /= aWrapResult.factor;
}
return noWrap;
}
// check if the line through p1 and p2 intersects the ellipsoid
for (i = 0; i < 3; i++)
{
f1[i] = p1p2[i] / a[i];
f2[i] = (p2[i] - m[i]) / a[i];
}
aa = Mtx::DotProduct(3, f1, f1);
bb = 2.0 * Mtx::DotProduct(3, f1, f2);
cc = Mtx::DotProduct(3, f2, f2) - 1.0;
disc = SQR(bb) - 4.0 * aa * cc;
if (disc < 0.0)
{
// no intersection
aFlag = false;
aWrapResult.wrap_path_length = 0.0;
// transform back to starting coordinate system
for (i = 0; i < 3; i++)
{
aWrapResult.r1[i] /= aWrapResult.factor;
aWrapResult.r2[i] /= aWrapResult.factor;
}
return noWrap;
}
l1 = (-bb + sqrt(disc)) / (2.0 * aa);
l2 = (-bb - sqrt(disc)) / (2.0 * aa);
if ( ! (0.0 < l1 && l1 < 1.0) || ! (0.0 < l2 && l2 < 1.0) )
{
// no intersection
aFlag = false;
aWrapResult.wrap_path_length = 0.0;
// transform back to starting coordinate system
for (i = 0; i < 3; i++)
{
aWrapResult.r1[i] /= aWrapResult.factor;
aWrapResult.r2[i] /= aWrapResult.factor;
}
return noWrap;
}
// r1 & r2: intersection points of p1->p2 with the ellipsoid
for (i = 0; i < 3; i++)
{
aWrapResult.r1[i] = p2[i] + l1 * p1p2[i];
aWrapResult.r2[i] = p2[i] + l2 * p1p2[i];
}
// ==== COMPUTE WRAPPING PLANE (begin) ====
MAKE_3DVECTOR21(aWrapResult.r2, aWrapResult.r1, r1r2);
// (1) Frans technique: choose the most parallel coordinate axis, then set
// 'sv' to the point along the muscle line that crosses the plane where
// that major axis equals zero. This takes advantage of the special-case
// handling in pt_to_ellipsoid() that reduces the 3d point-to-ellipsoid
// problem to a 2d point-to-ellipse problem. The 2d case returns a nice
// c1 in situations where the "fan" has a sharp discontinuity.
Mtx::Normalize(3, p1p2, mu);
for (i = 0; i < 3; i++)
{
mu[i] = fabs(mu[i]);
t[i] = (m[i] - aWrapResult.r1[i]) / r1r2[i];
for (j = 0; j < 3; j++)
t_sv[i][j] = aWrapResult.r1[j] + t[i] * r1r2[j];
findClosestPoint(a[0], a[1], a[2], t_sv[i][0], t_sv[i][1], t_sv[i][2], &t_c1[i][0], &t_c1[i][1], &t_c1[i][2], i);
}
// pick most parallel major axis
for (bestMu = 0, i = 1; i < 3; i++)
if (mu[i] > mu[bestMu])
bestMu = i;
if (aPathWrap.getMethod() == PathWrap::hybrid ||
aPathWrap.getMethod() == PathWrap::axial)
{
if (aPathWrap.getMethod() == PathWrap::hybrid && mu[bestMu] > MU_BLEND_MIN)
{
// If Frans' technique produces an sv that is not within the r1->r2
// line segment, then that means that sv will be outside the ellipsoid.
// This can create an sv->c1 vector that points roughly 180-degrees
// opposite to the fan solution's sv->c1 vector. This creates problems
// when interpolating between the Frans and fan solutions because the
// interpolated c1 can become collinear to the muscle line during
// interpolation. Therefore we detect Frans-solution sv points near
// the ends of r1->r2 here, and fade out the Frans result for them.
double s = 1.0;
if (t[bestMu] < 0.0 || t[bestMu] > 1.0)
s = 0.0;
else if (t[bestMu] < SV_BOUNDARY_BLEND)
s = t[bestMu] / SV_BOUNDARY_BLEND;
else if (t[bestMu] > (1.0 - SV_BOUNDARY_BLEND))
s = (1.0 - t[bestMu]) / SV_BOUNDARY_BLEND;
if (s < 1.0)
mu[bestMu] = MU_BLEND_MIN + s * (mu[bestMu] - MU_BLEND_MIN);
}
if (aPathWrap.getMethod() == PathWrap::axial || mu[bestMu] > MU_BLEND_MIN)
{
// if the Frans solution produced a strong result, copy it into
// sv and c1.
for (i = 0; i < 3; i++)
{
aWrapResult.c1[i] = t_c1[bestMu][i];
aWrapResult.sv[i] = t_sv[bestMu][i];
}
}
if (aPathWrap.getMethod() == PathWrap::hybrid && mu[bestMu] < MU_BLEND_MAX)
{
// (2) Fan technique: sample the fan at fixed intervals and average the
// fan "blade" vectors together to determine c1. This only works when
// the fan is smoothly continuous. The sharper the discontinuity, the
// more jumpy c1 becomes.
SimTK::Vec3 v_sum(0,0,0);
for (i = 0; i < 3; i++)
t_sv[2][i] = aWrapResult.r1[i] + 0.5 * r1r2[i];
for (i = 1; i < NUM_FAN_SAMPLES - 1; i++)
{
SimTK::Vec3 v;
double tt = (double) i / NUM_FAN_SAMPLES;
for (j = 0; j < 3; j++)
t_sv[0][j] = aWrapResult.r1[j] + tt * r1r2[j];
findClosestPoint(a[0], a[1], a[2], t_sv[0][0], t_sv[0][1], t_sv[0][2], &t_c1[0][0], &t_c1[0][1], &t_c1[0][2]);
MAKE_3DVECTOR21(t_c1[0], t_sv[0], v);
Mtx::Normalize(3, v, v);
// add sv->c1 "fan blade" vector to the running total
for (j = 0; j < 3; j++)
v_sum[j] += v[j];
}
// use vector sum to determine c1
Mtx::Normalize(3, v_sum, v_sum);
for (i = 0; i < 3; i++)
t_c1[0][i] = t_sv[2][i] + v_sum[i];
if (mu[bestMu] <= MU_BLEND_MIN)
{
findClosestPoint(a[0], a[1], a[2], t_c1[0][0], t_c1[0][1], t_c1[0][2], &aWrapResult.c1[0], &aWrapResult.c1[1], &aWrapResult.c1[2]);
for (i = 0; i < 3; i++)
aWrapResult.sv[i] = t_sv[2][i];
fanWeight = 1.0;
}
else
{
double tt = (mu[bestMu] - MU_BLEND_MIN) / (MU_BLEND_MAX - MU_BLEND_MIN);
double oneMinusT = 1.0 - tt;
findClosestPoint(a[0], a[1], a[2], t_c1[0][0], t_c1[0][1], t_c1[0][2], &t_c1[1][0], &t_c1[1][1], &t_c1[1][2]);
for (i = 0; i < 3; i++)
{
t_c1[2][i] = tt * aWrapResult.c1[i] + oneMinusT * t_c1[1][i];
aWrapResult.sv[i] = tt * aWrapResult.sv[i] + oneMinusT * t_sv[2][i];
}
findClosestPoint(a[0], a[1], a[2], t_c1[2][0], t_c1[2][1], t_c1[2][2], &aWrapResult.c1[0], &aWrapResult.c1[1], &aWrapResult.c1[2]);
fanWeight = oneMinusT;
}
}
}
else // method == midpoint
{
for (i = 0; i < 3; i++)
aWrapResult.sv[i] = aWrapResult.r1[i] + 0.5 * (aWrapResult.r2[i] - aWrapResult.r1[i]);
findClosestPoint(a[0], a[1], a[2], aWrapResult.sv[0], aWrapResult.sv[1], aWrapResult.sv[2], &aWrapResult.c1[0], &aWrapResult.c1[1], &aWrapResult.c1[2]);
}
// ==== COMPUTE WRAPPING PLANE (end) ====
// The old way of initializing r1 used the intersection point
// of p1p2 and the ellipsoid. This caused the muscle path to
// "jump" to the other side of the ellipsoid as sv[] came near
// a plane of the ellipsoid. It jumped to the other side while
// c1[] was still on the first side. The new way of initializing
// r1 sets it to c1 so that it will stay on c1's side of the
// ellipsoid.
{
bool use_c1_to_find_tangent_pts = true;
if (aPathWrap.getMethod() == PathWrap::axial)
use_c1_to_find_tangent_pts = (bool) (t[bestMu] > 0.0 && t[bestMu] < 1.0);
if (use_c1_to_find_tangent_pts)
for (i = 0; i < 3; i++)
aWrapResult.r1[i] = aWrapResult.r2[i] = aWrapResult.c1[i];
}
// if wrapping is constrained to one half of the ellipsoid,
// check to see if we need to flip c1 to the active side of
// the ellipsoid.
if (_wrapSign != 0)
{
dist = aWrapResult.c1[_wrapAxis] - m[_wrapAxis];
if (DSIGN(dist) != _wrapSign)
{
SimTK::Vec3 orig_c1=aWrapResult.c1;
aWrapResult.c1[_wrapAxis] = - aWrapResult.c1[_wrapAxis];
aWrapResult.r1 = aWrapResult.r2 = aWrapResult.c1;
if (EQUAL_WITHIN_ERROR(fanWeight, -SimTK::Infinity))
fanWeight = 1.0 - (mu[bestMu] - MU_BLEND_MIN) / (MU_BLEND_MAX - MU_BLEND_MIN);
if (fanWeight > 1.0)
fanWeight = 1.0;
if (fanWeight > 0.0)
{
SimTK::Vec3 tc1;
double bisection = (orig_c1[_wrapAxis] + aWrapResult.c1[_wrapAxis]) / 2.0;
aWrapResult.c1[_wrapAxis] = aWrapResult.c1[_wrapAxis] + fanWeight * (bisection - aWrapResult.c1[_wrapAxis]);
tc1 = aWrapResult.c1;
findClosestPoint(a[0], a[1], a[2], tc1[0], tc1[1], tc1[2], &aWrapResult.c1[0], &aWrapResult.c1[1], &aWrapResult.c1[2]);
}
}
}
// use p1, p2, and c1 to create parameters for the wrapping plane
MAKE_3DVECTOR21(p1, aWrapResult.c1, p1c1);
Mtx::CrossProduct(p1p2, p1c1, vs);
Mtx::Normalize(3, vs, vs);
vs4 = - Mtx::DotProduct(3, vs, aWrapResult.c1);
// find r1 & r2 by starting at c1 moving toward p1 & p2
calcTangentPoint(p1e, aWrapResult.r1, p1, m, a, vs, vs4);
calcTangentPoint(p2e, aWrapResult.r2, p2, m, a, vs, vs4);
// create a series of line segments connecting r1 & r2 along the
// surface of the ellipsoid.
calc_wrap_path:
CalcDistanceOnEllipsoid(aWrapResult.r1, aWrapResult.r2, m, a, vs, vs4, far_side_wrap, aWrapResult);
if (_wrapSign != 0 && aWrapResult.wrap_pts.getSize() > 2 && ! far_side_wrap)
{
SimTK::Vec3 r1p1, r2p2, r1w1, r2w2;
SimTK::Vec3& w1 = aWrapResult.wrap_pts.updElt(1);
SimTK::Vec3& w2 = aWrapResult.wrap_pts.updElt(aWrapResult.wrap_pts.getSize() - 2);
// check for wrong-way wrap by testing angle of first and last
// wrap path segments:
MAKE_3DVECTOR(aWrapResult.r1, p1, r1p1);
MAKE_3DVECTOR(aWrapResult.r1, w1, r1w1);
MAKE_3DVECTOR(aWrapResult.r2, p2, r2p2);
MAKE_3DVECTOR(aWrapResult.r2, w2, r2w2);
Mtx::Normalize(3, r1p1, r1p1);
Mtx::Normalize(3, r1w1, r1w1);
Mtx::Normalize(3, r2p2, r2p2);
Mtx::Normalize(3, r2w2, r2w2);
if (Mtx::DotProduct(3, r1p1, r1w1) > 0.0 || Mtx::DotProduct(3, r2p2, r2w2) > 0.0)
{
// NOTE: I added the ability to call CalcDistanceOnEllipsoid() a 2nd time in this
// situation to force a far-side wrap instead of aborting the
// wrap. -- KMS 9/3/99
far_side_wrap = true;
goto calc_wrap_path;
}
}
// unfactor the output coordinates
aWrapResult.wrap_path_length /= aWrapResult.factor;
for (i = 0; i < aWrapResult.wrap_pts.getSize(); i++)
aWrapResult.wrap_pts[i] *= (1.0 / aWrapResult.factor);
// transform back to starting coordinate system
// Note: c1 and sv do not get transformed
for (i = 0; i < 3; i++)
{
aWrapResult.r1[i] /= aWrapResult.factor;
aWrapResult.r2[i] /= aWrapResult.factor;
}
return mandatoryWrap;
}
/**
* Calculate the wrapping of one line segment over the sphere.
*
* @param aPoint1 One end of the line segment
* @param aPoint2 The other end of the line segment
* @param aPathWrap An object holding the parameters for this line/sphere pairing
* @param aWrapResult The result of the wrapping (tangent points, etc.)
* @param aFlag A flag for indicating errors, etc.
* @return The status, as a WrapAction enum
*/
int WrapSphere::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::Vec3& aPoint2,
const PathWrap& aPathWrap, WrapResult& aWrapResult, bool& aFlag) const
{
double l1, l2, disc, a, b, c, a1, a2, j1, j2, j3, j4, r1r2, ra[3][3], rrx[3][3], aa[3][3], mat[4][4],
axis[4], vec[4], rotvec[4], angle, *r11, *r22;
Vec3 ri, p2m, p1m, mp, r1n, r2n,
p1p2, np2, hp2, r1m, r2m, y, z, n, r1a, r2a,
r1b, r2b, r1am, r2am, r1bm, r2bm;
int i, j, maxit, return_code = wrapped;
bool far_side_wrap = false;
static SimTK::Vec3 origin(0,0,0);
// In case you need any variables from the previous wrap, copy them from
// the PathWrap into the WrapResult, re-normalizing the ones that were
// un-normalized at the end of the previous wrap calculation.
const WrapResult& previousWrap = aPathWrap.getPreviousWrap();
aWrapResult.factor = previousWrap.factor;
for (i = 0; i < 3; i++)
{
aWrapResult.r1[i] = previousWrap.r1[i] * previousWrap.factor;
aWrapResult.r2[i] = previousWrap.r2[i] * previousWrap.factor;
aWrapResult.c1[i] = previousWrap.c1[i];
aWrapResult.sv[i] = previousWrap.sv[i];
}
maxit = 50;
aFlag = true;
aWrapResult.wrap_pts.setSize(0);
for (i = 0; i < 3; i++) {
p1m[i] = aPoint1[i] - origin[i];
p2m[i] = aPoint2[i] - origin[1];
ri[i] = aPoint1[i] - aPoint2[i];
mp[i] = origin[i] - aPoint2[i];
p1p2[i] = aPoint1[i] - aPoint2[i];
}
// check that neither point is inside the radius of the sphere
if (Mtx::Magnitude(3, p1m) < _radius || Mtx::Magnitude(3, p2m) < _radius)
return insideRadius;
a = Mtx::DotProduct(3, ri, ri);
b = -2.0 * Mtx::DotProduct(3, mp, ri);
c = Mtx::DotProduct(3, mp, mp) - _radius * _radius;
disc = b * b - 4.0 * a * c;
// check if there is an intersection of p1p2 and the sphere
if (disc < 0.0)
{
aFlag = false;
aWrapResult.wrap_path_length = 0.0;
return noWrap;
}
l1 = (-b + sqrt(disc)) / (2.0 * a);
l2 = (-b - sqrt(disc)) / (2.0 * a);
// check if the intersection is between p1 and p2
if ( ! (0.0 < l1 && l1 < 1.0) || ! (0.0 < l2 && l2 < 1.0))
{
aFlag = false;
aWrapResult.wrap_path_length = 0.0;
return noWrap;
}
if (l1 < l2)
{
aFlag = false;
aWrapResult.wrap_path_length = 0.0;
return noWrap;
}
Mtx::Normalize(3, p1p2, p1p2);
Mtx::Normalize(3, p2m, np2);
Mtx::CrossProduct(p1p2, np2, hp2);
// if the muscle line passes too close to the center of the sphere
// then give up
if (Mtx::Magnitude(3, hp2) < 0.00001) {
// JPL 12/28/06: r1 and r2 from the previous wrap have already
// been copied into aWrapResult (and not yet overwritten). So
// just go directly to calc_path.
#if 0
// no wait! don't give up! Instead use the previous r1 & r2:
// -- added KMS 9/9/99
//
const WrapResult& previousWrap = aPathWrap.getPreviousWrap();
for (i = 0; i < 3; i++) {
aWrapResult.r1[i] = previousWrap.r1[i];
aWrapResult.r2[i] = previousWrap.r2[i];
}
#endif
goto calc_path;
}
// calc tangent point candidates r1a, r1b
Mtx::Normalize(3, hp2, n);
for (i = 0; i < 3; i++)
y[i] = origin[i] - aPoint1[i];
Mtx::Normalize(3, y, y);
Mtx::CrossProduct(n, y, z);
for (i = 0; i < 3; i++)
{
ra[i][0] = n[i];
ra[i][1] = y[i];
ra[i][2] = z[i];
}
a1 = asin(_radius / Mtx::Magnitude(3, p1m));
WrapMath::Make3x3DirCosMatrix(a1, rrx);
Mtx::Multiply(3, 3, 3, (double*)ra, (double*)rrx, (double*)aa);
// TODO: test that this gives same result as SIMM code
for (i = 0; i < 3; i++)
r1a[i] = aPoint1[i] + aa[i][1] * Mtx::Magnitude(3, p1m) * cos(a1);
WrapMath::Make3x3DirCosMatrix(-a1, rrx);
Mtx::Multiply(3, 3, 3, (double*)ra, (double*)rrx, (double*)aa);
for (i = 0; i < 3; i++)
r1b[i] = aPoint1[i] + aa[i][1] * Mtx::Magnitude(3, p1m) * cos(a1);
// calc tangent point candidates r2a, r2b
for (i = 0; i < 3; i++)
y[i] = origin[i] - aPoint2[i];
Mtx::Normalize(3, y, y);
Mtx::CrossProduct(n, y, z);
for (i = 0; i < 3; i++)
{
ra[i][0] = n[i];
ra[i][1] = y[i];
ra[i][2] = z[i];
}
a2 = asin(_radius / Mtx::Magnitude(3, p2m));
WrapMath::Make3x3DirCosMatrix(a2, rrx);
Mtx::Multiply(3, 3, 3, (double*)ra, (double*)rrx, (double*)aa);
for (i = 0; i < 3; i++)
r2a[i] = aPoint2[i] + aa[i][1] * Mtx::Magnitude(3, p2m) * cos(a2);
WrapMath::Make3x3DirCosMatrix(-a2, rrx);
Mtx::Multiply(3, 3, 3, (double*)ra, (double*)rrx, (double*)aa);
for (i = 0; i < 3; i++)
r2b[i] = aPoint2[i] + aa[i][1] * Mtx::Magnitude(3, p2m) * cos(a2);
// determine wrapping tangent points r1 & r2
for (i = 0; i < 3; i++) {
r1am[i] = r1a[i] - origin[i];
r1bm[i] = r1b[i] - origin[i];
r2am[i] = r2a[i] - origin[i];
r2bm[i] = r2b[i] - origin[i];
}
Mtx::Normalize(3, r1am, r1am);
Mtx::Normalize(3, r1bm, r1bm);
Mtx::Normalize(3, r2am, r2am);
Mtx::Normalize(3, r2bm, r2bm);
{
// check which of the tangential points results in the shortest distance
j1 = Mtx::DotProduct(3, r1am, r2am);
j2 = Mtx::DotProduct(3, r1am, r2bm);
j3 = Mtx::DotProduct(3, r1bm, r2am);
j4 = Mtx::DotProduct(3, r1bm, r2bm);
if (j1 > j2 && j1 > j3 && j1 > j4)
{
for (i = 0; i < 3; i++) {
aWrapResult.r1[i] = r1a[i];
aWrapResult.r2[i] = r2a[i];
}
r11 = &r1b[0];
r22 = &r2b[0];
}
else if (j2 > j3 && j2 > j4)
{
for (i = 0; i < 3; i++) {
aWrapResult.r1[i] = r1a[i];
aWrapResult.r2[i] = r2b[i];
}
r11 = &r1b[0];
r22 = &r2a[0];
}
else if (j3 > j4)
{
for (i = 0; i < 3; i++) {
aWrapResult.r1[i] = r1b[i];
aWrapResult.r2[i] = r2a[i];
}
r11 = &r1a[0];
r22 = &r2b[0];
}
else
{
for (i = 0; i < 3; i++) {
aWrapResult.r1[i] = r1b[i];
aWrapResult.r2[i] = r2b[i];
}
r11 = &r1a[0];
r22 = &r2a[0];
}
}
if (_wrapSign != 0)
{
if (DSIGN(aPoint1[_wrapAxis]) == _wrapSign || DSIGN(aPoint2[_wrapAxis]) == _wrapSign)
{
double tt, r_squared = _radius * _radius;
Vec3 mm;
// If either muscle point is on the constrained side, then check for intersection
// of the muscle line and the cylinder. If there is an intersection, then
// you've found a mandatory wrap. If not, then if one point is not on the constrained
// side and the closest point on the line is not on the constrained side, you've
// found a potential wrap. Otherwise, there is no wrap.
WrapMath::GetClosestPointOnLineToPoint(origin, aPoint1, p1p2, mm, tt);
tt = -tt; // because p1p2 is actually aPoint2->aPoint1
if (WrapMath::CalcDistanceSquaredBetweenPoints(origin, mm) < r_squared && tt > 0.0 && tt < 1.0)
{
return_code = mandatoryWrap;
}
else
{
if (DSIGN(aPoint1[_wrapAxis]) != DSIGN(aPoint2[_wrapAxis]) && DSIGN(mm[_wrapAxis]) != _wrapSign)
{
return_code = wrapped;
}
else
{
return noWrap;
}
}
}
if (DSIGN(aPoint1[_wrapAxis]) != _wrapSign || DSIGN(aPoint2[_wrapAxis]) != _wrapSign)
{
SimTK::Vec3 wrapaxis, sum_musc, sum_r;
for (i = 0; i < 3; i++)
wrapaxis[i] = (i == _wrapAxis) ? (double) _wrapSign : 0.0;
// determine best constrained r1 & r2 tangent points:
for (i = 0; i < 3; i++)
sum_musc[i] = (origin[i] - aPoint1[i]) + (origin[i] - aPoint2[i]);
Mtx::Normalize(3, sum_musc, sum_musc);
if (Mtx::DotProduct(3, r1am, sum_musc) > Mtx::DotProduct(3, r1bm, sum_musc))
{
for (i = 0; i < 3; i++)
aWrapResult.r1[i] = r1a[i];
r11 = &r1b[0];
}
else
{
for (i = 0; i < 3; i++)
aWrapResult.r1[i] = r1b[i];
r11 = &r1a[0];
}
if (Mtx::DotProduct(3, r2am, sum_musc) > Mtx::DotProduct(3, r2bm, sum_musc))
{
for (i = 0; i < 3; i++)
aWrapResult.r2[i] = r2a[i];
r22 = &r2b[0];
}
else
{
for (i = 0; i < 3; i++)
aWrapResult.r2[i] = r2b[i];
r22 = &r2a[0];
}
// flip if necessary:
for (i = 0; i < 3; i++)
sum_musc[i] = (aWrapResult.r1[i] - aPoint1[i]) + (aWrapResult.r2[i] - aPoint2[i]);
Mtx::Normalize(3, sum_musc, sum_musc);
if (Mtx::DotProduct(3, sum_musc, wrapaxis) < 0.0)
{
for (i = 0; i < 3; i++) {
aWrapResult.r1[i] = r11[i];
aWrapResult.r2[i] = r22[i];
}
}
// determine if the resulting tangent points create a far side wrap
for (i = 0; i < 3; i++) {
sum_musc[i] = (aWrapResult.r1[i] - aPoint1[i]) + (aWrapResult.r2[i] - aPoint2[i]);
sum_r[i] = (aWrapResult.r1[i] - origin[i]) + (aWrapResult.r2[i] - origin[i]);
}
if (Mtx::DotProduct(3, sum_r, sum_musc) < 0.0)
far_side_wrap = true;
}
}
calc_path:
for (i = 0; i < 3; i++) {
r1m[i] = aWrapResult.r1[i] - origin[i];
r2m[i] = aWrapResult.r2[i] - origin[i];
}
Mtx::Normalize(3, r1m, r1n);
Mtx::Normalize(3, r2m, r2n);
angle = acos(Mtx::DotProduct(3, r1n, r2n));
if (far_side_wrap)
angle = -(2 * SimTK_PI - angle);
r1r2 = _radius * angle;
aWrapResult.wrap_path_length = r1r2;
Vec3 axis3;
Mtx::CrossProduct(r1n, r2n, axis3);
Mtx::Normalize(3, axis3, axis3);
for(int ii=0; ii<3; ii++) axis[ii]=axis3[ii];
axis[3] = 1.0;
aWrapResult.wrap_pts.setSize(0);
// Each muscle segment on the surface of the sphere 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;
//SimmPoint sp1(aWrapResult.r1);
aWrapResult.wrap_pts.append(aWrapResult.r1);
vec[0] = r1m[0];
vec[1] = r1m[1];
vec[2] = r1m[2];
vec[3] = 1.0;
for (i = 0; i < numWrapSegments - 2; i++) {
double wangle = angle * (i+1) / (numWrapSegments - 1) * SimTK_DEGREE_TO_RADIAN;
WrapMath::ConvertAxisAngleTo4x4DirCosMatrix(Vec3::getAs(axis), wangle, mat);
Mtx::Multiply(4, 4, 1, (double*)mat, (double*)vec, (double*)rotvec);
SimTK::Vec3 wp;
for (j = 0; j < 3; j++)
wp[j] = origin[j] + rotvec[j];
//SimmPoint wppt(wp);
aWrapResult.wrap_pts.append(wp);
}
//SimmPoint sp2(aWrapResult.r2);
aWrapResult.wrap_pts.append(aWrapResult.r2);
return return_code;
}
// Muscle Wrapping
void OpenSimContext::setStartPoint(PathWrap& mw, int newStartPt) {
mw.setStartPoint(*_configState, newStartPt );
return;
}
/**
* Calculate the wrapping of one line segment over the cylinder.
*
* @param aPoint1 One end of the line segment
* @param aPoint2 The other end of the line segment
* @param aPathWrap An object holding the parameters for this line/cylinder pairing
* @param aWrapResult The result of the wrapping (tangent points, etc.)
* @param aFlag A flag for indicating errors, etc.
* @return The status, as a WrapAction enum
*/
int WrapCylinder::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::Vec3& aPoint2,
const PathWrap& aPathWrap, WrapResult& aWrapResult, bool& aFlag) const
{
double dist, p11_dist, p22_dist, t, dot1, dot2, dot3, dot4, d, sin_theta,
*r11, *r22, alpha, beta, r_squared = _radius * _radius;
double dist1, dist2;
double t12, t00;
Vec3 pp, vv, uu, r1a, r1b, r2a, r2b, apex, plane_normal, sum_musc,
r1am, r1bm, r2am, r2bm, p11, p22, r1p, r2p, axispt, near12,
vert1, vert2, mpt, apex1, apex2, l1, l2, near00;
int i, return_code = wrapped;
bool r1_inter, r2_inter;
bool constrained = (bool) (_wrapSign != 0);
bool far_side_wrap = false, long_wrap = false;
// In case you need any variables from the previous wrap, copy them from
// the PathWrap into the WrapResult, re-normalizing the ones that were
// un-normalized at the end of the previous wrap calculation.
const WrapResult& previousWrap = aPathWrap.getPreviousWrap();
aWrapResult.factor = previousWrap.factor;
for (i = 0; i < 3; i++)
{
aWrapResult.r1[i] = previousWrap.r1[i] * previousWrap.factor;
aWrapResult.r2[i] = previousWrap.r2[i] * previousWrap.factor;
aWrapResult.c1[i] = previousWrap.c1[i];
aWrapResult.sv[i] = previousWrap.sv[i];
}
aFlag = false;
aWrapResult.wrap_path_length = 0.0;
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)
{
return insideRadius;
}
// Find the closest intersection between the muscle line segment and the line
// segment from one end of the cylinder to the other. This intersection is
// used in several places further down in the code to check for various
// wrapping conditions.
SimTK::Vec3 cylStart, cylEnd;
cylStart[0] = cylEnd[0] = 0.0;
cylStart[1] = cylEnd[1] = 0.0;
cylStart[2] = -0.5 * _length;
cylEnd[2] = 0.5 * _length;
WrapMath::IntersectLines(aPoint1, aPoint2, cylStart, cylEnd, near12, t12, near00, t00);
// abort if the cylinder is unconstrained and p1p2 misses the cylinder.
// Use the return values from the above call to IntersectLines()
// to perform the check.
if ( ! constrained)
{
if (WrapMath::CalcDistanceSquaredBetweenPoints(near12, near00) < r_squared && t12 > 0.0 && t12 < 1.0)
{
return_code = mandatoryWrap;
}
else
{
return noWrap;
}
}
// 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);
// find preliminary tangent point candidates r1a & r1b
MAKE_3DVECTOR(p11, aPoint1, vv);
p11_dist = Mtx::Normalize(3, vv, vv);
sin_theta = _radius / p11_dist;
dist = _radius * sin_theta;
for (i = 0; i < 3; i++)
pp[i] = p11[i] + dist * vv[i];
dist = sqrt(r_squared - dist * dist);
Mtx::CrossProduct(dn, vv, uu);
for (i = 0; i < 3; i++)
{
r1a[i] = pp[i] + dist * uu[i];
r1b[i] = pp[i] - dist * uu[i];
}
// find preliminary tangent point candidates r2a & r2b
MAKE_3DVECTOR(p22, aPoint2, vv);
p22_dist = Mtx::Normalize(3, vv, vv);
sin_theta = _radius / p22_dist;
dist = _radius * sin_theta;
for (i = 0; i < 3; i++)
pp[i] = p22[i] + dist * vv[i];
dist = sqrt(r_squared - dist * dist);
Mtx::CrossProduct(dn, vv, uu);
for (i = 0; i < 3; i++)
{
r2a[i] = pp[i] + dist * uu[i];
r2b[i] = pp[i] - dist * uu[i];
}
// choose the best preliminary tangent points r1 & r2 from the 4 candidates.
if (constrained)
{
SimTK::Vec3 sum_r;
if (DSIGN(aPoint1[_wrapAxis]) == _wrapSign || DSIGN(aPoint2[_wrapAxis]) == _wrapSign)
{
// If either muscle point is on the constrained side, then check for intersection
// of the muscle line and the cylinder. If there is an intersection, then
// you've found a mandatory wrap. If not, then if one point is not on the constrained
// side and the closest point on the line is not on the constrained side, you've
// found a potential wrap. Otherwise, there is no wrap.
// Use the return values from the previous call to IntersectLines()
// to perform these checks.
if (WrapMath::CalcDistanceSquaredBetweenPoints(near12, near00) < r_squared && t12 > 0.0 && t12 < 1.0)
{
return_code = mandatoryWrap;
}
else
{
if (DSIGN(aPoint1[_wrapAxis]) != DSIGN(aPoint2[_wrapAxis]) && DSIGN(near12[_wrapAxis]) != _wrapSign)
{
return_code = wrapped;
}
else
{
return noWrap;
}
}
}
MAKE_3DVECTOR(p11, r1a, r1am);
MAKE_3DVECTOR(p11, r1b, r1bm);
MAKE_3DVECTOR(p22, r2a, r2am);
MAKE_3DVECTOR(p22, r2b, r2bm);
alpha = Mtx::Angle(r1am, r2bm);
beta = Mtx::Angle(r1bm, r2am);
// check to see which of the four tangent points should be chosen by seeing which
// ones are on the 'active' portion of the cylinder. If r1a and r1b are both on or
// both off the active portion, then use r2a and r2b to decide.
if (DSIGN(r1a[_wrapAxis]) == _wrapSign && DSIGN(r1b[_wrapAxis]) == _wrapSign)
{
if (DSIGN(r2a[_wrapAxis]) == _wrapSign)
{
COPY_1X3VECTOR(r1b, aWrapResult.r1);
COPY_1X3VECTOR(r2a, aWrapResult.r2);
if (alpha > beta)
far_side_wrap = false;
else
far_side_wrap = true;
}
else
{
COPY_1X3VECTOR(r1a, aWrapResult.r1);
COPY_1X3VECTOR(r2b, aWrapResult.r2);
if (alpha > beta)
far_side_wrap = true;
else
far_side_wrap = false;
}
}
else if (DSIGN(r1a[_wrapAxis]) == _wrapSign && DSIGN(r1b[_wrapAxis]) != _wrapSign)
{
COPY_1X3VECTOR(r1a, aWrapResult.r1);
COPY_1X3VECTOR(r2b, aWrapResult.r2);
if (alpha > beta)
far_side_wrap = true;
else
far_side_wrap = false;
}
else if (DSIGN(r1a[_wrapAxis]) != _wrapSign && DSIGN(r1b[_wrapAxis]) == _wrapSign)
{
COPY_1X3VECTOR(r1b, aWrapResult.r1);
COPY_1X3VECTOR(r2a, aWrapResult.r2);
if (alpha > beta)
far_side_wrap = false;
else
far_side_wrap = true;
}
else if (DSIGN(r1a[_wrapAxis]) != _wrapSign && DSIGN(r1b[_wrapAxis]) != _wrapSign)
{
if (DSIGN(r2a[_wrapAxis]) == _wrapSign)
{
COPY_1X3VECTOR(r1b, aWrapResult.r1);
COPY_1X3VECTOR(r2a, aWrapResult.r2);
if (alpha > beta)
far_side_wrap = false;
else
far_side_wrap = true;
}
else if (DSIGN(r2b[_wrapAxis]) == _wrapSign)
{
COPY_1X3VECTOR(r1a, aWrapResult.r1);
COPY_1X3VECTOR(r2b, aWrapResult.r2);
if (alpha > beta)
far_side_wrap = true;
else
far_side_wrap = false;
}
else // none of the four tangent points is on the active portion
{
if (alpha > beta)
{
COPY_1X3VECTOR(r1a, aWrapResult.r1);
COPY_1X3VECTOR(r2b, aWrapResult.r2);
far_side_wrap = true;
}
else
{
COPY_1X3VECTOR(r1b, aWrapResult.r1);
COPY_1X3VECTOR(r2a, aWrapResult.r2);
far_side_wrap = true;
}
}
}
// determine if the resulting tangent points create a short wrap
// (less than half the cylinder) or a long wrap.
for (i = 0; i < 3; i++)
{
sum_musc[i] = (aWrapResult.r1[i] - aPoint1[i]) + (aWrapResult.r2[i] - aPoint2[i]);
sum_r[i] = (aWrapResult.r1[i] - p11[i]) + (aWrapResult.r2[i] - p22[i]);
}
if (Mtx::DotProduct(3, sum_r, sum_musc) < 0.0)
long_wrap = true;
}
else
{
MAKE_3DVECTOR(p11, r1a, r1am);
MAKE_3DVECTOR(p11, r1b, r1bm);
MAKE_3DVECTOR(p22, r2a, r2am);
MAKE_3DVECTOR(p22, r2b, r2bm);
Mtx::Normalize(3, r1am, r1am);
Mtx::Normalize(3, r1bm, r1bm);
Mtx::Normalize(3, r2am, r2am);
Mtx::Normalize(3, r2bm, r2bm);
dot1 = Mtx::DotProduct(3, r1am, r2am);
dot2 = Mtx::DotProduct(3, r1am, r2bm);
dot3 = Mtx::DotProduct(3, r1bm, r2am);
dot4 = Mtx::DotProduct(3, r1bm, r2bm);
if (dot1 > dot2 && dot1 > dot3 && dot1 > dot4)
{
COPY_1X3VECTOR(r1a, aWrapResult.r1);
COPY_1X3VECTOR(r2a, aWrapResult.r2);
r11 = &r1b[0];
r22 = &r2b[0];
}
else if (dot2 > dot3 && dot2 > dot4)
{
COPY_1X3VECTOR(r1a, aWrapResult.r1);
COPY_1X3VECTOR(r2b, aWrapResult.r2);
r11 = &r1b[0];
r22 = &r2a[0];
}
else if (dot3 > dot4)
{
COPY_1X3VECTOR(r1b, aWrapResult.r1);
COPY_1X3VECTOR(r2a, aWrapResult.r2);
r11 = &r1a[0];
r22 = &r2b[0];
}
else
{
COPY_1X3VECTOR(r1b, aWrapResult.r1);
COPY_1X3VECTOR(r2b, aWrapResult.r2);
r11 = &r1a[0];
r22 = &r2a[0];
}
}
// bisect angle between r1 & r2 vectors to find the apex edge of the
// cylinder:
MAKE_3DVECTOR(p11, aWrapResult.r1, uu);
MAKE_3DVECTOR(p22, aWrapResult.r2, vv);
for (i = 0; i < 3; i++)
vv[i] = uu[i] + vv[i];
Mtx::Normalize(3, vv, vv);
// find the apex point by using a ratio of the lengths of the
// aPoint1-p11 and aPoint2-p22 segments:
t = p11_dist / (p11_dist + p22_dist);
// find point along muscle line according to calculated t value
for (i = 0; i < 3; i++)
mpt[i] = aPoint1[i] + t * (aPoint2[i] - aPoint1[i]);
// find closest point on cylinder axis to mpt
WrapMath::GetClosestPointOnLineToPoint(mpt, p0, dn, axispt, t);
// find normal of plane through aPoint1, aPoint2, axispt
MAKE_3DVECTOR(axispt, aPoint1, l1);
MAKE_3DVECTOR(axispt, aPoint2, l2);
Mtx::Normalize(3, l1, l1);
Mtx::Normalize(3, l2, l2);
Mtx::CrossProduct(l1, l2, plane_normal);
Mtx::Normalize(3, plane_normal, plane_normal);
// 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).
Mtx::CrossProduct(dn, plane_normal, vert1);
Mtx::Normalize(3, vert1, vert1);
Mtx::CrossProduct(plane_normal, dn, vert2);
Mtx::Normalize(3, vert2, vert2);
// now find two potential apex points, one along each vector
for (i = 0; i < 3; i++)
{
apex1[i] = axispt[i] + _radius * vert1[i];
apex2[i] = axispt[i] + _radius * vert2[i];
}
// Now use the distance from these points to mpt to
// pick the right one.
dist1 = WrapMath::CalcDistanceSquaredBetweenPoints(mpt, apex1);
dist2 = WrapMath::CalcDistanceSquaredBetweenPoints(mpt, apex2);
if (far_side_wrap)
{
if (dist1 < dist2)
{
for (i = 0; i < 3; i++)
apex[i] = apex2[i];
}
else
{
for (i = 0; i < 3; i++)
apex[i] = apex1[i];
}
}
else
{
if (dist1 < dist2)
{
for (i = 0; i < 3; i++)
apex[i] = apex1[i];
}
else
{
for (i = 0; i < 3; i++)
apex[i] = apex2[i];
}
}
// determine how far to slide the preliminary r1/r2 along their
// "edge of tangency" with the cylinder by intersecting the aPoint1-ax
// line with the plane formed by aPoint1, aPoint2, and apex:
MAKE_3DVECTOR(apex, aPoint1, uu);
MAKE_3DVECTOR(apex, aPoint2, vv);
Mtx::Normalize(3, uu, uu);
Mtx::Normalize(3, vv, vv);
Mtx::CrossProduct(uu, vv, plane_normal);
Mtx::Normalize(3, plane_normal, plane_normal);
d = - aPoint1[0] * plane_normal[0] - aPoint1[1] * plane_normal[1] - aPoint1[2] * plane_normal[2];
for (i = 0; i < 3; i++)
{
r1a[i] = aWrapResult.r1[i] - 10.0 * dn[i];
r2a[i] = aWrapResult.r2[i] - 10.0 * dn[i];
r1b[i] = aWrapResult.r1[i] + 10.0 * dn[i];
r2b[i] = aWrapResult.r2[i] + 10.0 * dn[i];
}
r1_inter = WrapMath::IntersectLineSegPlane(r1a, r1b, plane_normal, d, r1p);
r2_inter = WrapMath::IntersectLineSegPlane(r2a, r2b, plane_normal, d, r2p);
if (r1_inter)
{
WrapMath::GetClosestPointOnLineToPoint(r1p, p11, p22, r1a, t);
if (WrapMath::CalcDistanceSquaredBetweenPoints(r1a, p22) < WrapMath::CalcDistanceSquaredBetweenPoints(p11, p22))
for (i = 0; i < 3; i++)
aWrapResult.r1[i] = r1p[i];
}
if (r2_inter)
{
WrapMath::GetClosestPointOnLineToPoint(r2p, p11, p22, r2a, t);
if (WrapMath::CalcDistanceSquaredBetweenPoints(r2a, p11) < WrapMath::CalcDistanceSquaredBetweenPoints(p22, p11))
for (i = 0; i < 3; i++)
aWrapResult.r2[i] = r2p[i];
}
// Now that you have r1 and r2, check to see if they are beyond the
// [display] length of the cylinder. If both are, there should be
// no wrapping. Since the axis of the cylinder is the Z axis, and
// the cylinder is centered on Z=0, check the Z components of r1 and r2
// to see if they are within _length/2.0 of zero.
if ((aWrapResult.r1[2] < -_length/2.0 || aWrapResult.r1[2] > _length/2.0) && (aWrapResult.r2[2] < -_length/2.0 || aWrapResult.r2[2] > _length/2.0))
return noWrap;
// make the path and calculate the muscle length:
_make_spiral_path(aPoint1, aPoint2, long_wrap, aWrapResult);
aFlag = true;
return return_code;
}
