public TrajectoryModel3D(Trajectory track, double thickness, double coeff = 1)
{
this.coeff = coeff;
this.pointRadius = thickness * this.coeff / 2;
this.trackLineRadius = this.pointRadius * 0.90;
this.track = track;
this.trackModelVisual3D = new List <ModelVisual3D>();
this.splitTrackModelVisual3D = new List <ModelVisual3D>();
this.trackModelVisual3D.Add(
this.CreateAnchorPointModelVisual3D(VRConvert.ConvertFromRealToVirtual(this.track.AnchorPoints[0], this.coeff), this.pointRadius));
for (var i = 1; i < this.track.AnchorPoints.Count; i++)
{
var trackLineMV3D = this.CreateTrajectoryLineModelVisual3D(
VRConvert.ConvertFromRealToVirtual(
this.track.AnchorPoints[i - 1],
this.coeff),
VRConvert.ConvertFromRealToVirtual(this.track.AnchorPoints[i], this.coeff),
this.trackLineRadius);
this.trackModelVisual3D.Add(trackLineMV3D);
this.trackModelVisual3D.Add(
this.CreateAnchorPointModelVisual3D(VRConvert.ConvertFromRealToVirtual(this.track.AnchorPoints[i], this.coeff), this.pointRadius));
}
foreach (var splitPoint in this.track.SplitPoints)
{
this.splitTrackModelVisual3D.Add(
this.CreateSplitPointModelVisual3D(VRConvert.ConvertFromRealToVirtual(splitPoint, this.coeff)));
}
}
/// <summary>
/// Change z coordinate of anchor point
/// </summary>
/// <param name="indexOfAnchorPoint"> begin from second point of trajectory,
/// because first point pinned to manipulator end point</param>
/// <param name="deltaZ">real deltaZ in cm</param>
public void ChangeAnchorPointZ(int indexOfAnchorPoint, double deltaZ)
{
if (indexOfAnchorPoint == 0)
{
throw new Exception("Can't change z coordinate of first trajectory point!");
}
this.track.AnchorPointOffsetZ(indexOfAnchorPoint, deltaZ);
((TranslateTransform3D)this.trackModelVisual3D[indexOfAnchorPoint * 2].Transform).OffsetZ += deltaZ * this.coeff;
if (indexOfAnchorPoint == this.track.AnchorPoints.Count - 1)
{
this.trackModelVisual3D[indexOfAnchorPoint * 2 - 1] = this.CreateTrajectoryLineModelVisual3D(
VRConvert.ConvertFromRealToVirtual(this.track.AnchorPoints[indexOfAnchorPoint - 1], this.coeff),
VRConvert.ConvertFromRealToVirtual(this.track.AnchorPoints[indexOfAnchorPoint], this.coeff),
this.trackLineRadius);
}
else
{
this.trackModelVisual3D[indexOfAnchorPoint * 2 - 1] = this.CreateTrajectoryLineModelVisual3D(
VRConvert.ConvertFromRealToVirtual(this.track.AnchorPoints[indexOfAnchorPoint - 1], this.coeff),
VRConvert.ConvertFromRealToVirtual(this.track.AnchorPoints[indexOfAnchorPoint], this.coeff),
this.trackLineRadius);
this.trackModelVisual3D[indexOfAnchorPoint * 2 + 1] = this.CreateTrajectoryLineModelVisual3D(
VRConvert.ConvertFromRealToVirtual(this.track.AnchorPoints[indexOfAnchorPoint], this.coeff),
VRConvert.ConvertFromRealToVirtual(this.track.AnchorPoints[indexOfAnchorPoint + 1], this.coeff),
this.trackLineRadius);
}
}
public void BuildModelVisual3DCollection(double thickness)
{
var boneRadius = thickness * this.coeff / 2;
var jointRadius = (thickness * this.coeff / 2) * 1.7;
var sup = new Point3D(0, 0, 0); // Start Unit Point
var eup = (Point3D)this.arm.F(0); // End Unit Point
// Zero unit of arm:
this.armModelVisual3D.Add(
this.CreateArmBoneModelVisual3D(
VRConvert.ConvertFromRealToVirtual(sup, this.coeff),
VRConvert.ConvertFromRealToVirtual(eup, this.coeff),
boneRadius));
var jointBrush = Brushes.Black;
this.armModelVisual3D.Add(
this.CreateArmJointModelVisual3D(VRConvert.ConvertFromRealToVirtual(eup, this.coeff), jointRadius, jointBrush));
sup = eup;
// 'P' and 'R' units:
for (var i = 0; i < this.arm.N; i++)
{
eup = (Point3D)this.arm.F(i + 1);
var axis = VRConvert.ConvertFromRealToVirtual(this.arm.GetZAxis(i), this.coeff);
MathFunctions.Normalize(axis);
jointBrush = this.arm.Units[i].Type == 'R' ? Brushes.OrangeRed : Brushes.Green;
this.armModelVisual3D.Add(
this.CreateArmJointModelVisual3D(
VRConvert.ConvertFromRealToVirtual(sup, this.coeff),
jointRadius,
jointBrush,
i + 1,
axis));
this.armModelVisual3D.Add(
this.CreateArmBoneModelVisual3D(
VRConvert.ConvertFromRealToVirtual(sup, this.coeff),
VRConvert.ConvertFromRealToVirtual(eup, this.coeff),
boneRadius,
i + 1));
sup = eup;
}
// Grip unit:
jointBrush = Brushes.Blue;
this.armModelVisual3D.Add(
this.CreateArmJointModelVisual3D(
VRConvert.ConvertFromRealToVirtual(sup, this.coeff),
jointRadius,
jointBrush,
this.arm.N));
}
public void AddAnchorPoint(Point3D newVirtualPoint)
{
this.track.AddAnchorPoint(VRConvert.ConvertFromVirtualToReal(newVirtualPoint, this.coeff));
var trackLineMV3D = this.CreateTrajectoryLineModelVisual3D(
VRConvert.ConvertFromRealToVirtual(
this.track.AnchorPoints[this.track.AnchorPoints.Count - 2],
this.coeff),
newVirtualPoint,
this.trackLineRadius);
var trackAnchorPointMV3D = this.CreateAnchorPointModelVisual3D(newVirtualPoint, this.pointRadius);
this.trackModelVisual3D.Add(trackLineMV3D);
this.trackModelVisual3D.Add(trackAnchorPointMV3D);
}
/// <summary>
///
/// </summary>
/// <param name="with3DLines"> if it false then only track points will be displayed</param>
public void ShowInterpolatedTrack(bool with3DLines)
{
this.splitTrackModelVisual3D.Add(
this.CreateAnchorPointModelVisual3D(VRConvert.ConvertFromRealToVirtual(this.track.SplitPoints[0], this.coeff), this.pointRadius));
for (var i = 1; i < this.track.SplitPoints.Count; i++)
{
if (with3DLines)
{
var trackLineMV3D = this.CreateTrajectoryLineModelVisual3D(
VRConvert.ConvertFromRealToVirtual(
this.track.SplitPoints[i - 1],
this.coeff),
VRConvert.ConvertFromRealToVirtual(this.track.SplitPoints[i], this.coeff),
this.trackLineRadius);
this.splitTrackModelVisual3D.Add(trackLineMV3D);
}
this.splitTrackModelVisual3D.Add(
this.CreateAnchorPointModelVisual3D(VRConvert.ConvertFromRealToVirtual(this.track.SplitPoints[i], this.coeff), this.pointRadius));
}
}
public void BeginAnimation(double[] dQ)
{
for (var i = 0; i < this.arm.N; i++)
{
switch (this.arm.Units[i].Type)
{
case 'R':
var rotAxis = VRConvert.ConvertFromRealToVirtual(this.arm.GetZAxis(i), this.coeff);
var centerRot = VRConvert.ConvertFromRealToVirtual((Point3D)this.arm.F(i), this.coeff);
for (var j = 2 * (i + 1); j < 2 * (this.arm.N + 1) + 1; j++)
{
((this.armModelVisual3D[j]
.Transform as Transform3DGroup)
.Children[2 * i + 1] as RotateTransform3D)
.CenterX = centerRot.X;
((this.armModelVisual3D[j]
.Transform as Transform3DGroup)
.Children[2 * i + 1] as RotateTransform3D)
.CenterY = centerRot.Y;
((this.armModelVisual3D[j]
.Transform as Transform3DGroup)
.Children[2 * i + 1] as RotateTransform3D)
.CenterZ = centerRot.Z;
(((this.armModelVisual3D[j]
.Transform as Transform3DGroup)
.Children[2 * i + 1] as RotateTransform3D)
.Rotation as AxisAngleRotation3D)
.Axis = rotAxis;
(((this.armModelVisual3D[j].Transform as Transform3DGroup)
.Children[2 * i + 1] as RotateTransform3D)
.Rotation as AxisAngleRotation3D)
.Angle = MathFunctions.RadianToDegree(dQ[i]);
}
break;
case 'P':
var prismaticAxis = VRConvert.ConvertFromRealToVirtual(this.arm.GetZAxis(i), this.coeff);
for (var j = 2 * (i + 1); j < 2 * (this.arm.N + 1) + 1; j++)
{
if (j == 2 * (i + 1) + 1)
{
continue;
}
((this.armModelVisual3D[j]
.Transform as Transform3DGroup)
.Children[2 * i] as TranslateTransform3D)
.OffsetX = prismaticAxis.X * dQ[i];
((this.armModelVisual3D[j]
.Transform as Transform3DGroup)
.Children[2 * i] as TranslateTransform3D)
.OffsetY = prismaticAxis.Y * dQ[i];
((this.armModelVisual3D[j]
.Transform as Transform3DGroup)
.Children[2 * i] as TranslateTransform3D)
.OffsetZ = prismaticAxis.Z * dQ[i];
}
break;
}
}
}