/// <summary>
/// returns the angle between the slide and the reference joint.
/// </summary>
/// <param name="blockJoint">The block joint.</param>
/// <param name="referenceJoint">The reference joint.</param>
/// <returns>System.Double.</returns>
internal double AngleOfBlockToJoint(Joint blockJoint, Joint referenceJoint)
{
if (blockJoint == referenceJoint)
{
return(0.0);
}
var i = Joints.IndexOf(blockJoint);
var j = Joints.IndexOf(referenceJoint);
return(angleFromBlockToJoint[numJoints * i + j]);
}
/// <summary>
/// Returns the Lengths the between two joints that are fixed with respect to this link.
/// </summary>
/// <param name="joint1">joint1.</param>
/// <param name="joint2">joint2.</param>
/// <returns>System.Double.</returns>
internal double LengthBetween(Joint joint1, Joint joint2)
{
if (joint1 == joint2)
{
return(0.0);
}
var i = Joints.IndexOf(joint1);
var j = Joints.IndexOf(joint2);
if (i > j)
{
return(lengths[numJoints * j + i]);
}
return(lengths[numJoints * i + j]);
}
/// <summary>
/// returns the shortest distance between the sliding joint and the reference point.
/// This is a signed distance! Given the unit vector created from the slide angle,
/// the positive distance is "on the left" or counter-clockwise, while a negative
/// distance is "on the right" or clockwise.
/// </summary>
/// <param name="slidingJoint">The sliding joint.</param>
/// <param name="referenceJoint">The reference joint.</param>
/// <returns>System.Double.</returns>
internal double DistanceBetweenSlides(Joint slidingJoint, Joint referenceJoint)
{
if (slidingJoint == referenceJoint)
{
return(0.0);
}
var slideIndex = Joints.IndexOf(slidingJoint);
var fixedIndex = Joints.IndexOf(referenceJoint);
var index = numJoints * slideIndex + fixedIndex;
if (distanceToSlideLine.ContainsKey(index))
{
return(distanceToSlideLine[index]);
}
return(distanceToSlideLine[numJoints * fixedIndex + slideIndex]);
}
/// <summary>
/// Sets the length between the two joints. This function is only called from Simulator.AssignLengths
/// and is to be used only when the solving a problem of construct-ability (or whatever they call it in
/// the literature).
/// </summary>
/// <param name="joint1">The joint1.</param>
/// <param name="joint2">The joint2.</param>
/// <param name="length">The length.</param>
/// <exception cref="System.Exception">link.setLength: cannot set the distance between joints because same joint provided
/// as both joint1 and joint2.</exception>
internal void SetLength(Joint joint1, Joint joint2, double length)
{
if (joint1 == joint2)
{
throw new Exception(
"link.setLength: cannot set the distance between joints because same joint provided as both joint1 and joint2.");
}
var i = Joints.IndexOf(joint1);
var j = Joints.IndexOf(joint2);
if (i > j)
{
lengths[numJoints * j + i] = length;
}
else
{
lengths[numJoints * i + j] = length;
}
}