/// <summary>
/// Update rendering frame
/// </summary>
/// <param name="systemState">Actual system staate</param>
public void UpdateFrame(SystemState systemState)
{
frame++;
UpdateState(systemState);
UpdateCamera(systemState);
// render pendulum trajectory
if (renderPendulumTrajectory)
{
SimulationScene.Children.Remove(pendulumTrajectoryLines);
}
if (frame >= FrameLineCutNumber)
{
pendulumTrajectoryPoints.RemoveAt(0);
pendulumTrajectoryPoints.RemoveAt(0);
}
pendulumTrajectoryPoints.Add(pendulum.MassLinkPoint);
pendulumTrajectoryPoints.Add(pendulum.MassLinkPoint);
// render cart trajectory
if (renderCartTrajectory)
{
SimulationScene.Children.Remove(cartTrajectoryLines);
}
var cartEndPoint = new Point3D(systemState.StateX.Position, systemState.StateY.Position, CartTrajectoryHeight);
if (frame >= FrameLineOptimizeNumber)
{
var A = cartTrajectoryPoints[cartTrajectoryPoints.Count - 1];
var B = cartTrajectoryPoints[cartTrajectoryPoints.Count - 3];
var C = cartEndPoint;
var area = A.X * (B.Y - C.Y) + B.X * (C.Y - A.Y) + C.X * (A.Y - B.Y);
if (Math.Abs(area) < double.Epsilon)
{
cartTrajectoryPoints.RemoveAt(cartTrajectoryPoints.Count - 1);
cartTrajectoryPoints.RemoveAt(cartTrajectoryPoints.Count - 1);
}
}
cartTrajectoryPoints.Add(cartEndPoint);
cartTrajectoryPoints.Add(cartEndPoint);
if (renderPendulumTrajectory)
{
SimulationScene.Children.Add(pendulumTrajectoryLines);
}
if (renderCartTrajectory)
{
SimulationScene.Children.Add(cartTrajectoryLines);
}
}
public static Point3DCollection ReorderUsingVector(Point3DCollection Input)
{
Point3D center = KneeInnovation3D.EntityTools.PointCollections.GetCenter(Input);
Point3DCollection reordered = new Point3DCollection();
reordered.Add(Input[0]);
Input.RemoveAt(0);
double a = 1000;
int f = -1;
while (Input.Count > 1)
{
a = 1000;
f = -1;
if (Input.Count != 1)
{
for (int i = Input.Count - 1; i >= 0; i--)
{
Vector3D v1 = center - Input[i];
Vector3D v2 = center - reordered[reordered.Count - 1];
double angle = Vector3D.AngleBetween(v1, v2);
if (angle < a)
{
a = angle;
f = i;
}
}
reordered.Add(Input[f]);
Input.RemoveAt(f);
}
else
{
reordered.Add(Input[0]);
Input.RemoveAt(0);
}
}
return(reordered);
}
private void trianglesList_KeyDown(object sender, KeyEventArgs e)
{
ListBox listBox = (ListBox)sender;
if (e.Key == Key.Delete)
{
int selectedIndex = listBox.SelectedIndex;
triangles3Ds.RemoveAt(selectedIndex);
TrianglesList();
}
}
public void AddCutMove(Point3D point)
{
bool sameDir = false;
if (lastType == MoveType.Cut && minDistanceSquared > 0d)
{
// If line segments AB and BC have the same direction (small cross product) then remove point B.
var delta = new Vector3D(point.X - point0.X, point.Y - point0.Y, point.Z - point0.Z);
delta.Normalize(); // use unit vectors (magnitude 1) for the cross product calculations
Vector3D cp; double xp2;
//if (path.Points.Count > (trace.Count == 1 ? 1 : 0))
//{
cp = Vector3D.CrossProduct(delta, delta0);
xp2 = cp.LengthSquared;
sameDir = xp2 > 0d && (xp2 < 0.0005d); // approx 0.001 seems to be a reasonable threshold from logging xp2 values
//if (!sameDir) Title = string.Format("xp2={0:F6}", xp2);
//}
if (sameDir) // extend the current line segment
{
//var last = linePoints.Last();
//last.X = point.X;
//last.Y = point.Y;
//last.Z = point.Z;
linePoints.RemoveAt(linePoints.Count - 1);
linePoints.Add(point);
delta0 += delta;
cutCount++;
}
else
{
if ((point - point0).LengthSquared < minDistanceSquared)
{
return; // less than min distance from last point
}
delta0 = delta;
}
}
if (!sameDir)
{
cutCount = 1;
linePoints.Add(point0);
linePoints.Add(point);
}
lastType = MoveType.Cut;
point0 = point;
}
public void Render()
{
bool showPointCloud = false;
bool showKeyFrameTrajectory = false;
bool showFrameTrajectory = false;
bool onlyNew = true;
_SyncContext.Send(d =>
{
showPointCloud = ShowPointCloud;
showKeyFrameTrajectory = ShowKeyFrameTrajectory;
showFrameTrajectory = ShowFrameTrajectory;
onlyNew = !(showPointCloud && Model != null && (Model as Model3DGroup).Children.Count == 0);
}, null);
if (_Map.HasFrames() || _Map.HasKeyFrames())
{
List <Vector3> pointsKeyFrame = new List <Vector3>();
List <Vector3> pointsFrame = new List <Vector3>();
if (showKeyFrameTrajectory)
{
pointsKeyFrame = _Map.GetTrajectory(TrajectoryType.Optimazation);
}
if (showFrameTrajectory)
{
pointsFrame = _Map.GetTrajectory(TrajectoryType.PreOptimazation);
}
List <GeometryModel3D> pointClouds = new List <GeometryModel3D>();
if (showPointCloud)
{
pointClouds = _Map.GetPointCloud(onlyNew);
}
_SyncContext.Post(d =>
{
TrajectoryKeyFrame = new Point3DCollection(pointsKeyFrame.SelectMany(c => new List <Point3D>()
{
new Point3D(c.X, c.Y, c.Z), new Point3D(c.X, c.Y, c.Z)
}));
if (TrajectoryKeyFrame.Count > 0)
{
TrajectoryKeyFrame.RemoveAt(0);
}
TrajectoryFrame = new Point3DCollection(pointsFrame.SelectMany(c => new List <Point3D>()
{
new Point3D(c.X, c.Y, c.Z), new Point3D(c.X, c.Y, c.Z)
}));
if (TrajectoryFrame.Count > 0)
{
TrajectoryFrame.RemoveAt(0);
}
CameraPosition = new MatrixTransform3D(_Map.LastTransformation().Matrix3D);
if (Model == null || !showPointCloud)
{
Model = new Model3DGroup();
}
foreach (GeometryModel3D pointCloud in pointClouds)
{
(Model as Model3DGroup).Children.Add(pointCloud);
}
if (ShowKeyFrameOrientations)
{
foreach (Visual3D visual3D in _Map.GetKeyFrameOrientations(CoordinateSystems.Count != 0))
{
CoordinateSystems.Add(visual3D);
}
}
else
{
CoordinateSystems.Clear();
}
}, null);
}
else
{
_SyncContext.Post(d =>
{
if (TrajectoryFrame != null && TrajectoryFrame.Count > 0)
{
TrajectoryFrame = new Point3DCollection();
}
if (TrajectoryKeyFrame != null && TrajectoryKeyFrame.Count > 0)
{
TrajectoryKeyFrame = new Point3DCollection();
}
}, null);
}
}
/// <summary>
///
/// </summary>
/// <param name="vertices"></param>
/// <param name="normals"></param>
/// <param name="indices"></param>
/// <param name="textures"></param>
protected void TriangleSubdivide(Point3DCollection vertices,
Vector3DCollection normals,
Int32Collection indices,
PointCollection textures)
{
for (int i = 0; i < 3; i++)
{
verticesBase[2 - i] = vertices[vertices.Count - 1];
normalsBase[2 - i] = normals[vertices.Count - 1];
texturesBase[2 - i] = textures[vertices.Count - 1];
vertices.RemoveAt(vertices.Count - 1);
normals.RemoveAt(normals.Count - 1);
indices.RemoveAt(indices.Count - 1);
textures.RemoveAt(textures.Count - 1);
}
int indexStart = vertices.Count;
for (int slice = 0; slice <= Slices; slice++)
{
double weight = (double)slice / Slices;
Point3D vertex1 = Point3DWeight(verticesBase[0], verticesBase[1], weight);
Point3D vertex2 = Point3DWeight(verticesBase[0], verticesBase[2], weight);
Vector3D normal1 = Vector3DWeight(normalsBase[0], normalsBase[1], weight);
Vector3D normal2 = Vector3DWeight(normalsBase[0], normalsBase[2], weight);
Point texture1 = PointWeight(texturesBase[0], texturesBase[1], weight);
Point texture2 = PointWeight(texturesBase[0], texturesBase[2], weight);
for (int i = 0; i <= slice; i++)
{
weight = (double)i / slice;
if (Double.IsNaN(weight))
{
weight = 0;
}
vertices.Add(Point3DWeight(vertex1, vertex2, weight));
normals.Add(Vector3DWeight(normal1, normal2, weight));
textures.Add(PointWeight(texture1, texture2, weight));
}
}
for (int slice = 0; slice < Slices; slice++)
{
int base1 = (slice + 1) * slice / 2;
int base2 = base1 + slice + 1;
for (int i = 0; i <= 2 * slice; i++)
{
int half = i / 2;
if ((i & 1) == 0) // even
{
indices.Add(indexStart + base1 + half);
indices.Add(indexStart + base2 + half);
indices.Add(indexStart + base2 + half + 1);
}
else // odd
{
indices.Add(indexStart + base1 + half);
indices.Add(indexStart + base2 + half + 1);
indices.Add(indexStart + base1 + half + 1);
}
}
}
}
private void detectGestures()
{
if (selectedChannels == 1)
{
foreach (List <int> subList in history)
{
int signChanges = 0, bandwidth = 0, step = 0, lastSig = 0;
for (int i = 1; i < subList.Count; i++)
{
step++;
if (subList[i - 1] != 0)
{
lastSig = subList[i - 1];
}
if (subList[i] * lastSig < 0)
{
signChanges++;
bandwidth += step;
step = 0;
}
}
if (KF[0].isBoth && KF[0].inverse_state > 5)
{
gestureDetected.Text = "Two Handed ";
}
else if (signChanges == 0 && (lastSig != 0))
{
gestureDetected.Text = "Scrolling ";
}
else if (signChanges == 2 || signChanges == 1)
{
gestureDetected.Text = "SingleTap ";
}
else if (signChanges >= 3)
{
gestureDetected.Text = "DoubleTap ";
}
}
}
else if (selectedChannels == 2)
{
double tot_X = 0, tot_Y = 0;
foreach (KeyFrequency now in KF)
{
tot_X += now.x;
tot_Y += now.y;
}
pointHist.Add(new Point(tot_X, tot_Y));
if (pointHist.Count > maxHistoryLength)
{
pointHist.RemoveAt(0);
}
generateStroke(pointHist);
}
else if (selectedChannels >= 3)
{
double tot_X = 0, tot_Y = 0, tot_Z = 0;
foreach (KeyFrequency now in KF)
{
tot_X += now.x;
tot_Y += now.y;
tot_Z += now.z;
}
pointHist.Add(new Point(tot_X, tot_Y));
point3DHist.Add(new Point3D(tot_X, tot_Y, tot_Z));
if (pointHist.Count > maxHistoryLength)
{
pointHist.RemoveAt(0);
}
if (point3DHist.Count > maxHistoryLength)
{
point3DHist.RemoveAt(0);
}
generateStroke(pointHist);
}
gestureCompleted();
}
