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();
}
public void Update()
{
LineBuilder lb = new LineBuilder();
lb.Add(false, Lines);
Leg = lb.ToLineGeometry3D();
OnPropertyChanged("Leg");
}
public static Geometry3D GetProbeDistanceModel(Point3D[] points)
{
var builder = new LineBuilder();
builder.Add(false, points.Select(i => i.ToVector3()).ToArray());
return(builder.ToLineGeometry3D());
}
//----------------------------------------------------------------//
public IKLeg(int index, Vector3 rPoint, float coxa, float femur, float tibia)
{
this.Index = index;
//--- create joint
LineBuilder lb = new LineBuilder();
Lines = new Vector3[] { rPoint,
((index > 3)? new Vector3(rPoint.X - coxa, rPoint.Y, rPoint.Z):new Vector3(rPoint.X + coxa, rPoint.Y, rPoint.Z)),
((index > 3)? new Vector3(rPoint.X - coxa - femur, rPoint.Y, rPoint.Z):new Vector3(rPoint.X + coxa + femur, rPoint.Y, rPoint.Z)),
((index > 3)? new Vector3(rPoint.X - coxa - femur, rPoint.Y - tibia, rPoint.Z):new Vector3(rPoint.X + coxa + femur, rPoint.Y - tibia, rPoint.Z)) };
lb.Add(false, Lines);
Leg = lb.ToLineGeometry3D();
Transform = new System.Windows.Media.Media3D.TranslateTransform3D();
//--- rotage leg ---//
RotageLeg(rotage_base[index - 1]);
Angle = 0;
}
private void CreateWireFrameModels(IFCItem item, Vector3D center)
{
while (item != null)
{
if (item.ifcID != IntPtr.Zero && item.noVerticesForWireFrame != 0 && item.noPrimitivesForWireFrame != 0)
{
var points = new Vector3Collection();
Vector3Collection positions;
if (item.verticesForWireFrame != null)
{
for (int i = 0; i < item.noVerticesForWireFrame; i++)
{
points.Add(new Vector3((float)(item.verticesForWireFrame[3 * i + 0] - center.X), (float)(item.verticesForWireFrame[3 * i + 1] - center.Y), (float)(item.verticesForWireFrame[3 * i + 2] - center.Z)));
}
}
if (item.indicesForWireFrameLineParts != null)
{
positions = new Vector3Collection();
for (int i = 0; i < item.noPrimitivesForWireFrame; i++)
{
var idx = item.indicesForWireFrameLineParts[2 * i + 0];
positions.Add(points[idx]);
idx = item.indicesForWireFrameLineParts[2 * i + 1];
positions.Add(points[idx]);
}
}
else
{
positions = points;
}
var lineBuilder = new LineBuilder();
lineBuilder.Add(false, positions.ToArray());
LineGeometryModel3D line = new LineGeometryModel3D();
line.Geometry = lineBuilder.ToLineGeometry3D();
line.Color = new SharpDX.Color(0, 0, 0, 0);
item.Wireframe = line;
model.Add(line as Element3D);
}
CreateFaceModels(item.child, center);
item = item.next;
}
}
