private void lblLargeMap_MouseUp(object sender, MouseButtonEventArgs e)
{
_isLargeMap = !_isLargeMap;
// Come up with new values (and color the control)
Point3D cameraPos = new Point3D();
double mouseCursorScale = 1d;
if (_isLargeMap)
{
lblLargeMap.Background = _toggleBrush;
_boundryMin = new Vector3D(-WIDTH_LARGE, -WIDTH_LARGE, -DEPTH);
_boundryMax = new Vector3D(WIDTH_LARGE, WIDTH_LARGE, DEPTH);
cameraPos = new Point3D(0, 0, CAMERAZ_LARGE);
mouseCursorScale = MOUSECURSORLARGESCALE;
}
else
{
lblLargeMap.Background = Brushes.Transparent;
_boundryMin = new Vector3D(-WIDTH_SMALL, -WIDTH_SMALL, -DEPTH);
_boundryMax = new Vector3D(WIDTH_SMALL, WIDTH_SMALL, DEPTH);
cameraPos = new Point3D(0, 0, CAMERAZ_SMALL);
}
#region Change Boundry
foreach (ScreenSpaceLines3D line in _lines)
{
_viewport.Children.Remove(line);
}
_lines.Clear();
List <Point3D[]> innerLines, outerLines;
_world.SetCollisionBoundry(out innerLines, out outerLines, _viewport, _boundryMin, _boundryMax);
//TODO: Make a private method that does this
Color lineColor = Color.FromArgb(255, 200, 200, 180);
foreach (Point3D[] line in innerLines)
{
// Need to wait until the window is loaded to call lineModel.CalculateGeometry
ScreenSpaceLines3D lineModel = new ScreenSpaceLines3D(false);
lineModel.Thickness = 1d;
lineModel.Color = lineColor;
lineModel.AddLine(line[0], line[1]);
_viewport.Children.Add(lineModel);
_lines.Add(lineModel);
}
RecalculateLineGeometries();
#endregion
_mouseCursorGeometry.Transform = new ScaleTransform3D(mouseCursorScale, mouseCursorScale, mouseCursorScale);
_camera.Position = cameraPos;
}
public Plot3D()
{
_wireframes = new ScreenSpaceLines3D
{
Color = Colors.SteelBlue
};
_axis = new ScreenSpaceLines3D
{
Color = Colors.Black,
Thickness = 0.5
};
_model3d = new Model3D();
_transformMatrix = new TransformMatrix();
_selectRect = new ViewportRect();
InitializeComponent();
var tg = TrackBall.Transform as Transform3DGroup;
if (tg != null && tg.Children.Count == 2)
{
_scale = (ScaleTransform3D)tg.Children[0];
_rotate = (AxisAngleRotation3D)((RotateTransform3D)tg.Children[1]).Rotation;
TrackBall.Viewport3D.Camera.Transform = tg;
}
MainViewport.Children.Add(_wireframes);
MainViewport.Children.Add(_axis);
TrackBall.IsManipulationEnabled = true;
TrackBall.ManipulationDelta += TouchDelta;
TrackBall.ManipulationStarted += TouchStarted;
TrackBall.PreviewMouseWheel += MouseWheelRotated;
}
private void AddAABBLines(Point3D min, Point3D max, Color color)
{
ScreenSpaceLines3D lineVisual = new ScreenSpaceLines3D(true);
lineVisual.Thickness = 1;
lineVisual.Color = color;
// Top
lineVisual.AddLine(new Point3D(min.X, min.Y, min.Z), new Point3D(max.X, min.Y, min.Z));
lineVisual.AddLine(new Point3D(max.X, min.Y, min.Z), new Point3D(max.X, max.Y, min.Z));
lineVisual.AddLine(new Point3D(max.X, max.Y, min.Z), new Point3D(min.X, max.Y, min.Z));
lineVisual.AddLine(new Point3D(min.X, max.Y, min.Z), new Point3D(min.X, min.Y, min.Z));
// Bottom
lineVisual.AddLine(new Point3D(min.X, min.Y, max.Z), new Point3D(max.X, min.Y, max.Z));
lineVisual.AddLine(new Point3D(max.X, min.Y, max.Z), new Point3D(max.X, max.Y, max.Z));
lineVisual.AddLine(new Point3D(max.X, max.Y, max.Z), new Point3D(min.X, max.Y, max.Z));
lineVisual.AddLine(new Point3D(min.X, max.Y, max.Z), new Point3D(min.X, min.Y, max.Z));
// Sides
lineVisual.AddLine(new Point3D(min.X, min.Y, min.Z), new Point3D(min.X, min.Y, max.Z));
lineVisual.AddLine(new Point3D(max.X, min.Y, min.Z), new Point3D(max.X, min.Y, max.Z));
lineVisual.AddLine(new Point3D(max.X, max.Y, min.Z), new Point3D(max.X, max.Y, max.Z));
lineVisual.AddLine(new Point3D(min.X, max.Y, min.Z), new Point3D(min.X, max.Y, max.Z));
_viewport.Children.Add(lineVisual);
_visuals.Add(lineVisual);
}
private ProjectedGeometry MagicLinesToProjectedGeometry(ScreenSpaceLines3D lines, Transform3D tx)
{
// These matrices are the same as the ones in MeshToProjectedGeometry
Matrix3D modelToWorld = tx.Value;
Matrix3D worldToEye = MatrixUtils.ViewMatrix(camera);
Matrix3D modelToEyeMatrix = modelToWorld * worldToEye;
Matrix3D projectionMatrix = MatrixUtils.ProjectionMatrix(camera);
Matrix3D toScreenSpace = MatrixUtils.HomogenousToScreenMatrix(bounds.ViewportBounds, camera is ProjectionCamera);
Matrix3D projectToViewport = projectionMatrix * toScreenSpace;
// For n points, there are n-1 lines.
int numLines = (lines.Points == null) ? 0 : lines.Points.Count - 1;
// ScreenSpaceLine triangles are created post-projection, therefore reverse winding caused by
// having a negative determinant in the modelToEyeMatrix will not affect our rendering.
SSLProjectedGeometry geometry = new SSLProjectedGeometry(projectToViewport);
for (int n = 0; n < numLines; n++)
{
Point3D begin = lines.Points[n];
Point3D end = lines.Points[n + 1];
begin *= modelToEyeMatrix;
end *= modelToEyeMatrix;
geometry.AddScreenSpaceLine(begin, end, lines.Thickness, lines.Color);
}
return(geometry);
}
/// <summary>
/// Dispose the volume cube and axes
/// </summary>
private void DisposeVolumeCube3DGraphics()
{
if (HaveAddedVolumeCube)
{
this.RemoveVolumeCube3DGraphics();
}
if (null != this.volumeCube)
{
this.volumeCube.Dispose();
this.volumeCube = null;
}
if (null != this.volumeCubeAxisX)
{
this.volumeCubeAxisX.Dispose();
this.volumeCubeAxisX = null;
}
if (null != this.volumeCubeAxisY)
{
this.volumeCubeAxisY.Dispose();
this.volumeCubeAxisY = null;
}
if (null != this.volumeCubeAxisZ)
{
this.volumeCubeAxisZ.Dispose();
this.volumeCubeAxisZ = null;
}
}
public static void CreateTriangleFace(Point3D p0, Point3D p1, Point3D p2,
Color color, bool isWireframe, Viewport3D viewport)
{
MeshGeometry3D mesh = new MeshGeometry3D();
mesh.Positions.Add(p0);
mesh.Positions.Add(p1);
mesh.Positions.Add(p2);
mesh.TriangleIndices.Add(0);
mesh.TriangleIndices.Add(1);
mesh.TriangleIndices.Add(2);
SolidColorBrush brush = new SolidColorBrush();
brush.Color = color;
Material material = new DiffuseMaterial(brush);
GeometryModel3D geometry = new GeometryModel3D(mesh, material);
ModelUIElement3D model = new ModelUIElement3D();
model.Model = geometry;
viewport.Children.Add(model);
if (isWireframe == true)
{
ScreenSpaceLines3D ssl = new ScreenSpaceLines3D();
ssl.Points.Add(p0);
ssl.Points.Add(p1);
ssl.Points.Add(p1);
ssl.Points.Add(p2);
ssl.Points.Add(p2);
ssl.Points.Add(p0);
ssl.Color = Colors.Black;
ssl.Thickness = 2;
viewport.Children.Add(ssl);
}
}
private ScreenSpaceLines3D[] AddLines(DoubleVector transformedLine, Vector3D rotationAxis)
{
ScreenSpaceLines3D[] retVal = new ScreenSpaceLines3D[4];
for (int cntr = 0; cntr < retVal.Length; cntr++)
{
retVal[cntr] = new ScreenSpaceLines3D(true);
retVal[cntr].Color = _colors.RotatedLine;
if (cntr == 1)
{
retVal[cntr].Thickness = 3d; // this one isn't really used
}
else if (cntr == 3)
{
retVal[cntr].Thickness = 1d;
}
else
{
retVal[cntr].Thickness = 6d;
}
}
retVal[0].AddLine(new Point3D(0, 0, 0), transformedLine.Standard.ToPoint());
retVal[1].AddLine(new Point3D(0, 0, 0), new Point3D(0, 0, 0)); // just adding this so everything stays lined up
retVal[2].AddLine(new Point3D(0, 0, 0), transformedLine.Orth.ToPoint());
retVal[3].AddLine(new Point3D(0, 0, 0), rotationAxis.ToPoint());
for (int cntr = 0; cntr < retVal.Length; cntr++)
{
_viewport.Children.Add(retVal[cntr]);
}
return(retVal);
}
public override void OnApplyTemplate()
{
base.OnApplyTemplate();
this._screenSpaceLines3D = this.Template.Resources["_screenSpaceLines3D"] as ScreenSpaceLines3D;
this._trackport3D = GetTemplateChild("PART_Trackport3D") as Trackport3D;
}
private void button1_Click(object sender, RoutedEventArgs e)
{
for (int cntr = 1; cntr <= 1; cntr++)
{
Color lineColor = UtilityWPF.GetRandomColor(64, 192);
ScreenSpaceLines3D lineModel = new ScreenSpaceLines3D(false);
lineModel.Thickness = 1d;
lineModel.Color = lineColor;
Point3D fromPoint = Math3D.GetRandomVector(15d).ToPoint();
Point3D toPoint = Math3D.GetRandomVector(15d).ToPoint();
//lineModel.AddLine(fromPoint, toPoint);
lineModel.Points.Add(fromPoint);
lineModel.Points.Add(toPoint);
_viewport.Children.Add(lineModel);
lineModel.CalculateGeometry();
_lastLine = lineModel;
}
}
private void SetCollisionBoundry()
{
// Remove existing lines from the viewport
if (_lines.Count > 0)
{
foreach (ScreenSpaceLines3D line in _lines)
{
_viewport.Children.Remove(line);
}
}
// Tell the world about the collision boundry (it will clean itself up if this is a second call)
var boundryLines = _world.SetCollisionBoundry(new Point3D(-100, -100, -100), new Point3D(100, 100, 100));
// Draw the lines
Color lineColor = Color.FromArgb(32, 0, 0, 0);
foreach (var line in boundryLines.innerLines)
{
ScreenSpaceLines3D lineModel = new ScreenSpaceLines3D(false);
lineModel.Thickness = 1d;
lineModel.Color = lineColor;
lineModel.AddLine(line.from, line.to);
_viewport.Children.Add(lineModel);
_lines.Add(lineModel);
}
RecalculateLineGeometries();
}
public ModelVisual3D CreateSurface(Function f)
{
double dx = (Xmax - Xmin) / Nx;
double dz = (Zmax - Zmin) / Nz;
if (Nx < 2 || Nz < 2)
{
return(null);
}
Model3DGroup surface = new Model3DGroup();
Point3D[,] pts = new Point3D[Nx, Nz];
for (int i = 0; i < Nx; i++)
{
double x = Xmin + i * dx;
for (int j = 0; j < Nz; j++)
{
double z = Zmin + j * dz;
pts[i, j] = GetNormalize(f(x, z));
pts[i, j] += (Vector3D)Center;
}
}
Point3D[] p = new Point3D[4];
for (int i = 0; i < Nx - 1; i++)
{
for (int j = 0; j < Nz - 1; j++)
{
p[0] = pts[i, j];
p[1] = pts[i, j + 1];
p[2] = pts[i + 1, j + 1];
p[3] = pts[i + 1, j];
surface.Children.Add(CreateTriangleModel(p[0], p[1], p[2]));
surface.Children.Add(CreateTriangleModel(p[2], p[3], p[0]));
ScreenSpaceLines3D ssl = new ScreenSpaceLines3D();
ssl.Points.Add(p[0]);
ssl.Points.Add(p[1]);
ssl.Points.Add(p[1]);
ssl.Points.Add(p[2]);
ssl.Points.Add(p[2]);
ssl.Points.Add(p[3]);
ssl.Points.Add(p[3]);
ssl.Points.Add(p[0]);
ssl.Color = LineColor;
ssl.Thickness = 2;
ViewPort3d.Children.Add(ssl);
}
}
ModelVisual3D modelVisual3D = new ModelVisual3D();
modelVisual3D.Content = surface;
return(modelVisual3D);
}
private void Render(ScreenSpaceLines3D lines)
{
ProjectedGeometry geometry = MagicLinesToProjectedGeometry(lines, lines.Transform);
Shader shader = new ScreenSpaceLinesShader(geometry.FrontFaceTriangles, buffer);
shader.Rasterize(bounds.RenderBounds);
// SSL are always front facing ( CCW winding ) triangles
RenderSilhouetteTolerance(geometry, buffer, VisibleFaces.Front);
}
private void ShowLine(Point3D point1, Point3D point2, Color color)
{
ScreenSpaceLines3D line = new ScreenSpaceLines3D();
line.Color = color;
line.Thickness = 1;
line.AddLine(point1, point2);
_debugVisuals.Add(line);
_viewport.Children.Add(line);
}
internal static void Stop(ModelVisual3D VP3D)
{
if (ssl == null)
{
return;
}
VP3D.Children.Remove(ssl);
ssl.ShutDown();
ssl = null;
}
private static Visual3D GetShipCompassBlip(Bot ship)
{
ScreenSpaceLines3D retVal = new ScreenSpaceLines3D(true);
retVal.Thickness = 2d;
retVal.Color = Colors.DodgerBlue;
retVal.AddLine(new Point3D(0, 170, 20), new Point3D(0, 1000, 20));
// Exit Function
return(retVal);
}
/// <summary>
/// This summary has not been prepared yet. NOSUMMARY - pantal07
/// </summary>
public static Model3D MakeModel(string model, Material material, Material backMaterial)
{
string exceptions = string.Empty;
try
{
MeshGeometry3D mesh = MeshFactory.MakeMesh(model);
Material front = (material == null) ? MaterialFactory.Default : material;
Material back = backMaterial;
GeometryModel3D model2 = new GeometryModel3D(mesh, front);
model2.BackMaterial = back;
return(model2);
}
catch (ArgumentException ex)
{
/* It wasn't a mesh */
exceptions += ex.Message + ", ";
}
#if SSL
try
{
ScreenSpaceLines3D lines = ScreenSpaceLinesFactory.MakeLines(model);
return(lines);
}
catch (ArgumentException)
{
/* It wasn't ScreenSpaceLines */
exceptions += ex.Message + ", ";
}
#endif
try
{
Light light = LightFactory.MakeLight(model);
return(light);
}
catch (ArgumentException ex)
{
/* It wasn't a Light */
exceptions += ex.Message + ", ";
}
try
{
Model3DGroup group = SceneFactory.MakeScene(model);
return(group);
}
catch (ArgumentException ex)
{
/* It wasn't a Scene */
exceptions += ex.Message;
}
throw new ArgumentException("Cannot create (" + model + "). Exceptions thrown: " + exceptions);
}
private void BuildAxes()
{
ScreenSpaceLines3D _axisX = BuildScreenSpaceLines3D(new Point3D(5, 0, 0), new Point3D(-5, 0, 0));
ScreenSpaceLines3D _axisY = BuildScreenSpaceLines3D(new Point3D(0, 5, 0), new Point3D(0, -5, 0));
ScreenSpaceLines3D _axisZ = BuildScreenSpaceLines3D(new Point3D(0, 0, 10), new Point3D(0, 0, -10));
myViewPort3D.Children.Add(_axisX);
myViewPort3D.Children.Add(_axisY);
myViewPort3D.Children.Add(_axisZ);
//return _axes;
}
private ScreenSpaceLines3D BuildScreenSpaceLines3D(Point3D point1, Point3D point2)
{
ScreenSpaceLines3D line = new ScreenSpaceLines3D();
line.Points.Add(point1);
line.Points.Add(point2);
line.Thickness = 2;
line.Color = Colors.Black;
return(line);
}
internal static void Start(ModelVisual3D VP3D, RotateTransform3D _rotxform)
{
ssl = new ScreenSpaceLines3D();
rotxform = _rotxform;
UpdateLocation();
ssl.Thickness = 2;
ssl.Color = Color.FromRgb(255, 0, 0);
VP3D.Children.Add(ssl);
}
public static Visual3D GetGuideLineDouble(Axis axis, Color color)
{
Transform3D transform = GetTransform(axis, true);
ScreenSpaceLines3D retVal = new ScreenSpaceLines3D(true);
retVal.Thickness = 1d;
retVal.Color = color;
retVal.AddLine(transform.Transform(new Point3D(-.75d, 0, 0)), transform.Transform(new Point3D(.75d, 0, 0)));
return(retVal);
}
/*
* 构造函数，初始化文件路径
*/
public ShowSTL3D(string filePath)
{
m_listPoint3D = new List <Point3D>();
m_listPoint3D.Clear();
preCameraDirection = new Vector3D();
preVisualDirection = new Vector3D();
_worldLine = new ScreenSpaceLines3D();
this.filePath = filePath;
ModelPos = new Point(0, 0);
}
public static Visual3D GetGuideLine(Axis axis, bool positiveDirection, Color color)
{
Transform3D transform = GetTransform(axis, positiveDirection);
ScreenSpaceLines3D retVal = new ScreenSpaceLines3D(true);
retVal.Thickness = 1d;
retVal.Color = color;
retVal.AddLine(new Point3D(0, 0, 0), transform.Transform(new Point3D(.75d, 0, 0)));
return(retVal);
}
private ModelVisual3D GetShipCompassBlip(Ship ship)
{
ScreenSpaceLines3D retVal = new ScreenSpaceLines3D(true);
retVal.Thickness = 2d;
retVal.Color = Colors.DodgerBlue;
retVal.AddLine(new Point3D(0, 170, 20), new Point3D(0, 1000, 20));
retVal.Transform = new TranslateTransform3D(ship.PositionWorld.ToVector());
// Exit Function
return(retVal);
}
private void ResetField()
{
_sceneRemaining = DateTime.MinValue;
if (_border != null)
{
_viewport.Children.Remove(_border);
}
if (_marker2D != null)
{
_viewport.Children.Remove(_marker2D);
}
_velocityLines.Clear();
_sizeMult = 1d / _field.Size;
_velocityMult = 4d;
// Border Lines
_border = new ScreenSpaceLines3D();
_border.Color = Colors.DimGray;
_border.Thickness = 1d;
double l = (_field.Size * _sizeMult) / 2d; // using lower case l to represent half (just because it looks a lot like 1)
_border.AddLine(new Point3D(-l, -l, -l), new Point3D(l, -l, -l));
_border.AddLine(new Point3D(l, -l, -l), new Point3D(l, l, -l));
_border.AddLine(new Point3D(l, l, -l), new Point3D(-l, l, -l));
_border.AddLine(new Point3D(-l, l, -l), new Point3D(-l, -l, -l));
_border.AddLine(new Point3D(-l, -l, l), new Point3D(l, -l, l));
_border.AddLine(new Point3D(l, -l, l), new Point3D(l, l, l));
_border.AddLine(new Point3D(l, l, l), new Point3D(-l, l, l));
_border.AddLine(new Point3D(-l, l, l), new Point3D(-l, -l, l));
_border.AddLine(new Point3D(-l, -l, -l), new Point3D(-l, -l, l));
_border.AddLine(new Point3D(l, -l, -l), new Point3D(l, -l, l));
_border.AddLine(new Point3D(l, l, -l), new Point3D(l, l, l));
_border.AddLine(new Point3D(-l, l, -l), new Point3D(-l, l, l));
_viewport.Children.Add(_border);
ShowHideBlockedCells(); // this will update _blockedCellsWireframe
if (_lookDirection != null)
{
ViewChanged(_lookDirection.Value); // this will update _marker2D
}
}
private Visual3D[] GetStrokeVisual(Tuple <Point3D, Vector3D>[] points)
{
Model3DGroup models = new Model3DGroup();
MaterialGroup materials = new MaterialGroup();
materials.Children.Add(new DiffuseMaterial(new SolidColorBrush(UtilityWPF.ColorFromHex("EEE"))));
materials.Children.Add(new SpecularMaterial(new SolidColorBrush(UtilityWPF.ColorFromHex("FFF")), 4));
ScreenSpaceLines3D lines = new ScreenSpaceLines3D();
lines.Color = UtilityWPF.ColorFromHex("EEE");
lines.Thickness = 2;
for (int cntr = 0; cntr < points.Length - 1; cntr++)
{
#region velocity line
Point3D secondPoint = points[cntr].Item1;
if (points[cntr].Item2.IsNearZero())
{
secondPoint += Math3D.GetRandomVector_Spherical(.02);
}
else
{
secondPoint += points[cntr].Item2 * .75;
}
lines.AddLine(points[cntr].Item1, secondPoint);
#endregion
#region dot
GeometryModel3D geometry = new GeometryModel3D();
geometry.Material = materials;
geometry.BackMaterial = materials;
geometry.Geometry = UtilityWPF.GetSphere_Ico(.15, 1, true);
geometry.Transform = new TranslateTransform3D(points[cntr].Item1.ToVector());
models.Children.Add(geometry);
#endregion
}
ModelVisual3D dotVisual = new ModelVisual3D();
dotVisual.Content = models;
return(new Visual3D[] { lines, dotVisual });
}
public MapObject_ChasePoint_Direct(IMapObject item, Vector3D offset, bool shouldMoveWithSpring, bool shouldSpringCauseTorque, bool shouldDampenWhenSpring, Viewport3D viewport, Color?springColor = null)
{
this.Item = item;
this.Offset = item.PhysicsBody.DirectionFromWorld(offset); // convert to model coords
_viewport = viewport;
this.SpringForceMultiplier = 1d;
// Newton uses zero (or maybe negative?) mass for bodies that ignore physics. So a spring is only effective on
// bodies with mass
//TODO: See if PhysicsBody.IsFrozen will block the spring
if (shouldMoveWithSpring && item.PhysicsBody.MassMatrix.Mass > 0)
{
#region Init spring
this.IsUsingSpring = true;
this.ShouldDampenWhenSpring = shouldDampenWhenSpring;
_shouldSpringCauseTorque = shouldSpringCauseTorque;
if (springColor != null)
{
_springVisual = new ScreenSpaceLines3D();
_springVisual.Thickness = 1d;
_springVisual.Color = springColor.Value;
_viewport.Children.Add(_springVisual);
}
else
{
_springVisual = null;
}
_springConstant = item.PhysicsBody.MassMatrix.Mass * 50d;
this.Item.PhysicsBody.ApplyForceAndTorque += new EventHandler <NewtonDynamics.BodyApplyForceAndTorqueArgs>(PhysicsBody_ApplyForceAndTorque);
#endregion
}
else
{
#region Init direct move
this.IsUsingSpring = false;
this.ShouldDampenWhenSpring = false;
_shouldSpringCauseTorque = false;
_springVisual = null;
_springConstant = 0d;
#endregion
}
}
private void ShowLine(IEnumerable <Tuple <Point3D, Point3D> > lines, Color color)
{
ScreenSpaceLines3D line = new ScreenSpaceLines3D();
line.Color = color;
line.Thickness = 1;
foreach (var segment in lines)
{
line.AddLine(segment.Item1, segment.Item2);
}
_debugVisuals.Add(line);
_viewport.Children.Add(line);
}
public static ScreenSpaceLines3D GetScreenSpaceLines3D(Point3DCollection pcol, Color mat)
{
ScreenSpaceLines3D spacerlines = new ScreenSpaceLines3D();
spacerlines.Thickness = 2;
spacerlines.Color = mat;
for (int i = 1; i < pcol.Count; i++)
{
spacerlines.Points.Add(pcol[i - 1]);
spacerlines.Points.Add(pcol[i]);
}
return(spacerlines);
}
void addSkeletonLine(Joint j1, Joint j2)
{
if (j1.TrackingState == TrackingState.NotTracked || j2.TrackingState == TrackingState.NotTracked)
{
return;
}
for (int i = 0; i < kinectList.Count; ++i)
{
CameraSpacePoint p = new CameraSpacePoint();
p.X = (float)transfMatrix[i][0, 3];
p.Y = (float)transfMatrix[i][1, 3];
p.Z = (float)transfMatrix[i][2, 3];
//Too close to kinect origin
if (Math.Sqrt((j1.Position.Z - p.Z) * (j1.Position.Z - p.Z) + (j1.Position.X - p.X) * (j1.Position.X - p.X) + (j1.Position.Y - p.Y) * (j1.Position.Y - p.Y)) < 0.2 ||
Math.Sqrt((j2.Position.Z - p.Z) * (j2.Position.Z - p.Z) + (j2.Position.X - p.X) * (j2.Position.X - p.X) + (j2.Position.Y - p.Y) * (j2.Position.Y - p.Y)) < 0.2)
{
return;
}
}
int width = 5;
Color trackedColor = Colors.Violet;
Color inferredColor = Colors.Gray;
Color c;
if (j1.TrackingState == TrackingState.Tracked &&
j2.TrackingState == TrackingState.Tracked)
{
c = trackedColor;
}
else
{
c = inferredColor;
}
ScreenSpaceLines3D l = new ScreenSpaceLines3D();
l.Thickness = width;
l.Color = c;
l.Points.Add(getPoint3DfromJoint(j1));
l.Points.Add(getPoint3DfromJoint(j2));
this.show_viewport3D.Children.Add(l);
}
public static void CreateWireframe(Point3D p0, Point3D p1, Point3D p2, Point3D p3,
Color lineColor, Viewport3D viewport)
{
ScreenSpaceLines3D ssl = new ScreenSpaceLines3D();
ssl.Points.Add(p0);
ssl.Points.Add(p1);
ssl.Points.Add(p1);
ssl.Points.Add(p2);
ssl.Points.Add(p2);
ssl.Points.Add(p3);
ssl.Points.Add(p3);
ssl.Points.Add(p0);
ssl.Color = lineColor;
ssl.Thickness = 2;
viewport.Children.Add(ssl);
}
/// <summary>
/// Dispose the coordinate cross axes from the visual tree
/// </summary>
private void DisposeAxisAlignedCoordinateCross3DGraphics()
{
if (this.haveAddedCoordinateCross)
{
this.RemoveAxisAlignedCoordinateCross3DGraphics();
}
if (null != this.axisX)
{
this.axisX.Dispose();
this.axisX = null;
}
if (null != this.axisY)
{
this.axisY.Dispose();
this.axisY = null;
}
if (null != this.axisZ)
{
this.axisZ.Dispose();
this.axisZ = null;
}
}
/// <summary>
/// Create an axis-aligned coordinate cross for rendering in the WPF3D coordinate system.
/// Red is the +X axis, Green is the +Y axis, Blue is the +Z axis
/// </summary>
/// <param name="crossOrigin">The origin of the coordinate cross in world space.</param>
/// <param name="axisSize">The size of the axis in m.</param>
/// <param name="thickness">The thickness of the lines in screen pixels.</param>
private void CreateAxisAlignedCoordinateCross3DGraphics(Point3D crossOrigin, float axisSize, int thickness)
{
this.axisX = new ScreenSpaceLines3D();
this.axisX.Points = new Point3DCollection();
this.axisX.Points.Add(crossOrigin);
this.axisX.Points.Add(new Point3D(crossOrigin.X + axisSize, crossOrigin.Y, crossOrigin.Z));
this.axisX.Thickness = 2;
this.axisX.Color = System.Windows.Media.Color.FromArgb(200, 255, 0, 0); // Red (X)
this.axisY = new ScreenSpaceLines3D();
this.axisY.Points = new Point3DCollection();
this.axisY.Points.Add(crossOrigin);
this.axisY.Points.Add(new Point3D(crossOrigin.X, crossOrigin.Y + axisSize, crossOrigin.Z));
this.axisY.Thickness = 2;
this.axisY.Color = System.Windows.Media.Color.FromArgb(200, 0, 255, 0); // Green (Y)
this.axisZ = new ScreenSpaceLines3D();
this.axisZ.Points = new Point3DCollection();
this.axisZ.Points.Add(crossOrigin);
this.axisZ.Points.Add(new Point3D(crossOrigin.X, crossOrigin.Y, crossOrigin.Z + axisSize));
this.axisZ.Thickness = thickness;
this.axisZ.Color = System.Windows.Media.Color.FromArgb(200, 0, 0, 255); // Blue (Z)
}
/// <summary>
/// Create an axis-aligned volume cube for rendering.
/// </summary>
/// <param name="color">The color of the volume cube.</param>
/// <param name="thickness">The thickness of the lines in screen pixels.</param>
/// <param name="translation">World to volume translation vector.</param>
private void CreateCube3DGraphics(System.Windows.Media.Color color, int thickness, Vector3D translation)
{
// Scaler for cube size
float cubeSizeScaler = 1.0f;
// Before we created a volume which contains the head
// Here we create a graphical representation of this volume cube
float oneOverVpm = 1.0f / this.voxelsPerMeter;
// This cube is world axis aligned
float cubeSideX = this.voxelsX * oneOverVpm * cubeSizeScaler;
float halfSideX = cubeSideX * 0.5f;
float cubeSideY = this.voxelsY * oneOverVpm * cubeSizeScaler;
float halfSideY = cubeSideY * 0.5f;
float cubeSideZ = this.voxelsZ * oneOverVpm * cubeSizeScaler;
float halfSideZ = cubeSideZ * 0.5f;
// The translation vector is from the origin to the volume front face
// And here we describe the translation Z as from the origin to the cube center
// So we continue to translate half volume size align Z
translation.Z -= halfSideZ / cubeSizeScaler;
this.volumeCube = new ScreenSpaceLines3D();
this.volumeCube.Points = new Point3DCollection();
// Front face
// TL front - TR front
this.volumeCube.Points.Add(new Point3D(-halfSideX + translation.X, halfSideY + translation.Y, -halfSideZ + translation.Z));
this.volumeCube.Points.Add(new Point3D(halfSideX + translation.X, halfSideY + translation.Y, -halfSideZ + translation.Z));
// TR front - BR front
this.volumeCube.Points.Add(new Point3D(halfSideX + translation.X, halfSideY + translation.Y, -halfSideZ + translation.Z));
this.volumeCube.Points.Add(new Point3D(halfSideX + translation.X, -halfSideY + translation.Y, -halfSideZ + translation.Z));
// BR front - BL front
this.volumeCube.Points.Add(new Point3D(halfSideX + translation.X, -halfSideY + translation.Y, -halfSideZ + translation.Z));
this.volumeCube.Points.Add(new Point3D(-halfSideX + translation.X, -halfSideY + translation.Y, -halfSideZ + translation.Z));
// BL front - TL front
this.volumeCube.Points.Add(new Point3D(-halfSideX + translation.X, -halfSideY + translation.Y, -halfSideZ + translation.Z));
this.volumeCube.Points.Add(new Point3D(-halfSideX + translation.X, halfSideY + translation.Y, -halfSideZ + translation.Z));
// Rear face
// TL rear - TR rear
this.volumeCube.Points.Add(new Point3D(-halfSideX + translation.X, halfSideY + translation.Y, halfSideZ + translation.Z));
this.volumeCube.Points.Add(new Point3D(halfSideX + translation.X, halfSideY + translation.Y, halfSideZ + translation.Z));
// TR rear - BR rear
this.volumeCube.Points.Add(new Point3D(halfSideX + translation.X, halfSideY + translation.Y, halfSideZ + translation.Z));
this.volumeCube.Points.Add(new Point3D(halfSideX + translation.X, -halfSideY + translation.Y, halfSideZ + translation.Z));
// BR rear - BL rear
this.volumeCube.Points.Add(new Point3D(halfSideX + translation.X, -halfSideY + translation.Y, halfSideZ + translation.Z));
this.volumeCube.Points.Add(new Point3D(-halfSideX + translation.X, -halfSideY + translation.Y, halfSideZ + translation.Z));
// BL rear - TL rear
this.volumeCube.Points.Add(new Point3D(-halfSideX + translation.X, -halfSideY + translation.Y, halfSideZ + translation.Z));
this.volumeCube.Points.Add(new Point3D(-halfSideX + translation.X, halfSideY + translation.Y, halfSideZ + translation.Z));
// Connecting lines
// TL front - TL rear
this.volumeCube.Points.Add(new Point3D(-halfSideX + translation.X, halfSideY + translation.Y, -halfSideZ + translation.Z));
this.volumeCube.Points.Add(new Point3D(-halfSideX + translation.X, halfSideY + translation.Y, halfSideZ + translation.Z));
// TR front - TR rear
this.volumeCube.Points.Add(new Point3D(halfSideX + translation.X, halfSideY + translation.Y, -halfSideZ + translation.Z));
this.volumeCube.Points.Add(new Point3D(halfSideX + translation.X, halfSideY + translation.Y, halfSideZ + translation.Z));
// BR front - BR rear
this.volumeCube.Points.Add(new Point3D(halfSideX + translation.X, -halfSideY + translation.Y, -halfSideZ + translation.Z));
this.volumeCube.Points.Add(new Point3D(halfSideX + translation.X, -halfSideY + translation.Y, halfSideZ + translation.Z));
// BL front - BL rear
this.volumeCube.Points.Add(new Point3D(-halfSideX + translation.X, -halfSideY + translation.Y, -halfSideZ + translation.Z));
this.volumeCube.Points.Add(new Point3D(-halfSideX + translation.X, -halfSideY + translation.Y, halfSideZ + translation.Z));
this.volumeCube.Thickness = thickness;
this.volumeCube.Color = color;
this.volumeCubeAxisX = new ScreenSpaceLines3D();
this.volumeCubeAxisX.Points = new Point3DCollection();
this.volumeCubeAxisX.Points.Add(new Point3D(-halfSideX + translation.X, halfSideY + translation.Y, halfSideZ + translation.Z));
this.volumeCubeAxisX.Points.Add(
new Point3D(-halfSideX + 0.1f + translation.X, halfSideY + translation.Y, halfSideZ + translation.Z));
this.volumeCubeAxisX.Thickness = thickness + 2;
this.volumeCubeAxisX.Color = System.Windows.Media.Color.FromArgb(200, 255, 0, 0); // Red (X)
this.volumeCubeAxisY = new ScreenSpaceLines3D();
this.volumeCubeAxisY.Points = new Point3DCollection();
this.volumeCubeAxisY.Points.Add(new Point3D(-halfSideX + translation.X, halfSideY + translation.Y, halfSideZ + translation.Z));
this.volumeCubeAxisY.Points.Add(
new Point3D(-halfSideX + translation.X, halfSideY - 0.1f + translation.Y, halfSideZ + translation.Z));
this.volumeCubeAxisY.Thickness = thickness + 2;
this.volumeCubeAxisY.Color = System.Windows.Media.Color.FromArgb(200, 0, 255, 0); // Green (Y)
this.volumeCubeAxisZ = new ScreenSpaceLines3D();
this.volumeCubeAxisZ.Points = new Point3DCollection();
this.volumeCubeAxisZ.Points.Add(new Point3D(-halfSideX + translation.X, halfSideY + translation.Y, halfSideZ + translation.Z));
this.volumeCubeAxisZ.Points.Add(
new Point3D(-halfSideX + translation.X, halfSideY + translation.Y, halfSideZ - 0.1f + translation.Z));
this.volumeCubeAxisZ.Thickness = thickness + 2;
this.volumeCubeAxisZ.Color = System.Windows.Media.Color.FromArgb(200, 0, 0, 255); // Blue (Z)
}
/// <summary>
/// Dispose the volume cube and axes
/// </summary>
private void DisposeVolumeCube3DGraphics()
{
if (this.haveAddedVolumeCube)
{
this.RemoveVolumeCube3DGraphics();
}
if (null != this.volumeCube)
{
this.volumeCube.Dispose();
this.volumeCube = null;
}
if (null != this.volumeCubeAxisX)
{
this.volumeCubeAxisX.Dispose();
this.volumeCubeAxisX = null;
}
if (null != this.volumeCubeAxisY)
{
this.volumeCubeAxisY.Dispose();
this.volumeCubeAxisY = null;
}
if (null != this.volumeCubeAxisZ)
{
this.volumeCubeAxisZ.Dispose();
this.volumeCubeAxisZ = null;
}
}
