/// <summary>
/// Draw some debug information like the chain bones
/// </summary>
/// <param name="g">the graphic context in which draw</param>
/// <param name="areaInStud">the area in which draw</param>
/// <param name="scalePixelPerStud">the current scale</param>
public void draw(Graphics g, RectangleF areaInStud, double scalePixelPerStud)
{
int lastIndex = mBoneList.Count - 1;
// draw the bones
for (int i = 0; i < lastIndex; i++)
{
IKSolver.Bone_2D_CCD firstBone = mBoneList[i];
IKSolver.Bone_2D_CCD nextBone = mBoneList[i + 1];
g.DrawLine(Pens.Black, (float)((firstBone.worldX - areaInStud.Left) * scalePixelPerStud),
(float)((-firstBone.worldY - areaInStud.Top) * scalePixelPerStud),
(float)((nextBone.worldX - areaInStud.Left) * scalePixelPerStud),
(float)((-nextBone.worldY - areaInStud.Top) * scalePixelPerStud));
}
// the last bone vector
LayerBrick.Brick.ConnectionPoint endConnection = mBoneList[lastIndex].connectionPoint;
if (!endConnection.IsFree)
{
// rotate the second target vector according to the orientation of the snapped connection
PointF[] translation = { mLastBoneVector };
Matrix rotation = new Matrix();
rotation.Rotate(endConnection.ConnectedBrick.Orientation + endConnection.ConnectionLink.Angle + 180);
rotation.TransformVectors(translation);
// draw the translation
IKSolver.Bone_2D_CCD lastBone = mBoneList[mBoneList.Count - 1];
float endX = (float)(lastBone.worldX - areaInStud.Left);
float endY = (float)(-lastBone.worldY - areaInStud.Top);
g.DrawLine(Pens.Green, endX * (float)scalePixelPerStud, endY * (float)scalePixelPerStud,
(endX + translation[0].X) * (float)scalePixelPerStud,
(endY + translation[0].Y) * (float)scalePixelPerStud);
}
}
private void addNewBone(LayerBrick.Brick.ConnectionPoint connection, LayerBrick.Brick brick, double maxAngleInDeg)
{
IKSolver.Bone_2D_CCD newBone = new IKSolver.Bone_2D_CCD();
newBone.maxAbsoluteAngleInRad = maxAngleInDeg * (Math.PI / 180);
if (connection != null)
{
newBone.worldX = connection.PositionInStudWorldCoord.X;
newBone.worldY = -connection.PositionInStudWorldCoord.Y; // BlueBrick use an indirect coord sys, and the IKSolver a direct one
}
else
{
newBone.worldX = brick.Center.X;
newBone.worldY = -brick.Center.Y; // BlueBrick use an indirect coord sys, and the IKSolver a direct one
}
newBone.connectionPoint = connection;
mBoneList.Insert(0, newBone);
}
/// <summary>
/// compute the position and orientation of all the bricks along the Flex Chain
/// based on the angle of each rotationable (flexible) connection point
/// </summary>
private void computeBrickPositionAndOrientation()
{
// start with the first bone of the list
int boneIndex = 0;
IKSolver.Bone_2D_CCD currentBone = mBoneList[boneIndex];
// start with a null total flexible orientation that we will increase with the angle of every bone
float flexibleCumulativeOrientation = 0.0f;
// init the static cumulative orientation with the one saved in this class.
// we cannot use the orientation of the root brick of the chain because this brick orientation
// is also changed in the loop, leading to some divergence
float staticCumulativeOrientation = mInitialStaticCumulativeOrientation;
// iterate on the link list and change the world angle everytime we meet an hinge
// start with the first hinged connection of the chain (because everything before doesn't move)
for (int linkIndex = mRootHingedLinkIndex; linkIndex < mFlexChainList.Count; ++linkIndex)
{
// get the previous and current brick
ChainLink currentLink = mFlexChainList[linkIndex];
LayerBrick.Brick.ConnectionPoint previousConnection = currentLink.mFirstConnection;
PointF previousPosition = previousConnection.PositionInStudWorldCoord;
LayerBrick.Brick.ConnectionPoint currentConnection = currentLink.mSecondConnection;
LayerBrick.Brick currentBrick = currentConnection.mMyBrick;
// check if we reach an hinge connection
if (currentConnection == currentBone.connectionPoint)
{
// set the new world position to the current bone with the previous connection position
// because the previous brick was already placed at the correct position
currentBone.worldX = previousPosition.X;
currentBone.worldY = -previousPosition.Y; // BlueBrick use an indirect coord sys, and the IKSolver a direct one
// increase the flexible angle
flexibleCumulativeOrientation += (float)(currentBone.localAngleInRad * (180.0 / Math.PI));
// take the next bone
boneIndex++;
if (boneIndex < mBoneList.Count)
{
currentBone = mBoneList[boneIndex];
}
}
// add the difference of orientation between the previous brick and current brick through their linked connections
staticCumulativeOrientation += currentLink.mAngleBetween;
// set the orientation of the current brick
currentBrick.Orientation = -flexibleCumulativeOrientation - staticCumulativeOrientation;
// compute the new position of the current brick by putting the current connection at the same
// place than the previous connection
PointF newBrickPosition = currentBrick.Position;
newBrickPosition.X += previousPosition.X - currentConnection.PositionInStudWorldCoord.X;
newBrickPosition.Y += previousPosition.Y - currentConnection.PositionInStudWorldCoord.Y;
currentBrick.Position = newBrickPosition;
}
// update the last bone position
if (mBoneList.Count > 0)
{
int lastIndex = mBoneList.Count - 1;
// get the last position
PointF lastPosition;
if (mBoneList[lastIndex].connectionPoint != null)
{
lastPosition = mBoneList[lastIndex].connectionPoint.PositionInStudWorldCoord;
}
else
{
lastPosition = mBoneList[lastIndex - 1].connectionPoint.mMyBrick.Center;
}
// and set it in the last bone
mBoneList[lastIndex].worldX = lastPosition.X;
mBoneList[lastIndex].worldY = -lastPosition.Y; // BlueBrick use an indirect coord sys, and the IKSolver a direct one
}
// update the bounding rectangle and connectivity
mBrickLayer.updateBoundingSelectionRectangle();
}
