public IKHexapod(Vector3 center_point)
{
var e1 = new LineBuilder();
e1.Add(true,
new Vector3(-Config.HALF_X2LENGTH, 0, 0),
new Vector3(-Config.HALF_X1LENGTH, 0, Config.HALF_ZLENGTH),
new Vector3(Config.HALF_X1LENGTH, 0, Config.HALF_ZLENGTH),
new Vector3(Config.HALF_X2LENGTH, 0, 0),
new Vector3(Config.HALF_X1LENGTH, 0, -Config.HALF_ZLENGTH),
new Vector3(-Config.HALF_X1LENGTH, 0, -Config.HALF_ZLENGTH));
e1.Add(true, new Vector3(-Config.HALF_X2LENGTH, Config.BODY_HIGH, 0),
new Vector3(-Config.HALF_X1LENGTH, Config.BODY_HIGH, Config.HALF_ZLENGTH),
new Vector3(Config.HALF_X1LENGTH, Config.BODY_HIGH, Config.HALF_ZLENGTH),
new Vector3(Config.HALF_X2LENGTH, Config.BODY_HIGH, 0),
new Vector3(Config.HALF_X1LENGTH, Config.BODY_HIGH, -Config.HALF_ZLENGTH),
new Vector3(-Config.HALF_X1LENGTH, Config.BODY_HIGH, -Config.HALF_ZLENGTH));
e1.AddLine(
new Vector3(Config.HALF_X1LENGTH, Config.BODY_HIGH, -Config.HALF_ZLENGTH),
new Vector3(Config.HALF_X1LENGTH, 0, -Config.HALF_ZLENGTH));
e1.AddLine(
new Vector3(Config.HALF_X2LENGTH, Config.BODY_HIGH, 0),
new Vector3(Config.HALF_X2LENGTH, 0, 0));
e1.AddLine(
new Vector3(Config.HALF_X1LENGTH, Config.BODY_HIGH, Config.HALF_ZLENGTH),
new Vector3(Config.HALF_X1LENGTH, 0, Config.HALF_ZLENGTH));
e1.AddLine(
new Vector3(-Config.HALF_X1LENGTH, Config.BODY_HIGH, Config.HALF_ZLENGTH),
new Vector3(-Config.HALF_X1LENGTH, 0, Config.HALF_ZLENGTH));
e1.AddLine(
new Vector3(-Config.HALF_X2LENGTH, Config.BODY_HIGH, 0),
new Vector3(-Config.HALF_X2LENGTH, 0, 0));
e1.AddLine(
new Vector3(-Config.HALF_X1LENGTH, Config.BODY_HIGH, -Config.HALF_ZLENGTH),
new Vector3(-Config.HALF_X1LENGTH, 0, -Config.HALF_ZLENGTH));
//var leg = new IKLeg(0, new Vector3(), 1, 1, 1);
Leg1 = new IKLeg(1,
new Vector3(Config.HALF_X1LENGTH, Config.TIBIA_LENGTH, -Config.HALF_ZLENGTH),
Config.COXA_LENGTH, Config.FEMUR_LENGTH, Config.TIBIA_LENGTH);
Leg2 = new IKLeg(2,
new Vector3(Config.HALF_X2LENGTH, Config.TIBIA_LENGTH, 0),
Config.COXA_LENGTH, Config.FEMUR_LENGTH, Config.TIBIA_LENGTH);
Leg3 = new IKLeg(3,
new Vector3(Config.HALF_X1LENGTH, Config.TIBIA_LENGTH, Config.HALF_ZLENGTH),
Config.COXA_LENGTH, Config.FEMUR_LENGTH, Config.TIBIA_LENGTH);
Leg4 = new IKLeg(4,
new Vector3(-Config.HALF_X1LENGTH, Config.TIBIA_LENGTH, Config.HALF_ZLENGTH),
Config.COXA_LENGTH, Config.FEMUR_LENGTH, Config.TIBIA_LENGTH);
Leg5 = new IKLeg(5,
new Vector3(-Config.HALF_X2LENGTH, Config.TIBIA_LENGTH, 0),
Config.COXA_LENGTH, Config.FEMUR_LENGTH, Config.TIBIA_LENGTH);
Leg6 = new IKLeg(6,
new Vector3(-Config.HALF_X1LENGTH, Config.TIBIA_LENGTH, -Config.HALF_ZLENGTH),
Config.COXA_LENGTH, Config.FEMUR_LENGTH, Config.TIBIA_LENGTH);
ModelSkeleton = e1.ToLineGeometry3D();
SkeletonTransform = new System.Windows.Media.Media3D.TranslateTransform3D(center_point.ToVector3D());
Calculate();
}
private void ApplyUnloadTool()
{
if ((_loadedTool != null) /*&& (_preLoadTransform != null)*/)
{
_loadedTool.Transform = _preLoadTransform;
Children.Add(_loadedTool);
_loadedTool = null;
_preLoadTransform = null;
}
}
public void RaiseBody(double distance)
{
SkeletonTransform = new System.Windows.Media.Media3D.TranslateTransform3D(
SkeletonTransform.Value.OffsetX,
SkeletonTransform.Value.OffsetY + distance,
SkeletonTransform.Value.OffsetZ);
UpdateAllLeg(distance, 0, "Y");
UpdateAllLeg(distance, 1, "Y");
Calculate();
}
private void InitializeViewCubes()
{
var builder = new MeshBuilder();
builder.AddPyramid(Vector3.Zero, 10, 10, true);
ViewCubeGeometry1 = builder.ToMesh();
ViewCubeMaterial1 = DiffuseMaterials.Orange;
builder = new MeshBuilder();
builder.AddDodecahedron(Vector3.Zero, Vector3.UnitX, Vector3.UnitY, 5);
ViewCubeGeometry2 = builder.ToMesh();
ViewCubeMaterial2 = DiffuseMaterials.Blue;
ViewCubeMaterial3 = DiffuseMaterials.Gray;
ViewCubeMaterial4 = DiffuseMaterials.Pearl;
//Center the model first and do scaling
var transform = Matrix.Translation(0, -2, 0) * Matrix.Scaling(3.5f);
ViewCubeTransform3 = new System.Windows.Media.Media3D.MatrixTransform3D(transform.ToMatrix3D());
}
private void OnLoadToolMessage(LoadToolMessage msg)
{
if (msg.ToolSource == ToolHolderId)
{
if (Children.Count == 1)
{
_loadedTool = Children[0];
_preLoadTransform = _loadedTool.Transform;
Children.Clear();
MessengerInstance.Send(new CompleteToolLoadingMessage()
{
ToolSink = msg.ToolSink,
ToolModel = _loadedTool,
ToolData = _tool,
BackNotifyId = msg.BackNotifyId
});
}
else
{
throw new InvalidOperationException("Load tool from empty tool holder source!");
}
}
}
