/// <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(); }