/// <summary>
/// This method can be called when the flex move didn't reach the target yet, to continue to converge
/// <returns>true if the flex move still need to be updated</returns>
/// </summary>
public bool update()
{
if (mCurrentUpdateStatus == IKSolver.CCDResult.Processing)
{
// a threshold tuning variable for reaching target
const double REACH_TARGET_PRECISION_IN_STUD = 0.1;
int lastBone = mBoneList.Count;
if (mUseTwoTargets)
{
// if we use two targets, do the first pass on the second target
// reverse the Y because BlueBrick use an indirect coord sys, and the IKSolver a direct one
IKSolver.CalcIK_2D_CCD(ref mBoneList, mSecondaryTarget.X, -mSecondaryTarget.Y,
REACH_TARGET_PRECISION_IN_STUD, lastBone - 1);
computeBrickPositionAndOrientation();
}
// do the normal pass on the final target
// reverse the Y because BlueBrick use an indirect coord sys, and the IKSolver a direct one
mCurrentUpdateStatus = IKSolver.CalcIK_2D_CCD(ref mBoneList, mPrimaryTarget.X, -mPrimaryTarget.Y,
REACH_TARGET_PRECISION_IN_STUD, lastBone);
computeBrickPositionAndOrientation();
}
// return true if we still need to update
return(mCurrentUpdateStatus == IKSolver.CCDResult.Processing);
}
/// <summary>
/// Try to reach the specify target position with this chain
/// </summary>
/// <param name="targetInWorldStudCoord">the target position in world stud coord</param>
/// <param name="targetConnection">If the end of the flex chain need to be connected to a connection, this parameter indicates which one, otherwise it is null</param>
public void reachTarget(PointF targetInWorldStudCoord, LayerBrick.Brick.ConnectionPoint targetConnection)
{
// save the primary target
mPrimaryTarget = targetInWorldStudCoord;
// check if we need to compute a second target position
mUseTwoTargets = (targetConnection != null);
if (mUseTwoTargets)
{
// rotate the second target vector according to the orientation of the snapped connection
PointF[] translation = { mLastBoneVector };
Matrix rotation = new Matrix();
rotation.Rotate(targetConnection.mMyBrick.Orientation + targetConnection.Angle + 180);
rotation.TransformVectors(translation);
// translate the target with the rotated vector for the second target
mSecondaryTarget.X = targetInWorldStudCoord.X + translation[0].X;
mSecondaryTarget.Y = targetInWorldStudCoord.Y + translation[0].Y;
}
// reset the status flag
mCurrentUpdateStatus = IKSolver.CCDResult.Processing;
// and call the update method
update();
}
/// <summary>
/// Perform an iteration of IK
/// </summary>
private void UpdateIK()
{
int numBones = Bones.Count;
if (numBones == 0)
{
return;
}
// calculate the bone angles
List <IKSolver.Bone_2D_CCD> ccdBones = new List <IKSolver.Bone_2D_CCD>();
for (int boneIdx = 0; boneIdx <= numBones; ++boneIdx)
{
IKSolver.Bone_2D_CCD newCcdBone = new IKSolver.Bone_2D_CCD();
newCcdBone.angle = (boneIdx < numBones) ? Bones[boneIdx].Radians : 0;
newCcdBone.x = (boneIdx > 0) ? Bones[boneIdx - 1].Length : 0;
newCcdBone.y = 0;
newCcdBone.minlimiter = (boneIdx < numBones) ? Bones[boneIdx].MinLimiter : 0;
newCcdBone.maxlimiter = (boneIdx < numBones) ? Bones[boneIdx].MaxLimiter : 0;
ccdBones.Add(newCcdBone);
}
// iterate CCD until limit is reached or we find a valid solution
for (int itrCount = 0; itrCount < IterationsPerUpdate; ++itrCount)
{
IKSolver.CCDResult result = IKSolver.CalcIK_2D_CCD(ref ccdBones, TargetPosX, TargetPosY, ArrivalDist);
if (result == IKSolver.CCDResult.Processing)
{
CCDResult = "Обработка";
}
else if (result == IKSolver.CCDResult.Success)
{
CCDResult = "Выполнено";
break;
}
else if (result == IKSolver.CCDResult.Failure)
{
CCDResult = "Неудача";
break;
}
else
{
Debug.Assert(false);
CCDResult = "[UNKNOWN]";
break;
}
}
// extract the new bone data from the results
for (int boneIdx = 0; boneIdx < numBones; ++boneIdx)
{
Bones[boneIdx].Radians = ccdBones[boneIdx].angle;
}
}
/// <summary>
/// This method can be called when the flex move didn't reach the target yet, to continue to converge
/// <returns>true if the flex move still need to be updated</returns>
/// </summary>
public bool update()
{
if (mCurrentUpdateStatus == IKSolver.CCDResult.Processing)
{
// a threshold tuning variable for reaching target
const double REACH_TARGET_PRECISION_IN_STUD = 0.1;
int lastBone = mBoneList.Count;
if (mUseTwoTargets)
{
// if we use two targets, do the first pass on the second target
// reverse the Y because BlueBrick use an indirect coord sys, and the IKSolver a direct one
IKSolver.CalcIK_2D_CCD(ref mBoneList, mSecondaryTarget.X, -mSecondaryTarget.Y,
REACH_TARGET_PRECISION_IN_STUD, lastBone - 1);
computeBrickPositionAndOrientation();
}
// do the normal pass on the final target
// reverse the Y because BlueBrick use an indirect coord sys, and the IKSolver a direct one
mCurrentUpdateStatus = IKSolver.CalcIK_2D_CCD(ref mBoneList, mPrimaryTarget.X, -mPrimaryTarget.Y,
REACH_TARGET_PRECISION_IN_STUD, lastBone);
computeBrickPositionAndOrientation();
}
// return true if we still need to update
return (mCurrentUpdateStatus == IKSolver.CCDResult.Processing);
}
/// <summary>
/// Try to reach the specify target position with this chain
/// </summary>
/// <param name="targetInWorldStudCoord">the target position in world stud coord</param>
/// <param name="targetConnection">If the end of the flex chain need to be connected to a connection, this parameter indicates which one, otherwise it is null</param>
public void reachTarget(PointF targetInWorldStudCoord, LayerBrick.Brick.ConnectionPoint targetConnection)
{
// save the primary target
mPrimaryTarget = targetInWorldStudCoord;
// check if we need to compute a second target position
mUseTwoTargets = (targetConnection != null);
if (mUseTwoTargets)
{
// rotate the second target vector according to the orientation of the snapped connection
PointF[] translation = { mLastBoneVector };
Matrix rotation = new Matrix();
rotation.Rotate(targetConnection.mMyBrick.Orientation + targetConnection.Angle + 180);
rotation.TransformVectors(translation);
// translate the target with the rotated vector for the second target
mSecondaryTarget.X = targetInWorldStudCoord.X + translation[0].X;
mSecondaryTarget.Y = targetInWorldStudCoord.Y + translation[0].Y;
}
// reset the status flag
mCurrentUpdateStatus = IKSolver.CCDResult.Processing;
// and call the update method
update();
}
