Пример #1
0
/**
 * 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;
}
Пример #2
0
/**
 * 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;
}
Пример #3
0
/**
 * 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;

}